Skip to content

Commit cf921fd

Browse files
rondlhrondlh
committed
Solve some warnings, some formatting fixes
Solve some warnings, some formatting fixes
1 parent 39b6915 commit cf921fd

8 files changed

Lines changed: 298 additions & 319 deletions

File tree

‎src/main/java/com/neuronrobotics/sdk/addons/kinematics/AbstractKinematicsNR.java‎

Lines changed: 45 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -117,15 +117,15 @@ public abstract class AbstractKinematicsNR extends NonBowlerDevice implements IP
117117
private Runnable renderWrangler=null;
118118

119119
public int getLinkIndex(AbstractLink l) {
120-
for (int i=0;i<getNumberOfLinks();i++) {
121-
if(getAbstractLink(i)==l)
120+
for (int i = 0; i < getNumberOfLinks(); i++) {
121+
if (getAbstractLink(i) == l)
122122
return i;
123123
}
124124
return -1;
125125
}
126126
public int getLinkIndex(LinkConfiguration l) {
127-
for (int i=0;i<getNumberOfLinks();i++) {
128-
if(getAbstractLink(i).getLinkConfiguration()==l)
127+
for (int i = 0; i < getNumberOfLinks(); i++) {
128+
if (getAbstractLink(i).getLinkConfiguration() == l)
129129
return i;
130130
}
131131
return -1;
@@ -223,8 +223,9 @@ public AbstractKinematicsNR() {
223223
*/
224224
public AbstractKinematicsNR(InputStream configFile, LinkFactory f) {
225225
this();
226-
if(configFile==null||f==null)
226+
if ((configFile == null) || (f == null))
227227
return;
228+
228229
Document doc = XmlFactory.getAllNodesDocument(configFile);
229230
NodeList nodListofLinks = doc.getElementsByTagName("appendage");
230231
for (int i = 0; i < 1; i++) {
@@ -546,9 +547,9 @@ public double[] getCurrentJointSpaceVector() {
546547

547548
public double[] getCurrentJointSpaceTarget() {
548549

549-
double[] currentJointSpaceTarget=new double[getNumberOfLinks()];
550-
for(int i=0;i<currentJointSpaceTarget.length;i++) {
551-
currentJointSpaceTarget[i]=readLinkTarget(i);
550+
double[] currentJointSpaceTarget = new double[getNumberOfLinks()];
551+
for (int i = 0; i < currentJointSpaceTarget.length; i++) {
552+
currentJointSpaceTarget[i] = readLinkTarget(i);
552553
}
553554
return currentJointSpaceTarget;
554555
}
@@ -574,15 +575,15 @@ public double[] setDesiredTaskSpaceTransform(TransformNR taskSpaceTransform, dou
574575
TickToc.tic("inverseOffset");
575576
double[] jointSpaceVect = inverseKinematics(inverseOffset);
576577
TickToc.tic("inverseKinematics");
577-
if(checkVector(this,jointSpaceVect,seconds)) {
578+
if (checkVector(this, jointSpaceVect, seconds)) {
578579
TickToc.tic("checkVector success");
579580
if (jointSpaceVect == null)
580581
throw new RuntimeException("The kinematics model must return an array, not null");
581582

582-
_setDesiredJointSpaceVector(jointSpaceVect, seconds,false);
583+
_setDesiredJointSpaceVector(jointSpaceVect, seconds, false);
583584
TickToc.tic("_setDesiredJointSpaceVector complete");
584585
return jointSpaceVect;
585-
}else
586+
} else
586587
TickToc.tic("checkVector fail");
587588

588589
double[] currentJointSpaceTarget2 = getCurrentJointSpaceTarget();
@@ -620,40 +621,41 @@ private static boolean checkVector(AbstractKinematicsNR dev, double[] jointSpace
620621
for (int i = 0; i < jointSpaceVect.length; i++) {
621622
AbstractLink link = dev.factory.getLink(dev.getLinkConfiguration(i));
622623
double val = jointSpaceVect[i];
623-
Double double1 = new Double(val);
624-
if(double1.isNaN() ||double1.isInfinite() ) {
625-
Log.error(dev.getScriptingName()+" Link "+i+" Invalid unput "+double1);
624+
625+
if (Double.isNaN(val) || Double.isInfinite(val)) {
626+
Log.error(dev.getScriptingName() + " Link " + i + " Invalid input " + val);
626627
return false;
627628
}
628629
if (val > link.getMaxEngineeringUnits()) {
629-
Log.error(dev.getScriptingName()+" Link "+i+" can not reach "+val+" limited to "+link.getMaxEngineeringUnits());
630+
Log.error(dev.getScriptingName() + " Link " + i + " can not reach " + val + " limited to " + link.getMaxEngineeringUnits());
630631
return false;
631632
}
632633
if (val < link.getMinEngineeringUnits()) {
633-
Log.error(dev.getScriptingName()+" Link "+i+" can not reach "+val+" limited to "+link.getMinEngineeringUnits());
634+
Log.error(dev.getScriptingName() + " Link " + i + " can not reach " + val + " limited to " + link.getMinEngineeringUnits());
634635
return false;
635636
}
636-
if(seconds>0) {
637+
if (seconds > 0) {
637638
double maxVel = Math.abs(link.getMaxVelocityEngineeringUnits());
638639
double deltaPosition = Math.abs(current[i] - jointSpaceVect[i]);
639-
double computedVelocity = deltaPosition/seconds;
640-
if((computedVelocity-maxVel)>0.0001) {
641-
Log.error("Link "+i+" can not move at rate of "+computedVelocity+" capped at "+maxVel+" requested position of "+jointSpaceVect[i]+" from current position of "+current[i]+" in "+seconds+" seconds");
640+
double computedVelocity = deltaPosition / seconds;
641+
if ((computedVelocity-maxVel)>0.0001) {
642+
Log.error("Link " + i + " can not move at rate of " + computedVelocity + " capped at " + maxVel + " requested position of " + jointSpaceVect[i] + " from current position of " + current[i] + " in " + seconds + " seconds");
642643
return false;
643644
}
644645
}
645646
}
646647
return true;
647648
}
648649

650+
649651
/**
650652
* Checks the desired pose for ability for the IK to calculate a valid pose.
651653
*
652654
* @param taskSpaceTransform the task space transform
653655
* @return True if pose is reachable, false if it is not
654656
*/
655657
public boolean checkTaskSpaceTransform(TransformNR taskSpaceTransform, double seconds) {
656-
return AbstractKinematicsNR.checkTaskSpaceTransform(this, taskSpaceTransform,seconds);
658+
return AbstractKinematicsNR.checkTaskSpaceTransform(this, taskSpaceTransform, seconds);
657659
}
658660
/**
659661
* Checks the desired pose for ability for the IK to calculate a valid pose.
@@ -662,7 +664,7 @@ public boolean checkTaskSpaceTransform(TransformNR taskSpaceTransform, double se
662664
* @return True if pose is reachable, false if it is not
663665
*/
664666
public boolean checkTaskSpaceTransform(TransformNR taskSpaceTransform) {
665-
return checkTaskSpaceTransform(this, taskSpaceTransform,0);
667+
return checkTaskSpaceTransform(this, taskSpaceTransform, 0);
666668
}
667669

668670
/**
@@ -689,7 +691,7 @@ public double getBestTime(TransformNR currentTaskSpaceTransform) {
689691
* @return the time of translation at best possible speed based on checking each link
690692
*/
691693
public double getBestTime(double[] jointSpaceVect) {
692-
double best=0;
694+
double best = 0;
693695
double[] current = getCurrentJointSpaceTarget();
694696
for (int i = 0; i < current.length; i++) {
695697
AbstractLink link = getAbstractLink(i);
@@ -730,8 +732,8 @@ private double[] _setDesiredJointSpaceVector(double[] jointSpaceVect, double se
730732
+ " links, actual number of links = " + jointSpaceVect.length);
731733
}
732734
double best = getBestTime(jointSpaceVect);
733-
if(seconds<best)
734-
seconds=best;
735+
if (seconds < best)
736+
seconds = best;
735737
//synchronized(AbstractKinematicsNR.class) {
736738
int except = 0;
737739
Exception e = null;
@@ -762,7 +764,7 @@ private double[] _setDesiredJointSpaceVector(double[] jointSpaceVect, double se
762764
TickToc.tic("FK from vector");
763765
fireTargetJointsUpdate(getCurrentJointSpaceTarget(), fwd);
764766
TickToc.tic("Joint space updates");
765-
if(fireTaskUpdate) {
767+
if (fireTaskUpdate) {
766768
setCurrentPoseTarget(forwardOffset(fwd));
767769
TickToc.tic("task space updates");
768770
}
@@ -952,7 +954,7 @@ public void setGlobalToFiducialTransform(TransformNR frameToBase, boolean fireUp
952954
return;
953955
}
954956
this.fiducial2RAS = frameToBase;
955-
if(!fireUpdate)
957+
if (!fireUpdate)
956958
return;
957959
for (int i = 0; i < regListeners.size(); i++) {
958960
IRegistrationListenerNR r = regListeners.get(i);
@@ -1009,8 +1011,9 @@ public TransformNR forwardOffset(TransformNR t) {
10091011
* @param l the l
10101012
*/
10111013
public void addJointSpaceListener(IJointSpaceUpdateListenerNR l) {
1012-
if (jointSpaceUpdateListeners.contains(l) || l == null)
1014+
if (jointSpaceUpdateListeners.contains(l) || (l == null))
10131015
return;
1016+
10141017
jointSpaceUpdateListeners.add(l);
10151018
}
10161019

@@ -1030,8 +1033,9 @@ public void removeJointSpaceUpdateListener(IJointSpaceUpdateListenerNR l) {
10301033
* @param l the l
10311034
*/
10321035
public void addRegistrationListener(IRegistrationListenerNR l) {
1033-
if (regListeners.contains(l) || l == null)
1036+
if (regListeners.contains(l) || (l == null))
10341037
return;
1038+
10351039
regListeners.add(l);
10361040
l.onBaseToFiducialUpdate(this, getRobotToFiducialTransform());
10371041
}
@@ -1052,7 +1056,7 @@ public void removeRegestrationUpdateListener(IRegistrationListenerNR l) {
10521056
* @param l the l
10531057
*/
10541058
public void addPoseUpdateListener(ITaskSpaceUpdateListenerNR l) {
1055-
if (taskSpaceUpdateListeners.contains(l) || l == null) {
1059+
if (taskSpaceUpdateListeners.contains(l) || (l == null)) {
10561060
return;
10571061
}
10581062
// new RuntimeException("adding "+l.getClass().getName()).printStackTrace();
@@ -1083,16 +1087,16 @@ public void onLinkPositionUpdate(AbstractLink source, double engineeringUnitsVal
10831087
for (LinkConfiguration c : getLinkConfigurations()) {
10841088
AbstractLink tmp = getFactory().getLink(c);
10851089
if (tmp == source) {// Check to see if this lines up with a known link
1086-
// // Log.info("Got PID event "+source+" value="+engineeringUnitsValue);
1087-
// if(new Double(engineeringUnitsValue).isNaN()) {
1088-
// new RuntimeException("Link values can not ne NaN").printStackTrace();
1089-
// engineeringUnitsValue=0;
1090+
// // Log.info("Got PID event " + source + " value=" + engineeringUnitsValue);
1091+
// if (new Double(engineeringUnitsValue).isNaN()) {
1092+
// new RuntimeException("Link values can not be NaN").printStackTrace();
1093+
// engineeringUnitsValue = 0;
10901094
// }
10911095
// ArrayList<LinkConfiguration> linkConfigurations = getLinkConfigurations();
1092-
// if(linkConfigurations!=null) {
1096+
// if (linkConfigurations!=null) {
10931097
// int indexOf = linkConfigurations.indexOf(c);
1094-
// if(currentJointSpacePositions!=null)
1095-
// if(indexOf>=0 && indexOf<currentJointSpacePositions.length)
1098+
// if (currentJointSpacePositions != null)
1099+
// if ((indexOf >= 0) && (indexOf < currentJointSpacePositions.length))
10961100
// currentJointSpacePositions[indexOf] = engineeringUnitsValue;
10971101
// }
10981102
firePoseUpdate();
@@ -1200,7 +1204,7 @@ public void onPIDEvent(PIDEvent e) {
12001204
* @param link the link
12011205
*/
12021206
public void homeLink(int link) {
1203-
if (link < 0 || link >= getNumberOfLinks()) {
1207+
if ((link < 0) || (link >= getNumberOfLinks())) {
12041208
throw new IndexOutOfBoundsException(
12051209
"There are only " + getNumberOfLinks() + " known links, requested:" + link);
12061210
}
@@ -1637,7 +1641,7 @@ public void clearChangeListener(int linkIndex) {
16371641

16381642
public void runRenderWrangler() {
16391643
firePoseUpdate();
1640-
if(renderWrangler!=null)
1644+
if (renderWrangler != null)
16411645
try {
16421646
renderWrangler.run();
16431647
}catch(Throwable t) {
@@ -1719,7 +1723,7 @@ public InterpolationMoveState blockingInterpolatedMove(TransformNR target, doubl
17191723
}
17201724
wait((int) msPerStep);
17211725
}
1722-
}else {
1726+
} else {
17231727
return InterpolationMoveState.FAULT;
17241728
}
17251729
return InterpolationMoveState.READY;
@@ -1728,7 +1732,7 @@ public InterpolationMoveState blockingInterpolatedMove(TransformNR target, doubl
17281732
public void setTimeProvider(ITimeProvider t) {
17291733
super.setTimeProvider(t);
17301734
imu.setTimeProvider(getTimeProvider());
1731-
for(int i=0;i<getNumberOfLinks();i++) {
1735+
for (int i = 0; i < getNumberOfLinks(); i++) {
17321736
AbstractLink l = getAbstractLink(i);
17331737
l.setTimeProvider(getTimeProvider());
17341738
}

‎src/main/java/com/neuronrobotics/sdk/addons/kinematics/WalkingDriveEngine.java‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -35,10 +35,10 @@ public void DriveArc(MobileBase source, TransformNR newPose, double seconds) {
3535
global.translateX(newPose.getX());
3636
global.translateY(newPose.getY());
3737
global.translateZ(newPose.getZ());
38-
double rotz = newPose.getRotation().getRotationAzimuth() +global.getRotation().getRotationAzimuth() ;
39-
double roty = newPose.getRotation().getRotationElevation() ;
40-
double rotx = newPose.getRotation().getRotationTilt() ;
41-
global.setRotation(new RotationNR( rotx,roty, rotz) );
38+
double rotz = newPose.getRotation().getRotationAzimuthRadians() + global.getRotation().getRotationAzimuthRadians();
39+
double roty = newPose.getRotation().getRotationElevationRadians();
40+
double rotx = newPose.getRotation().getRotationTiltRadians();
41+
global.setRotation(new RotationNR(rotx, roty, rotz));
4242
// New target calculated appliaed to global offset
4343
source.setGlobalToFiducialTransform(global);
4444
for(int i=0;i<numlegs;i++){

‎src/main/java/com/neuronrobotics/sdk/addons/kinematics/ik/DeltaIKModel.java‎

Lines changed: 29 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -87,11 +87,11 @@ public double[] inverseKinematics6dof(TransformNR target, double[] jointSpaceVec
8787
-Math.pow(wristVect,2)
8888
)
8989
/
90-
(2*elbowLink2CompositeLength*elbowLink1CompositeLength)
90+
(2 * elbowLink2CompositeLength * elbowLink1CompositeLength)
9191
)
9292
));
93-
if(debug)com.neuronrobotics.sdk.common.Log.error( "Elbow angle "+elbowTiltAngle);
94-
jointSpaceVector[2]=elbowTiltAngle - Math.toDegrees(links.get(2).getTheta());
93+
if(debug)com.neuronrobotics.sdk.common.Log.error( "Elbow angle " + elbowTiltAngle);
94+
jointSpaceVector[2] = elbowTiltAngle - Math.toDegrees(links.get(2).getTheta());
9595

9696
TransformNR local = new TransformNR(0,0,0,new RotationNR(0, -baseVectorAngle, 0));
9797
TransformNR tipOnXVect = local.times(newCenter);
@@ -141,10 +141,10 @@ public double[] inverseKinematics6dof(TransformNR target, double[] jointSpaceVec
141141
TransformNR sphericalElbowTartget = reorient.times(newCenter);
142142
//com.neuronrobotics.sdk.common.Log.error( newCenter
143143
//com.neuronrobotics.sdk.common.Log.error( sphericalElbowTartget
144-
sphericalElbowTartget = new TransformNR(0.0,-sphericalElbowTartget.getY(),0.0, new RotationNR()).times(sphericalElbowTartget);
144+
sphericalElbowTartget = new TransformNR(0.0,-sphericalElbowTartget.getY(), 0.0, new RotationNR()).times(sphericalElbowTartget);
145145
//com.neuronrobotics.sdk.common.Log.error( sphericalElbowTartget
146146
double theta3 = Math.atan2(sphericalElbowTartget.getZ(), sphericalElbowTartget.getX());
147-
jointSpaceVector[1]=-Math.toDegrees(theta3) ;
147+
jointSpaceVector[1] = -Math.toDegrees(theta3) ;
148148

149149
//return jointSpaceVector
150150

@@ -153,34 +153,34 @@ public double[] inverseKinematics6dof(TransformNR target, double[] jointSpaceVec
153153
*
154154
*/
155155
double[] wristLinks=new double[jointSpaceVector.length];
156-
for(int i=0;i<3;i++) {
156+
for (int i = 0;i < 3; i++)
157157
wristLinks[i]=jointSpaceVector[i];
158-
}
159-
for(int i=3;i<jointSpaceVector.length;i++) {
160-
wristLinks[i]=0;
161-
}
158+
159+
for (int i = 3; i < jointSpaceVector.length; i++)
160+
wristLinks[i] = 0;
161+
162162
ArrayList<TransformNR> chainToLoad =new ArrayList<>();
163163
chain.forwardKinematicsMatrix(wristLinks,chainToLoad);
164164
TransformNR startOfWristSet=chain.kin.inverseOffset(chainToLoad.get(2));
165-
TransformNR virtualcenter = newCenter.times(new TransformNR(0,0,10,
166-
new RotationNR(Math.toDegrees(links.get(5).getAlpha()),0,0)));
165+
TransformNR virtualcenter = newCenter.times(new TransformNR(0, 0, 10,
166+
new RotationNR(Math.toDegrees(links.get(5).getAlpha()),0 ,0)));
167167
TransformNR wristMOvedToCenter0 =startOfWristSet
168168
.inverse()// move back from base ot wrist to world home
169169
.times(virtualcenter);// move forward to target, leaving the angle between the tip and the start of the rotation
170170
//if(debug)com.neuronrobotics.sdk.common.Log.error( wristMOvedToCenter0
171171
RotationNR qWrist=wristMOvedToCenter0.getRotation();
172-
if(wristMOvedToCenter0.getX()==0&&wristMOvedToCenter0.getY()==0) {
172+
if ((wristMOvedToCenter0.getX() == 0) && (wristMOvedToCenter0.getY() == 0)) {
173173
com.neuronrobotics.sdk.common.Log.error( "Singularity! try something else");
174174
return inverseKinematics6dof(target.copy().translateX(0.01),jointSpaceVector,chain);
175175
}
176-
double closest= (Math.toDegrees(Math.atan2(wristMOvedToCenter0.getY(), wristMOvedToCenter0.getX()))-Math.toDegrees(links.get(3).getTheta()));
176+
double closest = (Math.toDegrees(Math.atan2(wristMOvedToCenter0.getY(), wristMOvedToCenter0.getX())) - Math.toDegrees(links.get(3).getTheta()));
177177

178-
jointSpaceVector[3]=closest;
179-
wristLinks[3]=jointSpaceVector[3];
180-
if(jointSpaceVector.length==4)
178+
jointSpaceVector[3] = closest;
179+
wristLinks[3] = jointSpaceVector[3];
180+
if(jointSpaceVector.length == 4)
181181
return jointSpaceVector;
182182

183-
chainToLoad =new ArrayList<>();
183+
chainToLoad = new ArrayList<>();
184184
/**
185185
// Calculte the second angle
186186
*
@@ -193,33 +193,33 @@ public double[] inverseKinematics6dof(TransformNR target, double[] jointSpaceVec
193193
.inverse()// move back from base ot wrist to world home
194194
.times(virtualcenter);// move forward to target, leaving the angle between the tip and the start of the rotation
195195
//if(debug)com.neuronrobotics.sdk.common.Log.error( " Middle link =" +wristMOvedToCenter1
196-
RotationNR qWrist2=wristMOvedToCenter1.getRotation();
197-
if(wristMOvedToCenter1.getX()==0&&wristMOvedToCenter1.getY()==0) {
196+
RotationNR qWrist2 = wristMOvedToCenter1.getRotation();
197+
if (wristMOvedToCenter1.getX()==0&&wristMOvedToCenter1.getY() == 0) {
198198
com.neuronrobotics.sdk.common.Log.error( "Singularity! try something else");
199199
return inverseKinematics6dof(target.copy().translateX(0.01),jointSpaceVector,chain);
200200
}
201-
jointSpaceVector[4]=(Math.toDegrees(Math.atan2(wristMOvedToCenter1.getY(), wristMOvedToCenter1.getX()))-
202-
Math.toDegrees(links.get(4).getTheta())-
203-
90);
204-
wristLinks[4]=jointSpaceVector[4];
205-
if(jointSpaceVector.length==5)
201+
jointSpaceVector[4] = (Math.toDegrees(Math.atan2(wristMOvedToCenter1.getY(), wristMOvedToCenter1.getX())) -
202+
Math.toDegrees(links.get(4).getTheta()) - 90);
203+
wristLinks[4] = jointSpaceVector[4];
204+
if (jointSpaceVector.length == 5)
206205
return jointSpaceVector;
207-
chainToLoad =new ArrayList<>();
206+
207+
chainToLoad = new ArrayList<>();
208208
/**
209209
// Calculte the last angle
210210
*
211211
*/
212212
chain.forwardKinematicsMatrix(wristLinks,chainToLoad);
213213
TransformNR startOfWristSet3=chain.kin.inverseOffset(chainToLoad.get(4));
214214
TransformNR tool = new TransformNR();
215-
if(linkNum==7)
215+
if (linkNum == 7)
216216
tool=linkOffset(links.get(6));
217217
TransformNR wristMOvedToCenter2 =startOfWristSet3
218218
.inverse()// move back from base ot wrist to world home
219219
.times(target.times(tool.inverse()));// move forward to target, leaving the angle between the tip and the start of the rotation
220220
//if(debug)com.neuronrobotics.sdk.common.Log.error( "\n\nLastLink " +wristMOvedToCenter2
221-
RotationNR qWrist3=wristMOvedToCenter2.getRotation();
222-
jointSpaceVector[5]=(Math.toDegrees(qWrist3.getRotationAzimuth())-Math.toDegrees(links.get(5).getTheta()));
221+
RotationNR qWrist3 = wristMOvedToCenter2.getRotation();
222+
jointSpaceVector[5] = (Math.toDegrees(qWrist3.getRotationAzimuthRadians()) - Math.toDegrees(links.get(5).getTheta()));
223223

224224
return jointSpaceVector;
225225
}

0 commit comments

Comments
 (0)