Skip to content

Commit 39b6915

Browse files
rondlhrondlh
committed
Fix orent --> orient typo
Fix orent --> orient typo
1 parent 722decb commit 39b6915

7 files changed

Lines changed: 93 additions & 93 deletions

File tree

‎src/main/java/com/neuronrobotics/sdk/addons/irobot/CreateArm.java‎

Lines changed: 30 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -61,8 +61,8 @@ public class CreateArm {
6161
/** The xy thresh hold. */
6262
private double xyThreshHold = .1;
6363

64-
/** The orent thresh hold. */
65-
private double orentThreshHold = 1;
64+
/** The orient thresh hold. */
65+
private double orientThreshHold = 1;
6666

6767
/**
6868
* Instantiates a new creates the arm.
@@ -263,14 +263,14 @@ public void setAngles(double shoulder, double elbow, double wrist,float time) {
263263
/**
264264
* Gets the cartesian pose.
265265
*
266-
* @return pose vector, X,Y,Orentation
266+
* @return pose vector, X,Y,Orientation
267267
*/
268268
public double [] getCartesianPose(){
269269
double [] angles =getAngles();
270-
pose[2] = GetOrentation();
270+
pose[2] = GetOrientation();
271271

272-
pose[0]=(l1* cos(ToRadians(angles[0]))+l2* cos(ToRadians(angles[0])+ToRadians(angles[1]))+(l3* cos(ToRadians(GetOrentation()))));
273-
pose[1]=(l1* sin(ToRadians(angles[0]))+l2* sin(ToRadians(angles[0])+ToRadians(angles[1]))+(l3* sin(ToRadians(GetOrentation()))));
272+
pose[0]=(l1* cos(ToRadians(angles[0]))+l2* cos(ToRadians(angles[0])+ToRadians(angles[1]))+(l3* cos(ToRadians(GetOrientation()))));
273+
pose[1]=(l1* sin(ToRadians(angles[0]))+l2* sin(ToRadians(angles[0])+ToRadians(angles[1]))+(l3* sin(ToRadians(GetOrientation()))));
274274
double [] p = new double [3];
275275
for ( int i = 0; i<3; i++){
276276
p[i]=pose[i];
@@ -318,37 +318,37 @@ public void setCartesianPose(double [] p, float time){
318318
*
319319
* @param x the x
320320
* @param y the y
321-
* @param orentation the orentation
321+
* @param orientation the orientation
322322
*/
323-
public void setCartesianPose(double x, double y, double orentation){
324-
setCartesianPose(x,y, orentation,(float).2);
323+
public void setCartesianPose(double x, double y, double orientation){
324+
setCartesianPose(x,y, orientation,(float).2);
325325
}
326326

327327
/**
328328
* Sets the cartesian pose.
329329
*
330330
* @param x the x
331331
* @param y the y
332-
* @param orentation the orentation
332+
* @param orientation the orientation
333333
* @param time the time
334334
*/
335-
public void setCartesianPose(double x, double y, double orentation, float time){
336-
if(orentation<-35)
337-
orentation=-35;
338-
if(orentation>35)
339-
orentation=35;
340-
if (!updateCartesian(x,y,orentation)){
335+
public void setCartesianPose(double x, double y, double orientation, float time){
336+
if(orientation<-35)
337+
orientation=-35;
338+
if(orientation>35)
339+
orientation=35;
340+
if (!updateCartesian(x,y,orientation)){
341341
return;
342342
}
343343

344344
pose[0]=x;
345345
pose[1]=y;
346-
pose[2]=orentation;
346+
pose[2]=orientation;
347347

348-
Log.info("Setting Pose X: "+x+" Y: "+y+" Orentation: "+orentation );
348+
Log.info("Setting Pose X: "+x+" Y: "+y+" Orientation: "+orientation );
349349

350-
x -= (l3*cos(orentation*M_PI/180));
351-
y -= (l3*sin(orentation*M_PI/180));
350+
x -= (l3*cos(orientation*M_PI/180));
351+
y -= (l3*sin(orientation*M_PI/180));
352352
if (sqrt(x*x+y*y) > l1+l2) {
353353
com.neuronrobotics.sdk.common.Log.error("Hypotenus too long"+x+" "+y+"\r\n");
354354
return;
@@ -361,7 +361,7 @@ public void setCartesianPose(double x, double y, double orentation, float time){
361361
shoulder =(atan2(y,x)+acos((x*x+y*y+l1*l1-l2*l2)/(2*l1*sqrt(x*x+y*y))));
362362
shoulder *=(180.0/M_PI);
363363

364-
double wrist = orentation-elbow-shoulder;
364+
double wrist = orientation-elbow-shoulder;
365365
setAngles(shoulder,elbow,wrist,time);
366366

367367

@@ -372,10 +372,10 @@ public void setCartesianPose(double x, double y, double orentation, float time){
372372
*
373373
* @param x the x
374374
* @param y the y
375-
* @param orentation the orentation
375+
* @param orientation the orientation
376376
* @return true, if successful
377377
*/
378-
private boolean updateCartesian(double x, double y, double orentation) {
378+
private boolean updateCartesian(double x, double y, double orientation) {
379379
if(((x>(pose[0]+xyThreshHold))) || (x<(pose[0]-xyThreshHold))){
380380
Log.info("X changed");
381381
return true;
@@ -388,8 +388,8 @@ private boolean updateCartesian(double x, double y, double orentation) {
388388
}else{
389389
Log.info("Y set: "+y+" was: "+pose[1]);
390390
}
391-
if((orentation>pose[2]+orentThreshHold) || (orentation<pose[2]-orentThreshHold)){
392-
Log.info("Orentation changed");
391+
if((orientation>pose[2]+orientThreshHold) || (orientation<pose[2]-orientThreshHold)){
392+
Log.info("Orientation changed");
393393
return true;
394394
}
395395
Log.info("No signifigant change");
@@ -415,11 +415,11 @@ public void setCartesianY(double y){
415415
}
416416

417417
/**
418-
* Sets the cartesian orentation.
418+
* Sets the cartesian orientation.
419419
*
420-
* @param o the new cartesian orentation
420+
* @param o the new cartesian orientation
421421
*/
422-
public void setCartesianOrentation(double o){
422+
public void setCartesianOrientation(double o){
423423
setCartesianPose(pose[0], pose[1], o);
424424
}
425425

@@ -488,11 +488,11 @@ private double ToRadians(double degrees){
488488
}
489489

490490
/**
491-
* Gets the orentation.
491+
* Gets the orientation.
492492
*
493493
* @return the current approach orientation of the wrist
494494
*/
495-
public double GetOrentation() {
495+
public double GetOrientation() {
496496
double [] angles =getAngles();
497497
return angles[0]+angles[1]+angles[2];
498498
}

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

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -359,12 +359,12 @@ public double[] inverseKinematics(TransformNR target,
359359
double lengthRectangleAdjustedXY = lengthXYPlaneVect* Math.cos(angleRectangleAdjustedXY)-r;
360360

361361

362-
double orentation = angleXYPlaneVect-angleRectangleAdjustedXY;
363-
if(Math.abs(Math.toDegrees(orentation))<0.01){
364-
orentation=0;
362+
double orientation = angleXYPlaneVect-angleRectangleAdjustedXY;
363+
if(Math.abs(Math.toDegrees(orientation))<0.01){
364+
orientation=0;
365365
}
366-
double ySet = lengthRectangleAdjustedXY*Math.sin(orentation);
367-
double xSet = lengthRectangleAdjustedXY*Math.cos(orentation);
366+
double ySet = lengthRectangleAdjustedXY*Math.sin(orientation);
367+
double xSet = lengthRectangleAdjustedXY*Math.cos(orientation);
368368

369369

370370
double zSet = target.getZ() - links.get(0).getD();
@@ -396,7 +396,7 @@ public double[] inverseKinematics(TransformNR target,
396396
println( "x Correction: "+xSet);
397397
println( "y Correction: "+ySet);
398398
399-
println( "Orentation: "+Math.toDegrees(orentation));
399+
println( "Orientation: "+Math.toDegrees(orientation));
400400
println( "z: "+zSet);
401401
*/
402402

@@ -420,7 +420,7 @@ public double[] inverseKinematics(TransformNR target,
420420
println( "l1 from x/y plane: "+Math.toDegrees(A+elevation));
421421
println( "l2 from l1: "+Math.toDegrees(C));
422422
*/
423-
inv[0] = Math.toDegrees(orentation);
423+
inv[0] = Math.toDegrees(orientation);
424424
inv[1] = -Math.toDegrees((A+elevation+links.get(1).getTheta()));
425425
if((int)links.get(1).getAlpha() ==180){
426426
inv[2] = (Math.toDegrees(C))-180-//interior angle of the triangle, map to external angle
@@ -433,7 +433,7 @@ public double[] inverseKinematics(TransformNR target,
433433
inv[3] =(inv[1] -inv[2]);// keep it parallell
434434
// We know the wrist twist will always be 0 for this model
435435
if(links.size()>4)
436-
inv[4] = inv[0];//keep the tool orentation paralell from the base
436+
inv[4] = inv[0];//keep the tool orientation paralell from the base
437437

438438
for(int i=0;i<inv.length;i++){
439439
if(Math.abs(inv[i]) < 0.01){

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

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -45,12 +45,12 @@ public double[] inverseKinematics(TransformNR target,double[] jointSpaceVector,
4545
double posOffset = 10;
4646
int iter=0;
4747
double vect=0;
48-
double orent = 0;
48+
double orient = 0;
4949
boolean stopped;
5050
boolean notArrived = false;
5151
boolean [] stop = new boolean [increments.length];
5252
double previousV =dhChain.forwardKinematics(jointSpaceVector).getOffsetVectorMagnitude(target);
53-
double previousO =dhChain.forwardKinematics(jointSpaceVector).getOffsetOrentationMagnitude(target);
53+
double previousO =dhChain.forwardKinematics(jointSpaceVector).getOffsetOrientationMagnitude(target);
5454
do{
5555
stopped = true;
5656
for(int i=increments.length-1;i>=0;i--){
@@ -60,13 +60,13 @@ public double[] inverseKinematics(TransformNR target,double[] jointSpaceVector,
6060
}
6161
}
6262
vect = dhChain.forwardKinematics(jointSpaceVector).getOffsetVectorMagnitude(target);
63-
orent = dhChain.forwardKinematics(jointSpaceVector).getOffsetOrentationMagnitude(target);
64-
if(previousV>=vect && previousO>=orent){
63+
orient = dhChain.forwardKinematics(jointSpaceVector).getOffsetOrientationMagnitude(target);
64+
if(previousV>=vect && previousO>=orient){
6565
for(int i=0;i<inv.length;i++){
6666
inv[i]=jointSpaceVector[i];
6767
}
6868
previousV=vect;
69-
previousO=orent;
69+
previousO=orient;
7070
}
7171

7272
notArrived = (previousV > posOffset|| previousO > .001);
@@ -83,7 +83,7 @@ public double[] inverseKinematics(TransformNR target,double[] jointSpaceVector,
8383
}
8484
}while(++iter<200 && notArrived && stopped == false);//preincrement and check
8585
if(debug){
86-
com.neuronrobotics.sdk.common.Log.error("Numer of iterations #"+iter+" \n\tStalled = "+stopped+" \n\tArrived = "+!notArrived+" \n\tFinal offset= "+vect+" \n\tFinal orent= "+orent);
86+
com.neuronrobotics.sdk.common.Log.error("Numer of iterations #"+iter+" \n\tStalled = "+stopped+" \n\tArrived = "+!notArrived+" \n\tFinal offset= "+vect+" \n\tFinal orient= "+orient);
8787
}
8888
return inv;
8989
}

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

Lines changed: 30 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -36,8 +36,8 @@ public class GradiantDecentNode{
3636
/** The inc vect. */
3737
double incVect;
3838

39-
/** The inc orent. */
40-
double incOrent;
39+
/** The inc orient. */
40+
double incOrient;
4141

4242
/** The integral size. */
4343
//integral
@@ -46,20 +46,20 @@ public class GradiantDecentNode{
4646
/** The integral index vect. */
4747
int integralIndexVect = 0;
4848

49-
/** The integral index orent. */
50-
int integralIndexOrent = 0;
49+
/** The integral index orient. */
50+
int integralIndexOrient = 0;
5151

5252
/** The integral total vect. */
5353
double integralTotalVect = 0;
5454

55-
/** The integral total orent. */
56-
double integralTotalOrent = 0;
55+
/** The integral total orient. */
56+
double integralTotalOrient = 0;
5757

5858
/** The int vect. */
5959
double intVect[] = new double[integralSize];
6060

61-
/** The int orent. */
62-
double intOrent[] = new double[integralSize];
61+
/** The int orient. */
62+
double intOrient[] = new double[integralSize];
6363

6464
/** The Kp. */
6565
double Kp = 1;
@@ -88,62 +88,62 @@ public GradiantDecentNode(DHChain chain,int index,double[] jointSpaceVector,Tran
8888
lower = l;
8989
for(int i=0;i<integralSize;i++){
9090
intVect[i]=0;
91-
intOrent[i]=0;
91+
intOrient[i]=0;
9292
}
9393
}
9494

9595
/**
96-
* Step orent.
96+
* Step orient.
9797
*
9898
* @return true, if successful
9999
*/
100-
public boolean stepOrent(){
100+
public boolean stepOrient(){
101101
double none = myStart+offset;
102102
double start = offset;
103103
jointSpaceVector[getIndex()]= bound (none);
104104
TransformNR tmp =chain.forwardKinematics(jointSpaceVector);
105105
tmp =chain.forwardKinematics(jointSpaceVector);
106-
double noneOrent = tmp.getOffsetOrentationMagnitude(target);
106+
double noneOrient = tmp.getOffsetOrientationMagnitude(target);
107107

108-
double incOrentP = (noneOrent*10);//Multiply by magic number
108+
double incOrientP = (noneOrient*10);//Multiply by magic number
109109
//Remove old values off rolling buffer
110-
integralTotalOrent-=intOrent[integralIndexOrent];
110+
integralTotalOrient-=intOrient[integralIndexOrient];
111111
//Store current values
112-
intOrent[integralIndexOrent] =incOrentP;
112+
intOrient[integralIndexOrient] =incOrientP;
113113
//Add current values to totals
114-
integralTotalOrent+=intOrent[integralIndexOrent];
114+
integralTotalOrient+=intOrient[integralIndexOrient];
115115
//Reset the index for next iteration
116-
integralIndexOrent++;
117-
if(integralIndexOrent==integralSize){
118-
integralIndexOrent=0;
116+
integralIndexOrient++;
117+
if(integralIndexOrient==integralSize){
118+
integralIndexOrient=0;
119119
}
120120

121121
//The 2 increment numbers
122-
incOrent = incOrentP*Kp + (integralTotalOrent/integralSize)*Ki;
122+
incOrient = incOrientP*Kp + (integralTotalOrient/integralSize)*Ki;
123123

124-
double upO = myStart+offset+incOrent;
125-
double downO =myStart+offset-incOrent;
124+
double upO = myStart+offset+incOrient;
125+
double downO =myStart+offset-incOrient;
126126

127127
jointSpaceVector[getIndex()]= bound (upO);
128128
tmp =chain.forwardKinematics(jointSpaceVector);
129-
double upOrent = tmp.getOffsetOrentationMagnitude(target);
129+
double upOrient = tmp.getOffsetOrientationMagnitude(target);
130130

131131
jointSpaceVector[getIndex()]= bound (downO);
132132
tmp =chain.forwardKinematics(jointSpaceVector);
133-
double downOrent = tmp.getOffsetOrentationMagnitude(target);
133+
double downOrient = tmp.getOffsetOrientationMagnitude(target);
134134

135135

136-
if( (upOrent>noneOrent && downOrent>noneOrent)){
136+
if( (upOrient>noneOrient && downOrient>noneOrient)){
137137
jointSpaceVector[getIndex()]=none;
138138
}
139139

140-
if(( noneOrent>upOrent && downOrent>upOrent)){
140+
if(( noneOrient>upOrient && downOrient>upOrient)){
141141
jointSpaceVector[getIndex()]=upO;
142-
offset+=incOrent;
142+
offset+=incOrient;
143143
}
144-
if((upOrent>downOrent && noneOrent>downOrent )){
144+
if((upOrient>downOrient && noneOrient>downOrient )){
145145
jointSpaceVector[getIndex()]=downO;
146-
offset-=incOrent;
146+
offset-=incOrient;
147147
}
148148

149149
jointSpaceVector[getIndex()] = myStart+offset;
@@ -218,7 +218,7 @@ public boolean stepLin(){
218218
* @return true, if successful
219219
*/
220220
public boolean step() {
221-
boolean back = stepOrent()||stepLin();
221+
boolean back = stepOrient()||stepLin();
222222
return back;
223223
}
224224

0 commit comments

Comments
 (0)