From 39b6915f8f1ddbd57b954af49018894951be1360 Mon Sep 17 00:00:00 2001 From: rondlh <77279634+rondlh@users.noreply.github.com> Date: Sun, 15 Mar 2026 17:56:12 +0800 Subject: [PATCH 1/3] Fix orent --> orient typo Fix orent --> orient typo --- .../sdk/addons/irobot/CreateArm.java | 60 +++++++++---------- .../sdk/addons/kinematics/DHChain.java | 16 ++--- .../sdk/addons/kinematics/GradiantDecent.java | 12 ++-- .../addons/kinematics/GradiantDecentNode.java | 60 +++++++++---------- .../addons/kinematics/SearchTreeSolver.java | 26 ++++---- .../addons/kinematics/ik/DeltaIKModel.java | 6 +- .../addons/kinematics/math/TransformNR.java | 6 +- 7 files changed, 93 insertions(+), 93 deletions(-) diff --git a/src/main/java/com/neuronrobotics/sdk/addons/irobot/CreateArm.java b/src/main/java/com/neuronrobotics/sdk/addons/irobot/CreateArm.java index 20fd2e95..a188d065 100644 --- a/src/main/java/com/neuronrobotics/sdk/addons/irobot/CreateArm.java +++ b/src/main/java/com/neuronrobotics/sdk/addons/irobot/CreateArm.java @@ -61,8 +61,8 @@ public class CreateArm { /** The xy thresh hold. */ private double xyThreshHold = .1; - /** The orent thresh hold. */ - private double orentThreshHold = 1; + /** The orient thresh hold. */ + private double orientThreshHold = 1; /** * Instantiates a new creates the arm. @@ -263,14 +263,14 @@ public void setAngles(double shoulder, double elbow, double wrist,float time) { /** * Gets the cartesian pose. * - * @return pose vector, X,Y,Orentation + * @return pose vector, X,Y,Orientation */ public double [] getCartesianPose(){ double [] angles =getAngles(); - pose[2] = GetOrentation(); + pose[2] = GetOrientation(); - pose[0]=(l1* cos(ToRadians(angles[0]))+l2* cos(ToRadians(angles[0])+ToRadians(angles[1]))+(l3* cos(ToRadians(GetOrentation())))); - pose[1]=(l1* sin(ToRadians(angles[0]))+l2* sin(ToRadians(angles[0])+ToRadians(angles[1]))+(l3* sin(ToRadians(GetOrentation())))); + pose[0]=(l1* cos(ToRadians(angles[0]))+l2* cos(ToRadians(angles[0])+ToRadians(angles[1]))+(l3* cos(ToRadians(GetOrientation())))); + pose[1]=(l1* sin(ToRadians(angles[0]))+l2* sin(ToRadians(angles[0])+ToRadians(angles[1]))+(l3* sin(ToRadians(GetOrientation())))); double [] p = new double [3]; for ( int i = 0; i<3; i++){ p[i]=pose[i]; @@ -318,10 +318,10 @@ public void setCartesianPose(double [] p, float time){ * * @param x the x * @param y the y - * @param orentation the orentation + * @param orientation the orientation */ - public void setCartesianPose(double x, double y, double orentation){ - setCartesianPose(x,y, orentation,(float).2); + public void setCartesianPose(double x, double y, double orientation){ + setCartesianPose(x,y, orientation,(float).2); } /** @@ -329,26 +329,26 @@ public void setCartesianPose(double x, double y, double orentation){ * * @param x the x * @param y the y - * @param orentation the orentation + * @param orientation the orientation * @param time the time */ - public void setCartesianPose(double x, double y, double orentation, float time){ - if(orentation<-35) - orentation=-35; - if(orentation>35) - orentation=35; - if (!updateCartesian(x,y,orentation)){ + public void setCartesianPose(double x, double y, double orientation, float time){ + if(orientation<-35) + orientation=-35; + if(orientation>35) + orientation=35; + if (!updateCartesian(x,y,orientation)){ return; } pose[0]=x; pose[1]=y; - pose[2]=orentation; + pose[2]=orientation; - Log.info("Setting Pose X: "+x+" Y: "+y+" Orentation: "+orentation ); + Log.info("Setting Pose X: "+x+" Y: "+y+" Orientation: "+orientation ); - x -= (l3*cos(orentation*M_PI/180)); - y -= (l3*sin(orentation*M_PI/180)); + x -= (l3*cos(orientation*M_PI/180)); + y -= (l3*sin(orientation*M_PI/180)); if (sqrt(x*x+y*y) > l1+l2) { com.neuronrobotics.sdk.common.Log.error("Hypotenus too long"+x+" "+y+"\r\n"); return; @@ -361,7 +361,7 @@ public void setCartesianPose(double x, double y, double orentation, float time){ shoulder =(atan2(y,x)+acos((x*x+y*y+l1*l1-l2*l2)/(2*l1*sqrt(x*x+y*y)))); shoulder *=(180.0/M_PI); - double wrist = orentation-elbow-shoulder; + double wrist = orientation-elbow-shoulder; setAngles(shoulder,elbow,wrist,time); @@ -372,10 +372,10 @@ public void setCartesianPose(double x, double y, double orentation, float time){ * * @param x the x * @param y the y - * @param orentation the orentation + * @param orientation the orientation * @return true, if successful */ - private boolean updateCartesian(double x, double y, double orentation) { + private boolean updateCartesian(double x, double y, double orientation) { if(((x>(pose[0]+xyThreshHold))) || (x<(pose[0]-xyThreshHold))){ Log.info("X changed"); return true; @@ -388,8 +388,8 @@ private boolean updateCartesian(double x, double y, double orentation) { }else{ Log.info("Y set: "+y+" was: "+pose[1]); } - if((orentation>pose[2]+orentThreshHold) || (orentationpose[2]+orientThreshHold) || (orientation4) - inv[4] = inv[0];//keep the tool orentation paralell from the base + inv[4] = inv[0];//keep the tool orientation paralell from the base for(int i=0;i=0;i--){ @@ -60,13 +60,13 @@ public double[] inverseKinematics(TransformNR target,double[] jointSpaceVector, } } vect = dhChain.forwardKinematics(jointSpaceVector).getOffsetVectorMagnitude(target); - orent = dhChain.forwardKinematics(jointSpaceVector).getOffsetOrentationMagnitude(target); - if(previousV>=vect && previousO>=orent){ + orient = dhChain.forwardKinematics(jointSpaceVector).getOffsetOrientationMagnitude(target); + if(previousV>=vect && previousO>=orient){ for(int i=0;i posOffset|| previousO > .001); @@ -83,7 +83,7 @@ public double[] inverseKinematics(TransformNR target,double[] jointSpaceVector, } }while(++iter<200 && notArrived && stopped == false);//preincrement and check if(debug){ - com.neuronrobotics.sdk.common.Log.error("Numer of iterations #"+iter+" \n\tStalled = "+stopped+" \n\tArrived = "+!notArrived+" \n\tFinal offset= "+vect+" \n\tFinal orent= "+orent); + com.neuronrobotics.sdk.common.Log.error("Numer of iterations #"+iter+" \n\tStalled = "+stopped+" \n\tArrived = "+!notArrived+" \n\tFinal offset= "+vect+" \n\tFinal orient= "+orient); } return inv; } diff --git a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/GradiantDecentNode.java b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/GradiantDecentNode.java index 9df51a52..117784ae 100644 --- a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/GradiantDecentNode.java +++ b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/GradiantDecentNode.java @@ -36,8 +36,8 @@ public class GradiantDecentNode{ /** The inc vect. */ double incVect; - /** The inc orent. */ - double incOrent; + /** The inc orient. */ + double incOrient; /** The integral size. */ //integral @@ -46,20 +46,20 @@ public class GradiantDecentNode{ /** The integral index vect. */ int integralIndexVect = 0; - /** The integral index orent. */ - int integralIndexOrent = 0; + /** The integral index orient. */ + int integralIndexOrient = 0; /** The integral total vect. */ double integralTotalVect = 0; - /** The integral total orent. */ - double integralTotalOrent = 0; + /** The integral total orient. */ + double integralTotalOrient = 0; /** The int vect. */ double intVect[] = new double[integralSize]; - /** The int orent. */ - double intOrent[] = new double[integralSize]; + /** The int orient. */ + double intOrient[] = new double[integralSize]; /** The Kp. */ double Kp = 1; @@ -88,62 +88,62 @@ public GradiantDecentNode(DHChain chain,int index,double[] jointSpaceVector,Tran lower = l; for(int i=0;inoneOrent && downOrent>noneOrent)){ + if( (upOrient>noneOrient && downOrient>noneOrient)){ jointSpaceVector[getIndex()]=none; } - if(( noneOrent>upOrent && downOrent>upOrent)){ + if(( noneOrient>upOrient && downOrient>upOrient)){ jointSpaceVector[getIndex()]=upO; - offset+=incOrent; + offset+=incOrient; } - if((upOrent>downOrent && noneOrent>downOrent )){ + if((upOrient>downOrient && noneOrient>downOrient )){ jointSpaceVector[getIndex()]=downO; - offset-=incOrent; + offset-=incOrient; } jointSpaceVector[getIndex()] = myStart+offset; @@ -218,7 +218,7 @@ public boolean stepLin(){ * @return true, if successful */ public boolean step() { - boolean back = stepOrent()||stepLin(); + boolean back = stepOrient()||stepLin(); return back; } diff --git a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/SearchTreeSolver.java b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/SearchTreeSolver.java index 0b367186..7a943994 100644 --- a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/SearchTreeSolver.java +++ b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/SearchTreeSolver.java @@ -52,7 +52,7 @@ public double[] inverseKinematics(TransformNR target,double[] jointSpaceVector, searchTree step=new searchTree(jointSpaceVector,startingIncrement);; boolean done = false; configuration conf = new configuration(jointSpaceVector, target); -// double previousV =conf.getOffsetOrentationMagnitude(); +// double previousV =conf.getOffsetOrientationMagnitude(); // double previousO =conf.getOffsetVectorMagnitude(); int iter = 1000; int i = 0; @@ -60,10 +60,10 @@ public double[] inverseKinematics(TransformNR target,double[] jointSpaceVector, double [] current = conf.getJoints(); conf = step.getBest(current); - double vect = conf.getOffsetOrentationMagnitude(); - double orent = conf.getOffsetVectorMagnitude(); + double vect = conf.getOffsetOrientationMagnitude(); + double orient = conf.getOffsetVectorMagnitude(); - if(vect<10 && orent< .05){ + if(vect<10 && orient< .05){ done = true; com.neuronrobotics.sdk.common.Log.error("SearchTreeSolver Success stats: \n\tIterations = "+i+" out of "+iter+"\n"+conf); } @@ -199,15 +199,15 @@ public configuration getBest(double[] jointSpaceVector){ int best = 0; int i=0; - double orent=configurations.get(0).getOffsetOrentationMagnitude(); + double orient=configurations.get(0).getOffsetOrientationMagnitude(); double vect =configurations.get(0).getOffsetVectorMagnitude(); for(configuration c:configurations){ - double tmpOrent = c.getOffsetOrentationMagnitude(); + double tmpOrient = c.getOffsetOrientationMagnitude(); double tmpVector= c.getOffsetVectorMagnitude(); if( - tmpOrent<=orent && + tmpOrient<=orient && tmpVector<=vect){ - orent=tmpOrent; + orient=tmpOrient; vect=tmpVector; best = i; } @@ -257,7 +257,7 @@ public configuration(double [] joints, TransformNR t){ public TransformNR getTransform() { if(transform == null){ transform = fk(getJoints()); - o = transform.getOffsetOrentationMagnitude(target); + o = transform.getOffsetOrientationMagnitude(target); v = transform.getOffsetVectorMagnitude(target); } return transform; @@ -273,11 +273,11 @@ public double[] getJoints() { } /** - * Gets the offset orentation magnitude. + * Gets the offset orientation magnitude. * - * @return the offset orentation magnitude + * @return the offset orientation magnitude */ - public double getOffsetOrentationMagnitude(){ + public double getOffsetOrientationMagnitude(){ getTransform(); return o; } @@ -313,7 +313,7 @@ public boolean same(configuration c){ */ public String toString(){ getTransform(); - String s="\tTarget = "+target.toString()+"\n\tVector = "+v+"\n\tOrent "+o+"\n\tCurrent = "+getTransform().toString(); + String s="\tTarget = "+target.toString()+"\n\tVector = "+v+"\n\tOrient "+o+"\n\tCurrent = "+getTransform().toString(); return s; } } diff --git a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/ik/DeltaIKModel.java b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/ik/DeltaIKModel.java index 5c55a71f..7395ee0a 100644 --- a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/ik/DeltaIKModel.java +++ b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/ik/DeltaIKModel.java @@ -123,9 +123,9 @@ public double[] inverseKinematics6dof(TransformNR target, double[] jointSpaceVec (2 * L1 *elX) ); jointSpaceVector[0]=-(90-(Math.toDegrees(theta1)+baseVectorAngle)); - TransformNR reorent; + TransformNR reorient; try { - reorent =new TransformNR(0,0,0,new RotationNR(0, -jointSpaceVector[0], 0)); + reorient =new TransformNR(0,0,0,new RotationNR(0, -jointSpaceVector[0], 0)); }catch (Throwable t){ //t.printStackTrace() throw new RuntimeException( "error calculating base angle: \nL1 "+L1+ @@ -138,7 +138,7 @@ public double[] inverseKinematics6dof(TransformNR target, double[] jointSpaceVec ); } - TransformNR sphericalElbowTartget = reorent.times(newCenter); + TransformNR sphericalElbowTartget = reorient.times(newCenter); //com.neuronrobotics.sdk.common.Log.error( newCenter //com.neuronrobotics.sdk.common.Log.error( sphericalElbowTartget sphericalElbowTartget = new TransformNR(0.0,-sphericalElbowTartget.getY(),0.0, new RotationNR()).times(sphericalElbowTartget); diff --git a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/TransformNR.java b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/TransformNR.java index 8406eecf..7c3e379b 100644 --- a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/TransformNR.java +++ b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/TransformNR.java @@ -339,12 +339,12 @@ public Matrix getMatrixTransform() { } /** - * Gets the offset orentation magnitude. + * Gets the offset orientation magnitude. * * @param t the t - * @return the offset orentation magnitude + * @return the offset orientation magnitude */ - public double getOffsetOrentationMagnitude(TransformNR t) { + public double getOffsetOrientationMagnitude(TransformNR t) { double x = getRotation().getRotationMatrix2QuaturnionX() - t.getRotation().getRotationMatrix2QuaturnionX(); double y = getRotation().getRotationMatrix2QuaturnionY() From cf921fd9c98e814a08dfd49f7fa69f1da0b2ce86 Mon Sep 17 00:00:00 2001 From: rondlh <77279634+rondlh@users.noreply.github.com> Date: Sun, 15 Mar 2026 20:15:48 +0800 Subject: [PATCH 2/3] Solve some warnings, some formatting fixes Solve some warnings, some formatting fixes --- .../kinematics/AbstractKinematicsNR.java | 86 ++++---- .../addons/kinematics/WalkingDriveEngine.java | 8 +- .../addons/kinematics/ik/DeltaIKModel.java | 58 +++--- .../addons/kinematics/math/RotationNR.java | 97 ++++----- .../kinematics/math/RotationNRLegacy.java | 10 +- .../addons/kinematics/math/TransformNR.java | 71 +++---- .../sdk/common/RpcEncapsulation.java | 189 +++++++++--------- .../utilities/RotationNRTest.java | 98 ++++----- 8 files changed, 298 insertions(+), 319 deletions(-) diff --git a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/AbstractKinematicsNR.java b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/AbstractKinematicsNR.java index 2233dada..dc76104a 100644 --- a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/AbstractKinematicsNR.java +++ b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/AbstractKinematicsNR.java @@ -117,15 +117,15 @@ public abstract class AbstractKinematicsNR extends NonBowlerDevice implements IP private Runnable renderWrangler=null; public int getLinkIndex(AbstractLink l) { - for (int i=0;i link.getMaxEngineeringUnits()) { - Log.error(dev.getScriptingName()+" Link "+i+" can not reach "+val+" limited to "+link.getMaxEngineeringUnits()); + Log.error(dev.getScriptingName() + " Link " + i + " can not reach " + val + " limited to " + link.getMaxEngineeringUnits()); return false; } if (val < link.getMinEngineeringUnits()) { - Log.error(dev.getScriptingName()+" Link "+i+" can not reach "+val+" limited to "+link.getMinEngineeringUnits()); + Log.error(dev.getScriptingName() + " Link " + i + " can not reach " + val + " limited to " + link.getMinEngineeringUnits()); return false; } - if(seconds>0) { + if (seconds > 0) { double maxVel = Math.abs(link.getMaxVelocityEngineeringUnits()); double deltaPosition = Math.abs(current[i] - jointSpaceVect[i]); - double computedVelocity = deltaPosition/seconds; - if((computedVelocity-maxVel)>0.0001) { - 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"); + double computedVelocity = deltaPosition / seconds; + if ((computedVelocity-maxVel)>0.0001) { + 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"); return false; } } @@ -646,6 +647,7 @@ private static boolean checkVector(AbstractKinematicsNR dev, double[] jointSpace return true; } + /** * Checks the desired pose for ability for the IK to calculate a valid pose. * @@ -653,7 +655,7 @@ private static boolean checkVector(AbstractKinematicsNR dev, double[] jointSpace * @return True if pose is reachable, false if it is not */ public boolean checkTaskSpaceTransform(TransformNR taskSpaceTransform, double seconds) { - return AbstractKinematicsNR.checkTaskSpaceTransform(this, taskSpaceTransform,seconds); + return AbstractKinematicsNR.checkTaskSpaceTransform(this, taskSpaceTransform, seconds); } /** * 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 * @return True if pose is reachable, false if it is not */ public boolean checkTaskSpaceTransform(TransformNR taskSpaceTransform) { - return checkTaskSpaceTransform(this, taskSpaceTransform,0); + return checkTaskSpaceTransform(this, taskSpaceTransform, 0); } /** @@ -689,7 +691,7 @@ public double getBestTime(TransformNR currentTaskSpaceTransform) { * @return the time of translation at best possible speed based on checking each link */ public double getBestTime(double[] jointSpaceVect) { - double best=0; + double best = 0; double[] current = getCurrentJointSpaceTarget(); for (int i = 0; i < current.length; i++) { AbstractLink link = getAbstractLink(i); @@ -730,8 +732,8 @@ private double[] _setDesiredJointSpaceVector(double[] jointSpaceVect, double se + " links, actual number of links = " + jointSpaceVect.length); } double best = getBestTime(jointSpaceVect); - if(seconds linkConfigurations = getLinkConfigurations(); -// if(linkConfigurations!=null) { +// if (linkConfigurations!=null) { // int indexOf = linkConfigurations.indexOf(c); -// if(currentJointSpacePositions!=null) -// if(indexOf>=0 && indexOf= 0) && (indexOf < currentJointSpacePositions.length)) // currentJointSpacePositions[indexOf] = engineeringUnitsValue; // } firePoseUpdate(); @@ -1200,7 +1204,7 @@ public void onPIDEvent(PIDEvent e) { * @param link the link */ public void homeLink(int link) { - if (link < 0 || link >= getNumberOfLinks()) { + if ((link < 0) || (link >= getNumberOfLinks())) { throw new IndexOutOfBoundsException( "There are only " + getNumberOfLinks() + " known links, requested:" + link); } @@ -1637,7 +1641,7 @@ public void clearChangeListener(int linkIndex) { public void runRenderWrangler() { firePoseUpdate(); - if(renderWrangler!=null) + if (renderWrangler != null) try { renderWrangler.run(); }catch(Throwable t) { @@ -1719,7 +1723,7 @@ public InterpolationMoveState blockingInterpolatedMove(TransformNR target, doubl } wait((int) msPerStep); } - }else { + } else { return InterpolationMoveState.FAULT; } return InterpolationMoveState.READY; @@ -1728,7 +1732,7 @@ public InterpolationMoveState blockingInterpolatedMove(TransformNR target, doubl public void setTimeProvider(ITimeProvider t) { super.setTimeProvider(t); imu.setTimeProvider(getTimeProvider()); - for(int i=0;i chainToLoad =new ArrayList<>(); chain.forwardKinematicsMatrix(wristLinks,chainToLoad); TransformNR startOfWristSet=chain.kin.inverseOffset(chainToLoad.get(2)); - TransformNR virtualcenter = newCenter.times(new TransformNR(0,0,10, - new RotationNR(Math.toDegrees(links.get(5).getAlpha()),0,0))); + TransformNR virtualcenter = newCenter.times(new TransformNR(0, 0, 10, + new RotationNR(Math.toDegrees(links.get(5).getAlpha()),0 ,0))); TransformNR wristMOvedToCenter0 =startOfWristSet .inverse()// move back from base ot wrist to world home .times(virtualcenter);// move forward to target, leaving the angle between the tip and the start of the rotation //if(debug)com.neuronrobotics.sdk.common.Log.error( wristMOvedToCenter0 RotationNR qWrist=wristMOvedToCenter0.getRotation(); - if(wristMOvedToCenter0.getX()==0&&wristMOvedToCenter0.getY()==0) { + if ((wristMOvedToCenter0.getX() == 0) && (wristMOvedToCenter0.getY() == 0)) { com.neuronrobotics.sdk.common.Log.error( "Singularity! try something else"); return inverseKinematics6dof(target.copy().translateX(0.01),jointSpaceVector,chain); } - double closest= (Math.toDegrees(Math.atan2(wristMOvedToCenter0.getY(), wristMOvedToCenter0.getX()))-Math.toDegrees(links.get(3).getTheta())); + double closest = (Math.toDegrees(Math.atan2(wristMOvedToCenter0.getY(), wristMOvedToCenter0.getX())) - Math.toDegrees(links.get(3).getTheta())); - jointSpaceVector[3]=closest; - wristLinks[3]=jointSpaceVector[3]; - if(jointSpaceVector.length==4) + jointSpaceVector[3] = closest; + wristLinks[3] = jointSpaceVector[3]; + if(jointSpaceVector.length == 4) return jointSpaceVector; - chainToLoad =new ArrayList<>(); + chainToLoad = new ArrayList<>(); /** // Calculte the second angle * @@ -193,18 +193,18 @@ public double[] inverseKinematics6dof(TransformNR target, double[] jointSpaceVec .inverse()// move back from base ot wrist to world home .times(virtualcenter);// move forward to target, leaving the angle between the tip and the start of the rotation //if(debug)com.neuronrobotics.sdk.common.Log.error( " Middle link =" +wristMOvedToCenter1 - RotationNR qWrist2=wristMOvedToCenter1.getRotation(); - if(wristMOvedToCenter1.getX()==0&&wristMOvedToCenter1.getY()==0) { + RotationNR qWrist2 = wristMOvedToCenter1.getRotation(); + if (wristMOvedToCenter1.getX()==0&&wristMOvedToCenter1.getY() == 0) { com.neuronrobotics.sdk.common.Log.error( "Singularity! try something else"); return inverseKinematics6dof(target.copy().translateX(0.01),jointSpaceVector,chain); } - jointSpaceVector[4]=(Math.toDegrees(Math.atan2(wristMOvedToCenter1.getY(), wristMOvedToCenter1.getX()))- - Math.toDegrees(links.get(4).getTheta())- - 90); - wristLinks[4]=jointSpaceVector[4]; - if(jointSpaceVector.length==5) + jointSpaceVector[4] = (Math.toDegrees(Math.atan2(wristMOvedToCenter1.getY(), wristMOvedToCenter1.getX())) - + Math.toDegrees(links.get(4).getTheta()) - 90); + wristLinks[4] = jointSpaceVector[4]; + if (jointSpaceVector.length == 5) return jointSpaceVector; - chainToLoad =new ArrayList<>(); + + chainToLoad = new ArrayList<>(); /** // Calculte the last angle * @@ -212,14 +212,14 @@ public double[] inverseKinematics6dof(TransformNR target, double[] jointSpaceVec chain.forwardKinematicsMatrix(wristLinks,chainToLoad); TransformNR startOfWristSet3=chain.kin.inverseOffset(chainToLoad.get(4)); TransformNR tool = new TransformNR(); - if(linkNum==7) + if (linkNum == 7) tool=linkOffset(links.get(6)); TransformNR wristMOvedToCenter2 =startOfWristSet3 .inverse()// move back from base ot wrist to world home .times(target.times(tool.inverse()));// move forward to target, leaving the angle between the tip and the start of the rotation //if(debug)com.neuronrobotics.sdk.common.Log.error( "\n\nLastLink " +wristMOvedToCenter2 - RotationNR qWrist3=wristMOvedToCenter2.getRotation(); - jointSpaceVector[5]=(Math.toDegrees(qWrist3.getRotationAzimuth())-Math.toDegrees(links.get(5).getTheta())); + RotationNR qWrist3 = wristMOvedToCenter2.getRotation(); + jointSpaceVector[5] = (Math.toDegrees(qWrist3.getRotationAzimuthRadians()) - Math.toDegrees(links.get(5).getTheta())); return jointSpaceVector; } diff --git a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/RotationNR.java b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/RotationNR.java index 4c1cd562..84a35b2d 100644 --- a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/RotationNR.java +++ b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/RotationNR.java @@ -25,13 +25,13 @@ public class RotationNR { // double[][] rotationMatrix = ; @Expose (serialize = true, deserialize = true) - double w=1; + double w = 1; @Expose (serialize = true, deserialize = true) - double x=0; + double x = 0; @Expose (serialize = true, deserialize = true) - double y=0; + double y = 0; @Expose (serialize = true, deserialize = true) - double z=0; + double z = 0; //private Rotation storage = new Rotation(1, 0, 0, 0, false); @Expose (serialize = false, deserialize = false) private static RotationOrder order = RotationOrder.ZYX; @@ -60,12 +60,9 @@ public RotationNR(Rotation store) { /** * Instantiates a new rotation nr. * - ** @param tilt - * the tilt in Degrees - * @param azimuth - * the azimuth in Degrees - * @param elevation - * the elevation in Degrees + ** @param tilt the tilt in Degrees + * @param azimuth the azimuth in Degrees + * @param elevation the elevation in Degrees */ // create a new object with the given simplified rotations public RotationNR(double tilt, double azimuth, double elevation) { @@ -87,6 +84,7 @@ public RotationNR(double tilt, double azimuth, double elevation) { } } + public RotationNR(EulerAxis axis, double rot) { this(axis==EulerAxis.tilt?rot:0,axis==EulerAxis.azimuth?rot:0,axis==EulerAxis.elevation?rot:0); } @@ -94,8 +92,7 @@ public RotationNR(EulerAxis axis, double rot) { /** * Instantiates a new rotation nr. * - * @param rotationMatrix - * the rotation matrix + * @param rotationMatrix the rotation matrix */ public RotationNR(double[][] rotationMatrix) { loadRotations(rotationMatrix); @@ -104,8 +101,7 @@ public RotationNR(double[][] rotationMatrix) { /** * Instantiates a new rotation nr. * - * @param values - * the values + * @param values the values */ public RotationNR(double[] values) { this(values[0], values[1], values[2], values[3]); @@ -114,8 +110,7 @@ public RotationNR(double[] values) { /** * Get a rotation matrix with a rotation around X. * - * @param rotationAngleDegrees - * in degrees + * @param rotationAngleDegrees in degrees * @return the static matrix */ public static RotationNR getRotationX(double rotationAngleDegrees) { @@ -141,8 +136,7 @@ public static RotationNR getRotationX(double rotationAngleDegrees) { /** * Get a rotation matrix with a rotation around Y. * - * @param rotationAngleDegrees - * in degrees + * @param rotationAngleDegrees in degrees * @return the static matrix */ public static RotationNR getRotationY(double rotationAngleDegrees) { @@ -168,8 +162,7 @@ public static RotationNR getRotationY(double rotationAngleDegrees) { /** * Get a rotation matrix with a rotation around Z. * - * @param rotationAngleDegrees - * in degrees + * @param rotationAngleDegrees in degrees * @return the static matrix */ public static RotationNR getRotationZ(double rotationAngleDegrees) { @@ -195,14 +188,10 @@ public static RotationNR getRotationZ(double rotationAngleDegrees) { /** * Instantiates a new rotation nr. * - * @param w - * the w - * @param x - * the x - * @param y - * the y - * @param z - * the z + * @param w the w + * @param x the x + * @param y the y + * @param z the z */ // create a new object with the given components public RotationNR(double w, double x, double y, double z) { @@ -212,8 +201,7 @@ public RotationNR(double w, double x, double y, double z) { /** * Instantiates a new rotation nr. * - * @param m - * the m + * @param m the m */ public RotationNR(Matrix m) { double[][] rotation = new double[3][3]; @@ -228,8 +216,7 @@ public RotationNR(Matrix m) { /** * Load rotations. * - * @param rotM - * the rot m + * @param rotM the rot m */ private void loadRotations(double[][] rotM) { if (rotM.length != 3) @@ -262,10 +249,10 @@ public String toString() { try{ return "Quaturnion: " + "W=" + getRotationMatrix2QuaturnionW() + ", " + "x=" + getRotationMatrix2QuaturnionX() + ", " + "y=" + getRotationMatrix2QuaturnionY() + ", " + "z=" + getRotationMatrix2QuaturnionZ() + "\n" - + "Rotation angle (degrees): " + "az= " + Math.toDegrees(getRotationAzimuth()) + ", elev= " + + "Rotation angle (degrees): " + "az= " + Math.toDegrees(getRotationAzimuthRadians()) + ", elev= " + Math.toDegrees(getRotationElevation()) + ", tilt=" + Math.toDegrees(getRotationTilt()); }catch(Exception ex){ - return "Rotation error"+ex.getLocalizedMessage(); + return "Rotation error" + ex.getLocalizedMessage(); } } @@ -273,8 +260,7 @@ public String toString() { /** * To string. * - * @param array - * the array + * @param array the array * @return the string */ // return a string representation of the invoking object @@ -294,14 +280,10 @@ public String toString(double[][] array) { /** * Quaternion2 rotation matrix. * - * @param w - * the w - * @param x - * the x - * @param y - * the y - * @param z - * the z + * @param w the w + * @param x the x + * @param y the y + * @param z the z */ protected void quaternion2RotationMatrix(double w, double x, double y, double z) { if (Double.isNaN(w)) @@ -321,12 +303,9 @@ protected void quaternion2RotationMatrix(double w, double x, double y, double z) /** * Bound. * - * @param low - * the low - * @param high - * the high - * @param n - * the n + * @param low the low + * @param high the high + * @param n the n * @return true, if successful */ public static boolean bound(double low, double high, double n) { @@ -365,6 +344,7 @@ public double getRotationElevationRadians() { public double getRotationAzimuthRadians() { return getAngle(0); } + /** * Gets the rotation tilt. * @@ -393,6 +373,7 @@ public double getRotationElevationDegrees() { public double getRotationAzimuthDegrees() { return Math.toDegrees( getRotationAzimuthRadians()); } + /** * Gets the rotation tilt. * @@ -423,6 +404,7 @@ public double getRotationElevation() { public double getRotationAzimuth() { return getRotationAzimuthRadians(); } + private void simpilfyAngles(double [] angles){ double epsilon=1.0E-7; if(Math.abs(angles[0] - Math.toRadians(180)) < epsilon&& @@ -433,6 +415,7 @@ private void simpilfyAngles(double [] angles){ angles[2]=0; } } + private double eulerFix(double offsetSize, int index){ double offset = (index==1?offsetSize:0); TransformNR current = new TransformNR(0, 0, 0, this); @@ -442,6 +425,7 @@ private double eulerFix(double offsetSize, int index){ double finalResult= angles[index]; return finalResult+offset; } + private double getAngle(int index){ try { @@ -455,7 +439,6 @@ private double getAngle(int index){ } } } - /** * Gets the rotation matrix2 quaturnion w. @@ -510,20 +493,18 @@ public static void setConvention(RotationConvention convention) { } private Rotation getStorage() { - return new Rotation(w,x,y,z,false); + return new Rotation(w, x, y, z, false); } private void setStorage(Rotation storage) { - w=storage.getQ0(); - x=storage.getQ1(); - y=storage.getQ2(); - z=storage.getQ3(); + w = storage.getQ0(); + x = storage.getQ1(); + y = storage.getQ2(); + z = storage.getQ3(); } public void set(double[][] poseRot) { loadRotations(poseRot); } - - } \ No newline at end of file diff --git a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/RotationNRLegacy.java b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/RotationNRLegacy.java index c159a582..4ce0977f 100644 --- a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/RotationNRLegacy.java +++ b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/RotationNRLegacy.java @@ -272,8 +272,8 @@ public String toString() { s += "]"; return "Quaturnion: " + "W=" + getRotationMatrix2QuaturnionW() + ", " + "x=" + getRotationMatrix2QuaturnionX() + ", " + "y=" + getRotationMatrix2QuaturnionY() + ", " + "z=" + getRotationMatrix2QuaturnionZ() + "\t" - + "Rotation angle (degrees): " + "Azimuth=" + getRotationAzimuth() + ", " + "Elevation=" + getRotationElevation() + ", " + "Tilt=" - + getRotationTilt() + ""; + + "Rotation angle (degrees): " + "Azimuth=" + getRotationAzimuthRadians() + ", " + "Elevation=" + getRotationElevationRadians() + ", " + "Tilt=" + + getRotationTiltRadians() + ""; } /** @@ -552,7 +552,7 @@ private double getRotAngle(int index) { * * @return the rotation tilt */ - public double getRotationTilt() { + public double getRotationTiltRadians() { return getRotAngle(0); @@ -563,7 +563,7 @@ public double getRotationTilt() { * * @return the rotation elevation */ - public double getRotationElevation() { + public double getRotationElevationRadians() { return getRotAngle(1); } @@ -573,7 +573,7 @@ public double getRotationElevation() { * * @return the rotation azimuth */ - public double getRotationAzimuth() { + public double getRotationAzimuthRadians() { return getRotAngle(2); } diff --git a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/TransformNR.java b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/TransformNR.java index 7c3e379b..4481ff1f 100644 --- a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/TransformNR.java +++ b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/TransformNR.java @@ -250,9 +250,11 @@ public String toString() { return "Transform error" + ex.getLocalizedMessage(); } } + public String toSimpleString() { return toPositionString()+" "+toAngleString(); } + public String toPositionString() { DecimalFormat decimalFormat = new DecimalFormat("000.00"); @@ -260,12 +262,13 @@ public String toPositionString() { "y="+decimalFormat.format(y)+" "+ "z="+decimalFormat.format(z); } + public String toAngleString() { DecimalFormat decimalFormat = new DecimalFormat("000.00"); - return "az="+decimalFormat.format(Math.toDegrees(getRotation().getRotationAzimuth()))+" "+ - "el="+decimalFormat.format(Math.toDegrees(getRotation().getRotationElevation()))+" "+ - "tl="+decimalFormat.format(Math.toDegrees(getRotation().getRotationTilt())); + return "az=" + decimalFormat.format(Math.toDegrees(getRotation().getRotationAzimuthRadians())) + " " + + "el=" + decimalFormat.format(Math.toDegrees(getRotation().getRotationElevationRadians())) + " " + + "tl=" + decimalFormat.format(Math.toDegrees(getRotation().getRotationTiltRadians())); } /** @@ -321,14 +324,13 @@ public Matrix getMatrixTransform() { double[][] rotation = getRotationMatrixArray(); - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) transform[i][j] = rotation[i][j]; - } - } - for (int i = 0; i < 3; i++) { + + for (int i = 0; i < 3; i++) transform[3][i] = 0; - } + transform[3][3] = 1; transform[0][3] = getX(); transform[1][3] = getY(); @@ -400,9 +402,9 @@ public TransformNR scale(double t) { if (t <= 0) return new TransformNR(); - double tilt = Math.toDegrees(getRotation().getRotationTilt() * t); - double az = Math.toDegrees(getRotation().getRotationAzimuth() * t); - double ele = Math.toDegrees(getRotation().getRotationElevation() * t); + double tilt = Math.toDegrees(getRotation().getRotationTiltRadians() * t); + double az = Math.toDegrees(getRotation().getRotationAzimuthRadians() * t); + double ele = Math.toDegrees(getRotation().getRotationElevationRadians() * t); return new TransformNR(getX() * t, getY() * t, getZ() * t, new RotationNR(tilt, az, ele)); } @@ -572,52 +574,51 @@ void fireChangeEvent() { } public void setTiltDegrees(double newAngleDegrees) { - double e=0; - try{ - e=Math.toDegrees(getRotation().getRotationElevation()); + double e = 0; + try { + e = Math.toDegrees(getRotation().getRotationElevationRadians()); }catch(Exception ex){ ex.printStackTrace(); } - double a=0; - try{ - a=Math.toDegrees(getRotation().getRotationAzimuth()); - }catch(Exception ex){ + double a = 0; + try { + a = Math.toDegrees(getRotation().getRotationAzimuthRadians()); + } catch(Exception ex){ ex.printStackTrace(); } setRotation(new RotationNR(newAngleDegrees, a, e)); - - + } public void setElevationDegrees(double newAngleDegrees) { - double t=0; - try{ - t=Math.toDegrees(getRotation().getRotationTilt()); - }catch(Exception ex){ + double t = 0; + try { + t = Math.toDegrees(getRotation().getRotationTiltRadians()); + } catch (Exception ex){ ex.printStackTrace(); } - double a=0; - try{ - a=Math.toDegrees(getRotation().getRotationAzimuth()); - }catch(Exception ex){ + double a = 0; + try { + a = Math.toDegrees(getRotation().getRotationAzimuthRadians()); + } catch(Exception ex){ ex.printStackTrace(); } setRotation(new RotationNR(t, a, newAngleDegrees)); } public void setAzimuthDegrees(double newAngleDegrees) { - double t=0; - try{ - t=Math.toDegrees(getRotation().getRotationTilt()); - }catch(Exception ex){ + double t = 0; + try { + t = Math.toDegrees(getRotation().getRotationTiltRadians()); + } catch (Exception ex) { ex.printStackTrace(); } - double e=0; + double e = 0; try{ - e=Math.toDegrees(getRotation().getRotationElevation()); + e = Math.toDegrees(getRotation().getRotationElevationRadians()); }catch(Exception ex){ ex.printStackTrace(); } diff --git a/src/main/java/com/neuronrobotics/sdk/common/RpcEncapsulation.java b/src/main/java/com/neuronrobotics/sdk/common/RpcEncapsulation.java index 40254ac9..ec2d1ab2 100644 --- a/src/main/java/com/neuronrobotics/sdk/common/RpcEncapsulation.java +++ b/src/main/java/com/neuronrobotics/sdk/common/RpcEncapsulation.java @@ -45,7 +45,7 @@ public class RpcEncapsulation { */ public RpcEncapsulation(int namespaceIndex,String namespace, String rpc, BowlerMethod downStreamMethod,BowlerDataType[] downstreamArguments, - BowlerMethod upStreamMethod,BowlerDataType[] upstreamArguments){ + BowlerMethod upStreamMethod,BowlerDataType[] upstreamArguments) { this(namespaceIndex, namespace, rpc, downStreamMethod, downstreamArguments, upStreamMethod, upstreamArguments, null); } @@ -63,7 +63,7 @@ public RpcEncapsulation(int namespaceIndex,String namespace, String rpc, */ public RpcEncapsulation(int namespaceIndex,String namespace, String rpc, BowlerMethod downStreamMethod,BowlerDataType[] downstreamArguments, - BowlerMethod upStreamMethod,BowlerDataType[] upstreamArguments, IBowlerCommandProcessor processor){ + BowlerMethod upStreamMethod,BowlerDataType[] upstreamArguments, IBowlerCommandProcessor processor) { this.setProcessor(processor); this.setNamespaceIndex(namespaceIndex); this.setNamespace(namespace); @@ -79,7 +79,7 @@ public RpcEncapsulation(int namespaceIndex,String namespace, String rpc, * @param upStreamMethod the up stream method * @param upstreamArguments the upstream arguments */ - public void setArguments(BowlerMethod downStreamMethod,BowlerDataType[] downstreamArguments, BowlerMethod upStreamMethod,BowlerDataType[] upstreamArguments){ + public void setArguments(BowlerMethod downStreamMethod,BowlerDataType[] downstreamArguments, BowlerMethod upStreamMethod,BowlerDataType[] upstreamArguments) { this.setUpStreamMethod(upStreamMethod); this.setDownstreamArguments(downstreamArguments); this.setUpstreamArguments(upstreamArguments); @@ -92,7 +92,7 @@ public void setArguments(BowlerMethod downStreamMethod,BowlerDataType[] downstre * @param doswnstreamData the doswnstream data * @return the command */ - public BowlerAbstractCommand getCommand(Object [] doswnstreamData){ + public BowlerAbstractCommand getCommand(Object [] doswnstreamData) { return getCommand(doswnstreamData, downstreamArguments); } @@ -102,7 +102,7 @@ public BowlerAbstractCommand getCommand(Object [] doswnstreamData){ * @param doswnstreamData the doswnstream data * @return the command upstream */ - public BowlerAbstractCommand getCommandUpstream(Object [] doswnstreamData){ + public BowlerAbstractCommand getCommandUpstream(Object [] doswnstreamData) { return getCommand(doswnstreamData, upstreamArguments); } @@ -114,16 +114,16 @@ public BowlerAbstractCommand getCommandUpstream(Object [] doswnstreamData){ * @param arguments the arguments * @return the command */ - public BowlerAbstractCommand getCommand(Object [] doswnstreamData, BowlerDataType [] arguments){ + public BowlerAbstractCommand getCommand(Object [] doswnstreamData, BowlerDataType [] arguments) { BowlerAbstractCommand command = new BowlerAbstractCommand() {}; command.setOpCode(getRpc()); command.setMethod(getDownstreamMethod()); command.setNamespaceIndex(getNamespaceIndex()); - for(int i=0;(i0){ + if (numVals > 0) { ByteList d = new ByteList(data.popList(numVals)); - for(int j=0;j= 90 || rotationAngleDegrees <= -90)) { assertArrayEquals( - new double[] { oldRot.getRotationAzimuth(), oldRot.getRotationElevation(), - oldRot.getRotationTilt() }, - new double[] { newRot.getRotationAzimuth(), newRot.getRotationElevation(), - newRot.getRotationTilt() }, + new double[] { oldRot.getRotationAzimuthRadians(), oldRot.getRotationElevationRadians(), + oldRot.getRotationTiltRadians() }, + new double[] { newRot.getRotationAzimuthRadians(), newRot.getRotationElevationRadians(), + newRot.getRotationTiltRadians() }, 0.001); // Check the old rotation against the known value assertArrayEquals(new double[] { Math.toRadians(rotationAngleDegrees), 0, 0 }, new double[] { - oldRot.getRotationAzimuth(), oldRot.getRotationElevation(), oldRot.getRotationTilt() }, + oldRot.getRotationAzimuthRadians(), oldRot.getRotationElevationRadians(), oldRot.getRotationTiltRadians() }, 0.001); } else { com.neuronrobotics.sdk.common.Log.error("Legacy angle would fail here " + rotationAngleDegrees); } // Check the new rotation against the known value assertArrayEquals(new double[] { Math.toRadians(rotationAngleDegrees), 0, 0 }, new double[] { - newRot.getRotationAzimuth(), newRot.getRotationElevation(), newRot.getRotationTilt() }, + newRot.getRotationAzimuthRadians(), newRot.getRotationElevationRadians(), newRot.getRotationTiltRadians() }, 0.001); } // frame(); @@ -252,10 +252,10 @@ public void compareElevation() throws FileNotFoundException { RotationNRLegacy oldRot = new RotationNRLegacy(rotation); double[][] rotationMatrix = newRot.getRotationMatrix(); com.neuronrobotics.sdk.common.Log.error("Testing pure elevation \nrotation " + rotationAngleDegrees + "\n as radian " - + Math.toRadians(rotationAngleDegrees) + "\n Az " + oldRot.getRotationAzimuth() - + "\n El " + oldRot.getRotationElevation() + "\n Tl " + oldRot.getRotationTilt() - + "\n New Az " + newRot.getRotationAzimuth() + "\n New El " + newRot.getRotationElevation() - + "\n New Tl " + newRot.getRotationTilt()); + + Math.toRadians(rotationAngleDegrees) + "\n Az " + oldRot.getRotationAzimuthRadians() + + "\n El " + oldRot.getRotationElevationRadians() + "\n Tl " + oldRot.getRotationTiltRadians() + + "\n New Az " + newRot.getRotationAzimuthRadians() + "\n New El " + newRot.getRotationElevationRadians() + + "\n New Tl " + newRot.getRotationTiltRadians()); assertArrayEquals(rotation[0], rotationMatrix[0], 0.001); assertArrayEquals(rotation[1], rotationMatrix[1], 0.001); assertArrayEquals(rotation[2], rotationMatrix[2], 0.001); @@ -281,21 +281,21 @@ public void compareElevation() throws FileNotFoundException { 0.001); // Check Euler angles assertArrayEquals( - new double[] { oldRot.getRotationAzimuth(), oldRot.getRotationElevation(), - oldRot.getRotationTilt() }, - new double[] { newRot.getRotationAzimuth(), newRot.getRotationElevation(), - newRot.getRotationTilt() }, + new double[] { oldRot.getRotationAzimuthRadians(), oldRot.getRotationElevationRadians(), + oldRot.getRotationTiltRadians() }, + new double[] { newRot.getRotationAzimuthRadians(), newRot.getRotationElevationRadians(), + newRot.getRotationTiltRadians() }, 0.001); // Check the old rotation against the known value assertArrayEquals(new double[] { 0, Math.toRadians(rotationAngleDegrees), 0 }, - new double[] { oldRot.getRotationAzimuth(), oldRot.getRotationElevation(), - oldRot.getRotationTilt() }, + new double[] { oldRot.getRotationAzimuthRadians(), oldRot.getRotationElevationRadians(), + oldRot.getRotationTiltRadians() }, 0.001); // Check the new rotation against the known value assertArrayEquals(new double[] { 0, Math.toRadians(rotationAngleDegrees), 0 }, new double[] { - newRot.getRotationAzimuth(), newRot.getRotationElevation(), newRot.getRotationTilt() }, + newRot.getRotationAzimuthRadians(), newRot.getRotationElevationRadians(), newRot.getRotationTiltRadians() }, 0.001); } // frame(); @@ -346,10 +346,10 @@ public void compareTilt() throws FileNotFoundException { RotationNRLegacy oldRot = new RotationNRLegacy(rotation); double[][] rotationMatrix = newRot.getRotationMatrix(); com.neuronrobotics.sdk.common.Log.error("Testing pure tilt \nrotation " + rotationAngleDegrees + "\n as radian " - + Math.toRadians(rotationAngleDegrees) + "\n Az " + oldRot.getRotationAzimuth() - + "\n El " + oldRot.getRotationElevation() + "\n Tl " + oldRot.getRotationTilt() - + "\n New Az " + newRot.getRotationAzimuth() + "\n New El " + newRot.getRotationElevation() - + "\n New Tl " + newRot.getRotationTilt()); + + Math.toRadians(rotationAngleDegrees) + "\n Az " + oldRot.getRotationAzimuthRadians() + + "\n El " + oldRot.getRotationElevationRadians() + "\n Tl " + oldRot.getRotationTiltRadians() + + "\n New Az " + newRot.getRotationAzimuthRadians() + "\n New El " + newRot.getRotationElevationRadians() + + "\n New Tl " + newRot.getRotationTiltRadians()); assertArrayEquals(rotation[0], rotationMatrix[0], 0.001); assertArrayEquals(rotation[1], rotationMatrix[1], 0.001); assertArrayEquals(rotation[2], rotationMatrix[2], 0.001); @@ -375,18 +375,18 @@ public void compareTilt() throws FileNotFoundException { 0.001); // Check Euler angles assertArrayEquals( - new double[] { oldRot.getRotationAzimuth(), oldRot.getRotationElevation(), - oldRot.getRotationTilt() }, - new double[] { newRot.getRotationAzimuth(), newRot.getRotationElevation(), - newRot.getRotationTilt() }, + new double[] { oldRot.getRotationAzimuthRadians(), oldRot.getRotationElevationRadians(), + oldRot.getRotationTiltRadians() }, + new double[] { newRot.getRotationAzimuthRadians(), newRot.getRotationElevationRadians(), + newRot.getRotationTiltRadians() }, 0.001); // Check the old rotation against the known value assertArrayEquals(new double[] { 0, 0, Math.toRadians(rotationAngleDegrees) }, new double[] { - oldRot.getRotationAzimuth(), oldRot.getRotationElevation(), oldRot.getRotationTilt() }, + oldRot.getRotationAzimuthRadians(), oldRot.getRotationElevationRadians(), oldRot.getRotationTiltRadians() }, 0.001); // Check the new rotation against the known value assertArrayEquals(new double[] { 0, 0, Math.toRadians(rotationAngleDegrees) }, new double[] { - newRot.getRotationAzimuth(), newRot.getRotationElevation(), newRot.getRotationTilt() }, + newRot.getRotationAzimuthRadians(), newRot.getRotationElevationRadians(), newRot.getRotationTiltRadians() }, 0.001); } // frame(); @@ -404,16 +404,16 @@ public void checkEulerSingularities() { 0.7071067811865476); RotationNR tester3 = new RotationNR(0.7064894449532356, 1.0769850738285257E-7, 0.7077235789272859, 1.0769850738285257E-7); - assertArrayEquals(new double[] { 0, 90, 0 }, new double[] { Math.toDegrees(tester1.getRotationAzimuth()), - Math.toDegrees(tester1.getRotationElevation()), Math.toDegrees(tester1.getRotationTilt()) }, + assertArrayEquals(new double[] { 0, 90, 0 }, new double[] { Math.toDegrees(tester1.getRotationAzimuthRadians()), + Math.toDegrees(tester1.getRotationElevationRadians()), Math.toDegrees(tester1.getRotationTiltRadians()) }, 0.001); - assertArrayEquals(new double[] { 0, 90, 180 }, new double[] { Math.toDegrees(tester2.getRotationAzimuth()), - Math.toDegrees(tester2.getRotationElevation()), Math.toDegrees(tester2.getRotationTilt()) }, + assertArrayEquals(new double[] { 0, 90, 180 }, new double[] { Math.toDegrees(tester2.getRotationAzimuthRadians()), + Math.toDegrees(tester2.getRotationElevationRadians()), Math.toDegrees(tester2.getRotationTiltRadians()) }, 0.001); - assertArrayEquals(new double[] { 179.99, 89.9, 179.99 }, new double[] { Math.toDegrees(tester3.getRotationAzimuth()), - Math.toDegrees(tester3.getRotationElevation()), Math.toDegrees(tester3.getRotationTilt()) }, + assertArrayEquals(new double[] { 179.99, 89.9, 179.99 }, new double[] { Math.toDegrees(tester3.getRotationAzimuthRadians()), + Math.toDegrees(tester3.getRotationElevationRadians()), Math.toDegrees(tester3.getRotationTiltRadians()) }, 0.001); } From 9d9e5f795d335afab8f149976946b4aee857ecec Mon Sep 17 00:00:00 2001 From: rondlh <77279634+rondlh@users.noreply.github.com> Date: Sun, 15 Mar 2026 21:54:15 +0800 Subject: [PATCH 3/3] Formatting update Formatting update --- .../addons/kinematics/math/RotationNR.java | 107 +++++++++--------- 1 file changed, 55 insertions(+), 52 deletions(-) diff --git a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/RotationNR.java b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/RotationNR.java index 84a35b2d..b3c54601 100644 --- a/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/RotationNR.java +++ b/src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/RotationNR.java @@ -10,11 +10,11 @@ import com.google.gson.annotations.Expose; import com.neuronrobotics.sdk.common.Log; -// Auto-generated Javadoc +// Auto-generated Javadoc /** * This class is to represent a 3x3 rotation sub-matrix This class also contains * static methods for dealing with 3x3 rotations. - * + * * @author Kevin Harrington * */ @@ -37,7 +37,6 @@ public class RotationNR { private static RotationOrder order = RotationOrder.ZYX; @Expose (serialize = false, deserialize = false) private static RotationConvention convention = RotationConvention.VECTOR_OPERATOR; - /** * Null constructor forms a. @@ -48,10 +47,10 @@ public RotationNR() { /** * Instatiate using the * org.apache.commons.math3.geometry.euclidean.threed.Rotation . - * + * * @param store - * A org.apache.commons.math3.geometry.euclidean.threed.Rotation - * instance + * A org.apache.commons.math3.geometry.euclidean.threed.Rotation + * instance */ public RotationNR(Rotation store) { setStorage(store); @@ -61,20 +60,24 @@ public RotationNR(Rotation store) { * Instantiates a new rotation nr. * ** @param tilt the tilt in Degrees - * @param azimuth the azimuth in Degrees + * @param azimuth the azimuth in Degrees * @param elevation the elevation in Degrees */ // create a new object with the given simplified rotations public RotationNR(double tilt, double azimuth, double elevation) { + if (!Double.isFinite(tilt)) - throw new RuntimeException("Value can not be "+tilt); + throw new RuntimeException("Value can not be " + tilt); + if (!Double.isFinite(azimuth)) - throw new RuntimeException("Value can not be "+azimuth); + throw new RuntimeException("Value can not be " + azimuth); + if (!Double.isFinite(elevation)) - throw new RuntimeException("Value can not be "+elevation); - if (elevation > 90 || elevation < -90) { + throw new RuntimeException("Value can not be " + elevation); + + if ((elevation > 90) || (elevation < -90)) throw new RuntimeException("Elevation can not be greater than 90 nor less than -90"); - } + loadFromAngles(tilt, azimuth, elevation); if (Double.isNaN(getRotationMatrix2QuaturnionW()) || Double.isNaN(getRotationMatrix2QuaturnionX()) || Double.isNaN(getRotationMatrix2QuaturnionY()) || Double.isNaN(getRotationMatrix2QuaturnionZ())) { @@ -86,7 +89,7 @@ public RotationNR(double tilt, double azimuth, double elevation) { } public RotationNR(EulerAxis axis, double rot) { - this(axis==EulerAxis.tilt?rot:0,axis==EulerAxis.azimuth?rot:0,axis==EulerAxis.elevation?rot:0); + this((axis == EulerAxis.tilt) ? rot : 0, (axis == EulerAxis.azimuth) ? rot : 0, (axis == EulerAxis.elevation) ? rot : 0); } /** @@ -115,7 +118,7 @@ public RotationNR(double[] values) { */ public static RotationNR getRotationX(double rotationAngleDegrees) { double[][] rotation = new double[3][3]; - double rotationAngleRadians = Math.PI / 180 * rotationAngleDegrees; + double rotationAngleRadians = rotationAngleDegrees * Math.PI / 180; // Rotation matrix, 1st column rotation[0][0] = 1; @@ -141,7 +144,7 @@ public static RotationNR getRotationX(double rotationAngleDegrees) { */ public static RotationNR getRotationY(double rotationAngleDegrees) { double[][] rotation = new double[3][3]; - double rotationAngleRadians = Math.PI / 180 * rotationAngleDegrees; + double rotationAngleRadians = rotationAngleDegrees * Math.PI / 180; // Rotation matrix, 1st column rotation[0][0] = Math.cos(rotationAngleRadians); @@ -167,7 +170,7 @@ public static RotationNR getRotationY(double rotationAngleDegrees) { */ public static RotationNR getRotationZ(double rotationAngleDegrees) { double[][] rotation = new double[3][3]; - double rotationAngleRadians = Math.PI / 180 * rotationAngleDegrees; + double rotationAngleRadians = rotationAngleDegrees * Math.PI / 180; // Rotation matrix, 1st column rotation[0][0] = Math.cos(rotationAngleRadians); @@ -205,11 +208,10 @@ public RotationNR(double w, double x, double y, double z) { */ public RotationNR(Matrix m) { double[][] rotation = new double[3][3]; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) rotation[i][j] = m.get(i, j); - } - } + loadRotations(rotation); } @@ -221,11 +223,11 @@ public RotationNR(Matrix m) { private void loadRotations(double[][] rotM) { if (rotM.length != 3) throw new RuntimeException("Must be 3x3 rotation matrix"); - for (int i = 0; i < 3; i++) { - if (rotM[i].length != 3) { + + for (int i = 0; i < 3; i++) + if (rotM[i].length != 3) throw new RuntimeException("Must be 3x3 rotation matrix"); - } - } + setStorage(new Rotation(rotM, 0.00001)); } @@ -241,7 +243,7 @@ public double[][] getRotationMatrix() { /* * (non-Javadoc) - * + * * @see java.lang.Object#toString() */ // return a string representation of the invoking object @@ -251,10 +253,9 @@ public String toString() { + ", " + "y=" + getRotationMatrix2QuaturnionY() + ", " + "z=" + getRotationMatrix2QuaturnionZ() + "\n" + "Rotation angle (degrees): " + "az= " + Math.toDegrees(getRotationAzimuthRadians()) + ", elev= " + Math.toDegrees(getRotationElevation()) + ", tilt=" + Math.toDegrees(getRotationTilt()); - }catch(Exception ex){ + } catch(Exception ex){ return "Rotation error" + ex.getLocalizedMessage(); } - } /** @@ -286,18 +287,23 @@ public String toString(double[][] array) { * @param z the z */ protected void quaternion2RotationMatrix(double w, double x, double y, double z) { + if (Double.isNaN(w)) throw new RuntimeException("Value can not be NaN"); + if (Double.isNaN(x)) throw new RuntimeException("Value can not be NaN"); + if (Double.isNaN(y)) throw new RuntimeException("Value can not be NaN"); + if (Double.isNaN(z)) throw new RuntimeException("Value can not be NaN"); - this.w=w; - this.x= -x; - this.y= -y; - this.z= -z; + + this.w = w; + this.x = -x; + this.y = -y; + this.z = -z; } /** @@ -309,7 +315,7 @@ protected void quaternion2RotationMatrix(double w, double x, double y, double z) * @return true, if successful */ public static boolean bound(double low, double high, double n) { - return n >= low && n <= high; + return ((n >= low) && (n <= high)); } private void loadFromAngles(double tilt, double azimuth, double elevation) { @@ -340,7 +346,6 @@ public double getRotationElevationRadians() { * * @return the rotation azimuth in radians */ - public double getRotationAzimuthRadians() { return getAngle(0); } @@ -361,7 +366,6 @@ public double getRotationTiltDegrees() { */ public double getRotationElevationDegrees() { return Math.toDegrees(getRotationElevationRadians()); - } /** @@ -369,7 +373,7 @@ public double getRotationElevationDegrees() { * * @return the rotation azimuth in degrees */ - + public double getRotationAzimuthDegrees() { return Math.toDegrees( getRotationAzimuthRadians()); } @@ -402,40 +406,39 @@ public double getRotationElevation() { */ @Deprecated public double getRotationAzimuth() { - return getRotationAzimuthRadians(); + return getRotationAzimuthRadians(); } private void simpilfyAngles(double [] angles){ - double epsilon=1.0E-7; - if(Math.abs(angles[0] - Math.toRadians(180)) < epsilon&& - Math.abs(angles[2] - Math.toRadians(180)) < epsilon ){ - if(!(Math.abs(getRotationMatrix2QuaturnionZ())>epsilon)) - angles[0]=0; - if(!(Math.abs(getRotationMatrix2QuaturnionX())>epsilon)) - angles[2]=0; + double epsilon = 1.0E-7; + if ((Math.abs(angles[0] - Math.toRadians(180)) < epsilon) && + Math.abs(angles[2] - Math.toRadians(180)) < epsilon)) { + if (!(Math.abs(getRotationMatrix2QuaturnionZ()) > epsilon)) + angles[0] = 0; + + if (!(Math.abs(getRotationMatrix2QuaturnionX()) > epsilon)) + angles[2] = 0; } } private double eulerFix(double offsetSize, int index){ - double offset = (index==1?offsetSize:0); + double offset = ((index == 1) ? offsetSize : 0); TransformNR current = new TransformNR(0, 0, 0, this); - TransformNR newTf = current.times(new TransformNR(0, 0, 0, new RotationNR(0,0,Math.toDegrees(offsetSize)))); + TransformNR newTf = current.times(new TransformNR(0, 0, 0, new RotationNR(0, 0, Math.toDegrees(offsetSize)))); double[] angles = newTf.getRotation().getStorage().getAngles(getOrder(), getConvention()); simpilfyAngles(angles); - double finalResult= angles[index]; - return finalResult+offset; + double finalResult = angles[index]; + return finalResult + offset; } - private double getAngle(int index){ - + private double getAngle(int index) { try { return getStorage().getAngles(getOrder(), getConvention())[index]; } catch (CardanEulerSingularityException e) { try { - return eulerFix( Math.toRadians(0.001), index); + return eulerFix(Math.toRadians(0.001), index); } catch (CardanEulerSingularityException ex) { - return eulerFix( Math.toRadians(-0.001), index); - + return eulerFix(Math.toRadians(-0.001), index); } } }