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.
package com.neuronrobotics.sdk.addons.kinematics;
import java.io.InputStream;
import java.sql.Timestamp;
import java.util.ArrayList;
import javafx.application.Platform;
import javafx.embed.swing.JFXPanel;
import javafx.scene.transform.Affine;
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.
*/
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{
new JFXPanel(); // this will prepare JavaFX toolkit and environment
}
/**
* 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() {
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;iuLim[i])
return false;
if(jointSpaceVect[i]0 && except =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 = new TransformNR();
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 © 2015 - 2025 Weber Informatics LLC | Privacy Policy