Skip to content

Fix orent --> orient typo #40

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
madhephaestus merged 3 commits into from
Mar 15, 2026
Merged

Fix orent --> orient typo #40

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
39b6915
Fix orent --> orient typo
rondlh Mar 15, 2026
cf921fd
Solve some warnings, some formatting fixes
rondlh Mar 15, 2026
9d9e5f7
Formatting update
rondlh Mar 15, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
60 changes: 30 additions & 30 deletions src/main/java/com/neuronrobotics/sdk/addons/irobot/CreateArm.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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];
Expand Down Expand Up @@ -318,37 +318,37 @@ 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);
}

/**
* Sets the cartesian pose.
*
* @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;
Expand All @@ -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);


Expand All @@ -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;
Expand All @@ -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) || (orentation<pose[2]-orentThreshHold)){
Log.info("Orentation changed");
if((orientation>pose[2]+orientThreshHold) || (orientation<pose[2]-orientThreshHold)){
Log.info("Orientation changed");
return true;
}
Log.info("No signifigant change");
Expand All @@ -415,11 +415,11 @@ public void setCartesianY(double y){
}

/**
* Sets the cartesian orentation.
* Sets the cartesian orientation.
*
* @param o the new cartesian orentation
* @param o the new cartesian orientation
*/
public void setCartesianOrentation(double o){
public void setCartesianOrientation(double o){
setCartesianPose(pose[0], pose[1], o);
}

Expand Down Expand Up @@ -488,11 +488,11 @@ private double ToRadians(double degrees){
}

/**
* Gets the orentation.
* Gets the orientation.
*
* @return the current approach orientation of the wrist
*/
public double GetOrentation() {
public double GetOrientation() {
double [] angles =getAngles();
return angles[0]+angles[1]+angles[2];
}
Expand Down
86 changes: 45 additions & 41 deletions src/main/java/com/neuronrobotics/sdk/addons/kinematics/AbstractKinematicsNR.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<getNumberOfLinks();i++) {
if(getAbstractLink(i)==l)
for (int i = 0; i < getNumberOfLinks(); i++) {
if (getAbstractLink(i) == l)
return i;
}
return -1;
}
public int getLinkIndex(LinkConfiguration l) {
for (int i=0;i<getNumberOfLinks();i++) {
if(getAbstractLink(i).getLinkConfiguration()==l)
for (int i = 0; i < getNumberOfLinks(); i++) {
if (getAbstractLink(i).getLinkConfiguration() == l)
return i;
}
return -1;
Expand Down Expand Up @@ -223,8 +223,9 @@ public AbstractKinematicsNR() {
*/
public AbstractKinematicsNR(InputStream configFile, LinkFactory f) {
this();
if(configFile==null||f==null)
if ((configFile == null) || (f == null))
return;

Document doc = XmlFactory.getAllNodesDocument(configFile);
NodeList nodListofLinks = doc.getElementsByTagName("appendage");
for (int i = 0; i < 1; i++) {
Expand Down Expand Up @@ -546,9 +547,9 @@ public double[] getCurrentJointSpaceVector() {

public double[] getCurrentJointSpaceTarget() {

double[] currentJointSpaceTarget=new double[getNumberOfLinks()];
for(int i=0;i<currentJointSpaceTarget.length;i++) {
currentJointSpaceTarget[i]=readLinkTarget(i);
double[] currentJointSpaceTarget = new double[getNumberOfLinks()];
for (int i = 0; i < currentJointSpaceTarget.length; i++) {
currentJointSpaceTarget[i] = readLinkTarget(i);
}
return currentJointSpaceTarget;
}
Expand All @@ -574,15 +575,15 @@ public double[] setDesiredTaskSpaceTransform(TransformNR taskSpaceTransform, dou
TickToc.tic("inverseOffset");
double[] jointSpaceVect = inverseKinematics(inverseOffset);
TickToc.tic("inverseKinematics");
if(checkVector(this,jointSpaceVect,seconds)) {
if (checkVector(this, jointSpaceVect, seconds)) {
TickToc.tic("checkVector success");
if (jointSpaceVect == null)
throw new RuntimeException("The kinematics model must return an array, not null");

_setDesiredJointSpaceVector(jointSpaceVect, seconds,false);
_setDesiredJointSpaceVector(jointSpaceVect, seconds, false);
TickToc.tic("_setDesiredJointSpaceVector complete");
return jointSpaceVect;
}else
} else
TickToc.tic("checkVector fail");

double[] currentJointSpaceTarget2 = getCurrentJointSpaceTarget();
Expand Down Expand Up @@ -620,40 +621,41 @@ private static boolean checkVector(AbstractKinematicsNR dev, double[] jointSpace
for (int i = 0; i < jointSpaceVect.length; i++) {
AbstractLink link = dev.factory.getLink(dev.getLinkConfiguration(i));
double val = jointSpaceVect[i];
Double double1 = new Double(val);
if(double1.isNaN() ||double1.isInfinite() ) {
Log.error(dev.getScriptingName()+" Link "+i+" Invalid unput "+double1);

if (Double.isNaN(val) || Double.isInfinite(val)) {
Log.error(dev.getScriptingName() + " Link " + i + " Invalid input " + val);
return false;
}
if (val > 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;
}
}
}
return true;
}


/**
* Checks the desired pose for ability for the IK to calculate a valid pose.
*
* @param taskSpaceTransform the task space transform
* @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.
Expand All @@ -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);
}

/**
Expand All @@ -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);
Expand Down Expand Up @@ -730,8 +732,8 @@ private double[] _setDesiredJointSpaceVector(double[] jointSpaceVect, double se
+ " links, actual number of links = " + jointSpaceVect.length);
}
double best = getBestTime(jointSpaceVect);
if(seconds<best)
seconds=best;
if (seconds < best)
seconds = best;
//synchronized(AbstractKinematicsNR.class) {
int except = 0;
Exception e = null;
Expand Down Expand Up @@ -762,7 +764,7 @@ private double[] _setDesiredJointSpaceVector(double[] jointSpaceVect, double se
TickToc.tic("FK from vector");
fireTargetJointsUpdate(getCurrentJointSpaceTarget(), fwd);
TickToc.tic("Joint space updates");
if(fireTaskUpdate) {
if (fireTaskUpdate) {
setCurrentPoseTarget(forwardOffset(fwd));
TickToc.tic("task space updates");
}
Expand Down Expand Up @@ -952,7 +954,7 @@ public void setGlobalToFiducialTransform(TransformNR frameToBase, boolean fireUp
return;
}
this.fiducial2RAS = frameToBase;
if(!fireUpdate)
if (!fireUpdate)
return;
for (int i = 0; i < regListeners.size(); i++) {
IRegistrationListenerNR r = regListeners.get(i);
Expand Down Expand Up @@ -1009,8 +1011,9 @@ public TransformNR forwardOffset(TransformNR t) {
* @param l the l
*/
public void addJointSpaceListener(IJointSpaceUpdateListenerNR l) {
if (jointSpaceUpdateListeners.contains(l) || l == null)
if (jointSpaceUpdateListeners.contains(l) || (l == null))
return;

jointSpaceUpdateListeners.add(l);
}

Expand All @@ -1030,8 +1033,9 @@ public void removeJointSpaceUpdateListener(IJointSpaceUpdateListenerNR l) {
* @param l the l
*/
public void addRegistrationListener(IRegistrationListenerNR l) {
if (regListeners.contains(l) || l == null)
if (regListeners.contains(l) || (l == null))
return;

regListeners.add(l);
l.onBaseToFiducialUpdate(this, getRobotToFiducialTransform());
}
Expand All @@ -1052,7 +1056,7 @@ public void removeRegestrationUpdateListener(IRegistrationListenerNR l) {
* @param l the l
*/
public void addPoseUpdateListener(ITaskSpaceUpdateListenerNR l) {
if (taskSpaceUpdateListeners.contains(l) || l == null) {
if (taskSpaceUpdateListeners.contains(l) || (l == null)) {
return;
}
// new RuntimeException("adding "+l.getClass().getName()).printStackTrace();
Expand Down Expand Up @@ -1083,16 +1087,16 @@ public void onLinkPositionUpdate(AbstractLink source, double engineeringUnitsVal
for (LinkConfiguration c : getLinkConfigurations()) {
AbstractLink tmp = getFactory().getLink(c);
if (tmp == source) {// Check to see if this lines up with a known link
// // Log.info("Got PID event "+source+" value="+engineeringUnitsValue);
// if(new Double(engineeringUnitsValue).isNaN()) {
// new RuntimeException("Link values can not ne NaN").printStackTrace();
// engineeringUnitsValue=0;
// // Log.info("Got PID event " + source + " value=" + engineeringUnitsValue);
// if (new Double(engineeringUnitsValue).isNaN()) {
// new RuntimeException("Link values can not be NaN").printStackTrace();
// engineeringUnitsValue = 0;
// }
// ArrayList<LinkConfiguration> linkConfigurations = getLinkConfigurations();
// if(linkConfigurations!=null) {
// if (linkConfigurations!=null) {
// int indexOf = linkConfigurations.indexOf(c);
// if(currentJointSpacePositions!=null)
// if(indexOf>=0 && indexOf<currentJointSpacePositions.length)
// if (currentJointSpacePositions != null)
// if ((indexOf >= 0) && (indexOf < currentJointSpacePositions.length))
// currentJointSpacePositions[indexOf] = engineeringUnitsValue;
// }
firePoseUpdate();
Expand Down Expand Up @@ -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);
}
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -1719,7 +1723,7 @@ public InterpolationMoveState blockingInterpolatedMove(TransformNR target, doubl
}
wait((int) msPerStep);
}
}else {
} else {
return InterpolationMoveState.FAULT;
}
return InterpolationMoveState.READY;
Expand All @@ -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<getNumberOfLinks();i++) {
for (int i = 0; i < getNumberOfLinks(); i++) {
AbstractLink l = getAbstractLink(i);
l.setTimeProvider(getTimeProvider());
}
Expand Down
Loading
Loading