com.neuronrobotics.sdk.addons.kinematics.AbstractKinematicsNR Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of java-bowler Show documentation
Show all versions of java-bowler Show documentation
A command line utility for accesing the bowler framework.
The newest version!
package com.neuronrobotics.sdk.addons.kinematics;
import java.io.InputStream;
import java.sql.Timestamp;
import java.util.ArrayList;
//import java.util.concurrent.CountDownLatch;
import javafx.application.Platform;
//import javafx.embed.swing.JFXPanel;
import javafx.scene.transform.Affine;
import javafx.stage.Stage;
import javax.management.RuntimeErrorException;
import org.w3c.dom.Document;
import org.w3c.dom.Element;
import org.w3c.dom.Node;
import org.w3c.dom.NodeList;
import Jama.Matrix;
import com.neuronrobotics.sdk.addons.kinematics.imu.IMU;
import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR;
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.addons.kinematics.xml.XmlFactory;
import com.neuronrobotics.sdk.common.BowlerAbstractDevice;
import com.neuronrobotics.sdk.common.BowlerDatagram;
import com.neuronrobotics.sdk.common.InvalidConnectionException;
//import com.neuronrobotics.sdk.addons.kinematics.PidRotoryLink;
import com.neuronrobotics.sdk.common.Log;
import com.neuronrobotics.sdk.common.NonBowlerDevice;
import com.neuronrobotics.sdk.namespace.bcs.pid.IPidControlNamespace;
import com.neuronrobotics.sdk.pid.IPIDEventListener;
import com.neuronrobotics.sdk.pid.PIDChannel;
//import com.neuronrobotics.sdk.pid.PIDCommandException;
import com.neuronrobotics.sdk.pid.PIDConfiguration;
import com.neuronrobotics.sdk.pid.PIDEvent;
import com.neuronrobotics.sdk.pid.PIDLimitEvent;
import com.neuronrobotics.sdk.util.ThreadUtil;
// TODO: Auto-generated Javadoc
//import javax.swing.JFrame;
//import javax.swing.JOptionPane;
/**
* The Class AbstractKinematicsNR.
*/
@SuppressWarnings("restriction")
public abstract class AbstractKinematicsNR extends NonBowlerDevice implements IPIDEventListener, ILinkListener {
/** The configurations. */
private ArrayList pidConfigurations = new ArrayList();
/** The task space update listeners. */
private ArrayList taskSpaceUpdateListeners = new ArrayList();
/** The joint space update listeners. */
protected ArrayList jointSpaceUpdateListeners = new ArrayList();
/** The reg listeners. */
private ArrayList regListeners = new ArrayList();
/** The mobile bases. */
private ArrayList mobileBases = new ArrayList();
/** The dh engine. */
private String[] dhEngine = new String[] { "https://github.com/madhephaestus/carl-the-hexapod.git",
"DefaultDhSolver.groovy" };
/** The cad engine. */
private String[] cadEngine = new String[] { "https://github.com/madhephaestus/carl-the-hexapod.git",
"ThreeDPrintCad.groovy" };
/** The current joint space positions. */
/* This is in RAW joint level ticks */
protected double[] currentJointSpacePositions = null;
/** The current joint space target. */
protected double[] currentJointSpaceTarget;
/** The current pose target. */
private TransformNR currentPoseTarget = new TransformNR();
/** The base2 fiducial. */
private TransformNR base2Fiducial = new TransformNR();
/** The fiducial2 ras. */
private TransformNR fiducial2RAS = new TransformNR();
/** The no flush. */
private boolean noFlush = false;
/** The no xml config. */
private boolean noXmlConfig = true;
/** The dh parameters chain. */
private DHChain dhParametersChain = null;
/** The root. */
private Affine root;
/* The device */
/** The factory. */
// private IPIDControl device =null;
private LinkFactory factory = null;
/** The retry number before fail. */
private int retryNumberBeforeFail = 5;
/**
* The object for communicating IMU information and registering it with the
* hardware
*/
private IMU imu = new IMU();
static {
JavaFXInitializer.go();
}
/**
* Gets the root listener.
*
* @return the root listener
*/
public Affine getRootListener() {
if (root == null)
root = new Affine();
return root;
}
/**
* Sets the root listener.
*
* @param listener the new root listener
*/
void setRootListener(Affine listener) {
this.root = listener;
}
/**
* This method tells the connection object to disconnect its pipes and close out
* the connection. Once this is called, it is safe to remove your device.
*/
public abstract void disconnectDevice();
/**
* Connect device.
*
* @return true, if successful
*/
public abstract boolean connectDevice();
/*
* (non-Javadoc)
*
* @see com.neuronrobotics.sdk.common.NonBowlerDevice#getNamespacesImp()
*/
@Override
public ArrayList getNamespacesImp() {
// TODO Auto-generated method stub
ArrayList back = new ArrayList();
back.add("bcs.cartesian.*");
return back;
}
/*
* (non-Javadoc)
*
* @see com.neuronrobotics.sdk.common.NonBowlerDevice#disconnectDeviceImp()
*/
public void disconnectDeviceImp() {
getFactory().removeLinkListener(this);
for (LinkConfiguration lf : getFactory().getLinkConfigurations())
if (getFactory().getPid(lf) != null)
getFactory().getPid(lf).removePIDEventListener(this);
disconnectDevice();
}
/*
* (non-Javadoc)
*
* @see com.neuronrobotics.sdk.common.NonBowlerDevice#connectDeviceImp()
*/
public boolean connectDeviceImp() {
return connectDevice();
}
/**
* Instantiates a new abstract kinematics nr.
*/
public AbstractKinematicsNR() {
// File l = new File("RobotLog_"+getDate()+"_"+System.currentTimeMillis()+".txt");
// //File e = new File("RobotError_"+getDate()+"_"+System.currentTimeMillis()+".txt");
// try {
// PrintStream p =new PrintStream(l);
// Log.setOutStream(new PrintStream(p));
// Log.setErrStream(new PrintStream(p));
// } catch (FileNotFoundException e1) {
// e1.printStackTrace();
// }
setDhParametersChain(new DHChain(this));
}
/**
* Instantiates a new abstract kinematics nr.
*
* @param configFile the config file
* @param f the f
*/
public AbstractKinematicsNR(InputStream configFile, LinkFactory f) {
this();
Document doc = XmlFactory.getAllNodesDocument(configFile);
NodeList nodListofLinks = doc.getElementsByTagName("appendage");
for (int i = 0; i < 1; i++) {
Node linkNode = nodListofLinks.item(i);
if (linkNode.getNodeType() == Node.ELEMENT_NODE) {
noXmlConfig = false;
if (configFile != null && f != null) {
setDevice(f, loadConfig((Element) linkNode));
}
} else {
Log.info("Not Element Node");
}
}
}
/**
* Instantiates a new abstract kinematics nr.
*
* @param doc the doc
* @param f the f
*/
public AbstractKinematicsNR(Element doc, LinkFactory f) {
this();
noXmlConfig = false;
if (doc != null && f != null) {
setDevice(f, loadConfig(doc));
}
}
/**
* Gets the date.
*
* @return the date
*/
private String getDate() {
Timestamp t = new Timestamp(System.currentTimeMillis());
return t.toString().split("\\ ")[0];
}
/**
* Load XML configuration file, then store in LinkConfiguration (ArrayList
* type).
*
* @param doc the doc
* @return the array list
*/
protected ArrayList loadConfig(Element doc) {
ArrayList localConfigsFromXml = new ArrayList();
NodeList nodListofLinks = doc.getChildNodes();
setGitCadEngine(getGitCodes(doc, "cadEngine"));
setGitDhEngine(getGitCodes(doc, "kinematics"));
for (int i = 0; i < nodListofLinks.getLength(); i++) {
Node linkNode = nodListofLinks.item(i);
if (linkNode.getNodeType() == Node.ELEMENT_NODE && linkNode.getNodeName().contentEquals("link")) {
LinkConfiguration newLinkConf = new LinkConfiguration((Element) linkNode);
localConfigsFromXml.add(newLinkConf);
NodeList dHParameters = linkNode.getChildNodes();
// System.out.println("Link "+newLinkConf.getName()+" has "+dHParameters
// .getLength()+" children");
for (int x = 0; x < dHParameters.getLength(); x++) {
Node nNode = dHParameters.item(x);
if (nNode.getNodeType() == Node.ELEMENT_NODE && nNode.getNodeName().contentEquals("DHParameters")) {
Element dhNode = (Element) nNode;
DHLink newLink = new DHLink(dhNode);
getDhParametersChain().addLink(newLink);// 0->1
NodeList mobileBasesNodeList = dhNode.getChildNodes();
for (int j = 0; j < mobileBasesNodeList.getLength(); j++) {
Node mb = mobileBasesNodeList.item(j);
if (mb.getNodeType() == Node.ELEMENT_NODE && mb.getNodeName().contentEquals("mobilebase")) {
final MobileBase newMobileBase = new MobileBase((Element) mb);
mobileBases.add(newMobileBase);
newLink.setMobileBaseXml(newMobileBase);
newLink.addDhLinkPositionListener(new IDhLinkPositionListener() {
@Override
public void onLinkGlobalPositionChange(TransformNR newPose) {
Log.debug("Motion in the D-H link has caused this mobile base to move");
newMobileBase.setGlobalToFiducialTransform(newPose);
}
});
}
}
} else {
if (nNode.getNodeType() == Node.ELEMENT_NODE
&& nNode.getNodeName().contentEquals("slaveLink")) {
// System.out.println("Slave link found: ");
LinkConfiguration jc = new LinkConfiguration((Element) nNode);
// System.out.println(jc);
newLinkConf.getSlaveLinks().add(jc);
}
}
}
} else if (linkNode.getNodeType() == Node.ELEMENT_NODE && linkNode.getNodeName().contentEquals("name")) {
try {
setScriptingName(XmlFactory.getTagValue("name", doc));
} catch (Exception E) {
E.printStackTrace();
}
} else if (linkNode.getNodeType() == Node.ELEMENT_NODE
&& linkNode.getNodeName().contentEquals("ZframeToRAS")) {
Element eElement = (Element) linkNode;
try {
setZframeToGlobalTransform(new TransformNR(
Double.parseDouble(XmlFactory.getTagValue("x", eElement)),
Double.parseDouble(XmlFactory.getTagValue("y", eElement)),
Double.parseDouble(XmlFactory.getTagValue("z", eElement)),
new RotationNR(new double[] { Double.parseDouble(XmlFactory.getTagValue("rotw", eElement)),
Double.parseDouble(XmlFactory.getTagValue("rotx", eElement)),
Double.parseDouble(XmlFactory.getTagValue("roty", eElement)),
Double.parseDouble(XmlFactory.getTagValue("rotz", eElement)) })));
} catch (Exception ex) {
ex.printStackTrace();
setZframeToGlobalTransform(new TransformNR());
}
} else if (linkNode.getNodeType() == Node.ELEMENT_NODE
&& linkNode.getNodeName().contentEquals("baseToZframe")) {
Element eElement = (Element) linkNode;
try {
setBaseToZframeTransform(new TransformNR(Double.parseDouble(XmlFactory.getTagValue("x", eElement)),
Double.parseDouble(XmlFactory.getTagValue("y", eElement)),
Double.parseDouble(XmlFactory.getTagValue("z", eElement)),
new RotationNR(new double[] { Double.parseDouble(XmlFactory.getTagValue("rotw", eElement)),
Double.parseDouble(XmlFactory.getTagValue("rotx", eElement)),
Double.parseDouble(XmlFactory.getTagValue("roty", eElement)),
Double.parseDouble(XmlFactory.getTagValue("rotz", eElement)) })));
} catch (Exception ex) {
ex.printStackTrace();
setBaseToZframeTransform(new TransformNR());
}
} else {
// System.err.println(linkNode.getNodeName());
// Log.error("Node not known: "+linkNode.getNodeName());
}
}
return localConfigsFromXml;
}
/**
* Gets the xml.
*
* @return the xml
*/
/*
*
* Generate the xml configuration to generate an XML of this robot.
*/
public String getXml() {
String xml = "\n";
xml += "\n";
xml += "\n" + getScriptingName() + " \n";
for (int i = 0; i < getLinkConfigurations().size(); i++) {
xml += "\n";
xml += getLinkConfiguration(i).getXml();
xml += "\n\n";
}
xml += "\n\n";
xml += getFiducialToGlobalTransform().getXml();
xml += "\n \n";
xml += "\n\n";
xml += getRobotToFiducialTransform().getXml();
xml += "\n \n";
xml += "\n ";
xml += "\n ";
return xml;
}
/**
* Gets the link configuration.
*
* @param linkIndex the link index
* @return the link configuration
*/
public LinkConfiguration getLinkConfiguration(int linkIndex) {
return getLinkConfigurations().get(linkIndex);
}
/**
* Gets the link configurations.
*
* @return the link configurations
*/
public ArrayList getLinkConfigurations() {
return getFactory().getLinkConfigurations();
}
/**
* Gets the link current configuration.
*
* @param chan the chan
* @return the link current configuration
*/
public PIDConfiguration getLinkCurrentConfiguration(int chan) {
return getAxisPidConfiguration().get(chan);
}
/**
* Sets the link current configuration.
*
* @param chan the chan
* @param c the c
*/
public void setLinkCurrentConfiguration(int chan, PIDConfiguration c) {
getAxisPidConfiguration().set(chan, c);
}
/**
* Gets the device.
*
* @return the device
*/
protected LinkFactory getDevice() {
return getFactory();
}
/**
* Gets the abstract link.
*
* @param index the index
* @return the abstract link
*/
public AbstractLink getAbstractLink(int index) {
return getFactory().getLink(getLinkConfiguration(index));
}
/**
* Sets the device.
*
* @param f the f
* @param linkConfigs the link configs
*/
protected void setDevice(LinkFactory f, ArrayList linkConfigs) {
Log.info("Loading device: " + f.getClass() + " " + f);
setFactory(f);
// Log.enableDebugPrint(true);
for (int i = 0; i < linkConfigs.size(); i++) {
LinkConfiguration c = linkConfigs.get(i);
c.setLinkIndex(i);
getFactory().getLink(c);
Log.info("\nAxis #" + i + " Configuration:\n" + c);
if (c.getTypeEnum() == LinkType.PID) {
IPidControlNamespace device = getFactory().getPid(c);
try {
PIDConfiguration tmpConf = device.getPIDConfiguration(c.getHardwareIndex());
tmpConf.setGroup(c.getHardwareIndex());
tmpConf.setKP(c.getKP());
tmpConf.setKI(c.getKI());
tmpConf.setKD(c.getKD());
tmpConf.setEnabled(true);
tmpConf.setInverted(c.isInverted());
tmpConf.setAsync(false);
tmpConf.setUseLatch(false);
tmpConf.setIndexLatch(c.getIndexLatch());
tmpConf.setStopOnIndex(false);
Log.info("\nAxis #" + i + " " + tmpConf);
getAxisPidConfiguration().add(tmpConf);
// setLinkCurrentConfiguration(i,tmpConf);
// Send configuration for ONE axis
device.ConfigurePIDController(tmpConf);
} catch (Exception ex) {
Log.error("Configuration #" + i + " failed!!");
ex.printStackTrace();
}
device.addPIDEventListener(this);
}
}
getCurrentTaskSpaceTransform();
getFactory().addLinkListener(this);
getDhParametersChain().setFactory(getFactory());
// filling up the d-h parameters so the chain sizes match
while (getDhParametersChain().getLinks().size() < linkConfigs.size()) {
getDhParametersChain().addLink(new DHLink(0, 0, 0, 0));
}
}
/**
* Gets the number of links defined in the configuration file.
*
* @return number of links in XML
*/
public int getNumberOfLinks() {
return getLinkConfigurations().size();
}
/**
* This takes a reading of the robots position and converts it to a joint space
* vector This vector is converted to task space and returned .
*
* @return taskSpaceVector in mm,radians [x,y,z,rotx,rotY,rotZ]
*/
public TransformNR getCurrentTaskSpaceTransform() {
TransformNR fwd = forwardKinematics(getCurrentJointSpaceVector());
if (fwd == null)
throw new RuntimeException("Implementations of the kinematics need to return a transform not null");
// Log.info("Getting robot task space "+fwd);
TransformNR taskSpaceTransform = forwardOffset(fwd);
// Log.info("Getting global task space "+taskSpaceTransform);
return taskSpaceTransform;
}
/**
* This takes a reading of the robots position and converts it to a joint pace
* vector This vector is converted to Joint space and returned .
*
* @return JointSpaceVector in mm,radians
*/
public double[] getCurrentJointSpaceVector() {
if (currentJointSpacePositions == null||currentJointSpacePositions.length!= getNumberOfLinks()) {
// Happens once and only once on the first initialization
currentJointSpacePositions = new double[getNumberOfLinks()];
currentJointSpaceTarget = new double[getNumberOfLinks()];
for (int i = 0; i < getNumberOfLinks(); i++) {
// double pos =
// currentLinkSpacePositions[getLinkConfigurations().get(i).getHardwareIndex()];
// Here the RAW values are converted to engineering units
try {
currentJointSpacePositions[i] = getFactory().getLink(getLinkConfiguration(i))
.getCurrentEngineeringUnits();
} catch (Exception ex) {
currentJointSpacePositions[i] = 0;
}
}
firePoseUpdate();
}
double[] jointSpaceVect = new double[getNumberOfLinks()];
for (int i = 0; i < getNumberOfLinks(); i++) {
// double pos =
// currentLinkSpacePositions[getLinkConfigurations().get(i).getHardwareIndex()];
// Here the RAW values are converted to engineering units
try {
jointSpaceVect[i] = currentJointSpacePositions[i];
}catch(Exception e) {
jointSpaceVect[i]=0;
}
}
return jointSpaceVect;
}
public double getCurrentLinkEngineeringUnits(int linkIndex) {
return getFactory().getLink(getLinkConfiguration(linkIndex)).getCurrentEngineeringUnits();
}
/**
* This calculates the target pose .
*
* @param taskSpaceTransform the task space transform
* @param seconds the time for the transition to take from current
* position to target, unit seconds
* @return The joint space vector is returned for target arrival referance
* @throws Exception If there is a workspace error
*/
public double[] setDesiredTaskSpaceTransform(TransformNR taskSpaceTransform, double seconds) throws Exception {
Log.info("Setting target pose: " + taskSpaceTransform);
setCurrentPoseTarget(taskSpaceTransform);
double[] jointSpaceVect = inverseKinematics(inverseOffset(taskSpaceTransform));
if (jointSpaceVect == null)
throw new RuntimeException("The kinematics model must return and array, not null");
setDesiredJointSpaceVector(jointSpaceVect, seconds);
return jointSpaceVect;
}
/**
* 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 static boolean checkTaskSpaceTransform(AbstractKinematicsNR dev, TransformNR taskSpaceTransform) {
try {
double[] jointSpaceVect = dev.inverseKinematics(dev.inverseOffset(taskSpaceTransform));
for (int i = 0; i < jointSpaceVect.length; i++) {
AbstractLink link = dev.factory.getLink(dev.getLinkConfiguration(i));
double val = link.toLinkUnits(jointSpaceVect[i]);
if (val > link.getUpperLimit()) {
return false;
}
if (val < link.getLowerLimit()) {
return false;
}
}
} catch (Exception ex) {
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) {
return AbstractKinematicsNR.checkTaskSpaceTransform(this, taskSpaceTransform);
}
/**
* This calculates the target pose .
*
* @param jointSpaceVect the joint space vect
* @param seconds the time for the transition to take from current
* position to target, unit seconds
* @return The joint space vector is returned for target arrival referance
* @throws Exception If there is a workspace error
*/
public double[] setDesiredJointSpaceVector(double[] jointSpaceVect, double seconds) throws Exception {
if (jointSpaceVect.length != getNumberOfLinks()) {
throw new IndexOutOfBoundsException("Vector must be " + getNumberOfLinks()
+ " links, actual number of links = " + jointSpaceVect.length);
}
synchronized(AbstractKinematicsNR.class) {
int except = 0;
Exception e = null;
do {
try {
factory.setCachedTargets(jointSpaceVect);
if (!isNoFlush()) {
//
factory.flush(seconds);
//
}
except = 0;
e = null;
} catch (Exception ex) {
except++;
e = ex;
}
} while (except > 0 && except < getRetryNumberBeforeFail());
if (e != null)
throw e;
for(int i=0;i= getNumberOfLinks()) {
throw new IndexOutOfBoundsException(
"There are only " + getNumberOfLinks() + " known links, requested:" + link);
}
LinkConfiguration conf = getLinkConfiguration(link);
if (conf.getTypeEnum() == LinkType.PID) {
getFactory().getPid(conf).removePIDEventListener(this);
// Range is in encoder units
double range = Math.abs(conf.getUpperLimit() - conf.getLowerLimit()) * 2;
Log.info("Homing link:" + link + " to latch value: " + conf.getIndexLatch());
PIDConfiguration pidConf = getLinkCurrentConfiguration(link);
PIDChannel joint = getFactory().getPid(conf).getPIDChannel(conf.getHardwareIndex());
// Clear the index
pidConf.setStopOnIndex(false);
pidConf.setUseLatch(false);
pidConf.setIndexLatch(conf.getIndexLatch());
joint.ConfigurePIDController(pidConf);// Sets up the latch
// Move forward to stop
runHome(joint, (int) (range));
// Enable index
pidConf.setStopOnIndex(true);
pidConf.setUseLatch(true);
pidConf.setIndexLatch(conf.getIndexLatch());
joint.ConfigurePIDController(pidConf);// Sets up the latch
// Move negative to the index
runHome(joint, (int) (range * -1));
pidConf.setStopOnIndex(false);
pidConf.setUseLatch(false);
pidConf.setIndexLatch(conf.getIndexLatch());
joint.ConfigurePIDController(pidConf);// Shuts down the latch
try {
setDesiredJointAxisValue(link, 0, 0);// go to zero instead of to the index itself
} catch (Exception e) {
e.printStackTrace();
}
getFactory().getPid(conf).addPIDEventListener(this);
} else {
getFactory().getLink(getLinkConfiguration(link)).Home();
getFactory().flush(1000);
}
}
/**
* This is a quick stop for all axis of the robot.
*/
public void emergencyStop() {
for (LinkConfiguration lf : getFactory().getLinkConfigurations())
if (getFactory().getPid(lf) != null)
getFactory().getPid(lf).killAllPidGroups();
}
// public void setAxisPidConfiguration(ArrayList conf) {
// this.pidConfigurations = conf;
// }
/**
* Gets the axis pid configuration.
*
* @return the axis pid configuration
*/
public ArrayList getAxisPidConfiguration() {
return pidConfigurations;
}
/**
* Inverse kinematics.
*
* @param taskSpaceTransform the task space transform
* @return Nx1 vector in task space, in mm where N is number of links
* @throws Exception the exception
*/
public abstract double[] inverseKinematics(TransformNR taskSpaceTransform) throws Exception;
/**
* Forward kinematics.
*
* @param jointSpaceVector the joint space vector
* @return 6x1 vector in task space, unit in mm,radians [x,y,z,rotx,rotY,rotZ]
*/
public abstract TransformNR forwardKinematics(double[] jointSpaceVector);
/**
* Gets the current pose target.
*
* @return the current pose target
*/
public TransformNR getCurrentPoseTarget() {
if (currentPoseTarget == null)
currentPoseTarget = calcHome();
return currentPoseTarget;
}
/**
* Sets the current pose target.
*
* @param currentPoseTarget the new current pose target
*/
public void setCurrentPoseTarget(TransformNR currentPoseTarget) {
this.currentPoseTarget = currentPoseTarget;
}
/**
* Sets the factory.
*
* @param factory the new factory
*/
public void setFactory(LinkFactory factory) {
this.factory = factory;
}
/**
* Gets the factory.
*
* @return the factory
*/
public LinkFactory getFactory() {
if (factory == null)
factory = new LinkFactory();
return factory;
}
/**
* Sets the no flush.
*
* @param noFlush the new no flush
*/
public void setNoFlush(boolean noFlush) {
this.noFlush = noFlush;
}
/**
* Checks if is no flush.
*
* @return true, if is no flush
*/
public boolean isNoFlush() {
return noFlush;
}
/**
* Gets the retry number before fail.
*
* @return the retry number before fail
*/
public int getRetryNumberBeforeFail() {
return retryNumberBeforeFail;
}
/**
* Sets the retry number before fail.
*
* @param retryNumberBeforeFail the new retry number before fail
*/
public void setRetryNumberBeforeFail(int retryNumberBeforeFail) {
this.retryNumberBeforeFail = retryNumberBeforeFail;
}
/*
* (non-Javadoc)
*
* @see com.neuronrobotics.sdk.addons.kinematics.ILinkListener#onLinkLimit(com.
* neuronrobotics.sdk.addons.kinematics.AbstractLink,
* com.neuronrobotics.sdk.pid.PIDLimitEvent)
*/
@Override
public void onLinkLimit(AbstractLink arg0, PIDLimitEvent arg1) {
for (int i = 0; i < getNumberOfLinks(); i++) {
if (getLinkConfiguration(i).getHardwareIndex() == arg0.getLinkConfiguration().getHardwareIndex())
fireJointSpaceLimitUpdate(i, new JointLimit(i, arg1, arg0.getLinkConfiguration()));
}
}
/**
* Sets the robot to fiducial transform.
*
* @param newTrans the new robot to fiducial transform
*/
public void setRobotToFiducialTransform(TransformNR newTrans) {
setBaseToZframeTransform(newTrans);
}
/**
* Gets the dh parameters chain.
*
* @return the dh parameters chain
*/
public DHChain getDhParametersChain() {
return dhParametersChain;
}
/**
* Sets the dh parameters chain.
*
* @param dhParametersChain the new dh parameters chain
*/
public void setDhParametersChain(DHChain dhParametersChain) {
this.dhParametersChain = dhParametersChain;
}
/**
* Gets the dh engine.
*
* @return the dh engine
*/
public String[] getGitDhEngine() {
return dhEngine;
}
/**
* Sets the dh engine.
*
* @param dhEngine the new dh engine
*/
public void setGitDhEngine(String[] dhEngine) {
if (dhEngine != null && dhEngine[0] != null && dhEngine[1] != null)
this.dhEngine = dhEngine;
}
/**
* Gets the cad engine.
*
* @return the cad engine
*/
public String[] getGitCadEngine() {
return cadEngine;
}
/**
* Sets the cad engine.
*
* @param cadEngine the new cad engine
*/
public void setGitCadEngine(String[] cadEngine) {
if (cadEngine != null && cadEngine[0] != null && cadEngine[1] != null)
this.cadEngine = cadEngine;
}
/**
* Gets the code.
*
* @param e the e
* @param tag the tag
* @return the code
*/
protected String getCode(Element e, String tag) {
try {
NodeList nodListofLinks = e.getChildNodes();
for (int i = 0; i < nodListofLinks.getLength(); i++) {
Node linkNode = nodListofLinks.item(i);
if (linkNode.getNodeType() == Node.ELEMENT_NODE && linkNode.getNodeName().contentEquals(tag)) {
return XmlFactory.getTagValue(tag, e);
}
}
} catch (Exception ex) {
ex.printStackTrace();
}
throw new RuntimeException("No tag " + tag + " found");
}
/**
* Gets the gist codes.
*
* @param doc the doc
* @param tag the tag
* @return the gist codes
*/
protected String[] getGitCodes(Element doc, String tag) {
String[] content = new String[3];
try {
NodeList nodListofLinks = doc.getChildNodes();
for (int i = 0; i < nodListofLinks.getLength(); i++) {
Node linkNode = nodListofLinks.item(i);
if (linkNode.getNodeType() == Node.ELEMENT_NODE && linkNode.getNodeName().contentEquals(tag)) {
Element e = (Element) linkNode;
try {
if (getCode(e, "gist") != null)
content[0] = "https://gist.github.com/" + getCode(e, "gist") + ".git";
} catch (Exception ex) {
}
try {
if (getCode(e, "git") != null)
content[0] = getCode(e, "git");
} catch (Exception ex) {
}
try {
if (getCode(e, "parallelGroup") != null)
content[2] = getCode(e, "parallelGroup");
} catch (Exception ex) {
}
content[1] = getCode(e, "file");
}
}
return content;
} catch (Exception e) {
e.printStackTrace();
}
return null;
}
public IMU getImu() {
return imu;
}
// New helper functions to make the API simpler
public void boundedLinkValueSet(int index, double value) throws Exception {
value = boundToLinkLimits(index, value);
double[] vect = getCurrentJointSpaceVector();
vect[index] = value;
setDesiredJointSpaceVector(vect, 0);
}
public double boundToLinkLimits(int index, double value) {
AbstractLink l1 = getAbstractLink(index);
if (value > l1.getMaxEngineeringUnits()) {
value = l1.getMaxEngineeringUnits();
}
if (value < l1.getMinEngineeringUnits()) {
value = l1.getMinEngineeringUnits();
}
return value;
}
public double linkMass(int linkIndex) {
return getLinkConfiguration(linkIndex).getMassKg();
}
/**
* Gets the max engineering units.
*
* @return the max engineering units
*/
public double getMaxEngineeringUnits(int linkIndex) {
return getAbstractLink(linkIndex).getMaxEngineeringUnits();
}
/**
* Gets the min engineering units.
*
* @return the min engineering units
*/
public double getMinEngineeringUnits(int linkIndex) {
return getAbstractLink(linkIndex).getMinEngineeringUnits();
}
public String getElectroMechanicalType(int linkIndex) {
return getLinkConfiguration(linkIndex).getElectroMechanicalType() ;
}
public void setElectroMechanicalType(int linkIndex,String electroMechanicalType) {
getLinkConfiguration(linkIndex).setElectroMechanicalType(electroMechanicalType);
}
public String getElectroMechanicalSize(int linkIndex) {
return getLinkConfiguration(linkIndex).getElectroMechanicalSize() ;
}
public void setElectroMechanicalSize(int linkIndex,String electroMechanicalSize) {
getLinkConfiguration(linkIndex).setElectroMechanicalSize(electroMechanicalSize);
}
public String getShaftType(int linkIndex) {
return getLinkConfiguration(linkIndex).getShaftType();
}
public void setShaftType(int linkIndex,String shaftType) {
getLinkConfiguration(linkIndex).setShaftType(shaftType);
}
public String getShaftSize(int linkIndex) {
return getLinkConfiguration(linkIndex).getShaftSize();
}
}
© 2015 - 2025 Weber Informatics LLC | Privacy Policy