1 package com.neuronrobotics.sdk.addons.kinematics;
3 import java.util.ArrayList;
4 import java.util.HashMap;
5 import java.util.NoSuchElementException;
7 import org.w3c.dom.Element;
8 import org.w3c.dom.Node;
9 import org.w3c.dom.NodeList;
11 import com.neuronrobotics.sdk.addons.kinematics.math.ITransformNRChangeListener;
12 import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR;
13 import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
14 import com.neuronrobotics.sdk.addons.kinematics.xml.XmlFactory;
15 import com.neuronrobotics.sdk.common.Log;
18 import com.neuronrobotics.sdk.namespace.bcs.pid.IPidControlNamespace;
19 import com.neuronrobotics.sdk.pid.PIDConfiguration;
26 private ArrayList<ILinkConfigurationChangeListener>
listeners =
null;
29 private String
name =
"newLink";
32 private String
type =
"virtual";
55 private double k[] =
new double[] { 1, 0, 0 };
79 private double mass = 0.01;
85 private ArrayList<LinkConfiguration>
slaveLinks =
new ArrayList<LinkConfiguration>();
99 private HashMap<String, String[]>
vitamins =
new HashMap<String, String[]>();
115 }
catch (Exception e) {
120 }
catch (Exception e) {
127 }
catch (NullPointerException e) {
134 }
catch (NullPointerException e) {
141 }
catch (NoSuchElementException e) {
145 }
catch (NullPointerException e) {
155 }
catch (Exception ex) {
161 }
catch (Exception e) {
166 }
catch (Exception e) {
172 }
catch (Exception e) {
177 }
catch (Exception e) {
183 }
catch (Exception e) {
188 }
catch (Exception e) {
194 }
catch (Exception e) {
199 }
catch (Exception e) {
202 NodeList nodListofLinks = eElement.getChildNodes();
204 for (
int i = 0; i < nodListofLinks.getLength(); i++) {
205 Node linkNode = nodListofLinks.item(i);
207 if (linkNode.getNodeType() == Node.ELEMENT_NODE
208 && linkNode.getNodeName().contentEquals(
"centerOfMassFromCentroid")) {
209 Element cntr = (Element) linkNode;
218 }
catch (Exception e) {
222 if (linkNode.getNodeType() == Node.ELEMENT_NODE
223 && linkNode.getNodeName().contentEquals(
"imuFromCentroid")) {
224 Element cntr = (Element) linkNode;
233 }
catch (Exception e) {
237 if (linkNode.getNodeType() == Node.ELEMENT_NODE && linkNode.getNodeName().contentEquals(
"vitamins")) {
240 }
catch (Exception e) {
278 NodeList nodListofLinks = doc.getChildNodes();
279 for (
int i = 0; i < nodListofLinks.getLength(); i++) {
280 Node linkNode = nodListofLinks.item(i);
281 if (linkNode.getNodeType() == Node.ELEMENT_NODE && linkNode.getNodeName().contentEquals(
"vitamin")) {
282 Element e = (Element) linkNode;
287 }
catch (Exception ex) {
292 }
catch (Exception e) {
397 String s =
"LinkConfiguration: \n\tName: " +
getName();
423 slaves +=
"\n\t<slaveLink>\n" +
slaveLinks.get(i).getXml() +
"\n\t</slaveLink>\n";
425 String allVitamins =
"";
427 String v =
"\t\t<vitamin>\n";
428 v +=
"\t\t\t<name>" + key +
"</name>\n" +
"\t\t\t<type>" +
getVitamins().get(key)[0] +
"</type>\n"
429 +
"\t\t\t<id>" +
getVitamins().get(key)[1] +
"</id>\n";
431 v +=
"\t\t\t<variant>" +
getVitamins().get(key)[1] +
"</variant>\n";
433 v +=
"\t\t</vitamin>\n";
437 return "\t<name>" +
getName() +
"</name>\n" +
"\t" + DevStr +
"\t<type>" +
getTypeString() +
"</type>\n"
440 +
"</lowerLimit>\n" +
"\t<upperVelocity>" +
getUpperVelocity() +
"</upperVelocity>\n"
441 +
"\t<lowerVelocity>" +
getLowerVelocity() +
"</lowerVelocity>\n" +
"\t<staticOffset>"
444 +
"</deviceTheoreticalMin>\n" +
"\t<isLatch>" +
isLatch() +
"</isLatch>\n" +
"\t<indexLatch>"
447 +
"\n\t</vitamins>\n" +
"\t<passive>" +
isPassive() +
"</passive>\n" +
"\t<mass>" +
getMassKg()
450 +
"</imuFromCentroid>\n" + slaves;
755 this.velocityLimit = upperVelocity;
930 if (this.centerOfMassFromCentroid !=
null)
932 this.centerOfMassFromCentroid = com;
942 if (this.imuFromCentroid !=
null)
944 this.imuFromCentroid = imu;
954 if (
vitamins.get(
"shaft") ==
null) {
955 vitamins.put(
"shaft",
new String[] {
"hobbyServoHorn",
"standardMicro1" });
961 if (
vitamins.get(
"electroMechanical") ==
null) {
962 vitamins.put(
"electroMechanical",
new String[] {
"hobbyServo",
"standardMicro" });
964 return vitamins.get(
"electroMechanical");
1026 if (typeString ==
null)
1027 throw new NullPointerException();
1038 switch (getTypeEnum()) {
1058 switch (getTypeEnum()) {
1060 case GCODE_STEPPER_TOOL:
1061 case GCODE_HEATER_TOOL:
1079 switch (getTypeEnum()) {
1080 case ANALOG_PRISMATIC:
1082 case GCODE_STEPPER_PRISMATIC:
1111 listeners =
new ArrayList<ILinkConfigurationChangeListener>();
1115 void fireChangeEvent() {
1119 for (
int i = 0; i <
listeners.size(); i++) {
1122 }
catch (Throwable t) {
1123 t.printStackTrace();
double getDeviceTheoreticalMax()
void setUpperLimit(double upperLimit)
String[] getCoreShaftPart()
TransformNR getCenterOfMassFromCentroid()
void setPidConfiguration(IPidControlNamespace pid)
void setShaftSize(String shaftSize)
String getDeviceScriptingName()
void setTotlaNumberOfLinks(int totlaNumberOfLinks)
void setimuFromCentroid(TransformNR imu)
void setDeviceTheoreticalMin(double deviceTheoreticalMin)
TransformNR imuFromCentroid
double getLowerVelocity()
HashMap< String, String[]> getVitamins()
void setName(String name)
void addChangeListener(ILinkConfigurationChangeListener l)
void setElectroMechanicalType(String electroMechanicalType)
TransformNR getimuFromCentroid()
double getDeviceTheoreticalMin()
void setPauseEvents(boolean pauseEvents)
String getElectroMechanicalSize()
String getElectroMechanicalType()
void setScale(double scale)
void setInverted(boolean inverted)
boolean isInvertLimitVelocityPolarity()
LinkConfiguration(Object[] args)
void setStaticOffset(double staticOffset)
double deviceTheoreticalMin
LinkConfiguration(LinkConfiguration from)
ArrayList< ILinkConfigurationChangeListener > listeners
ArrayList< ILinkConfigurationChangeListener > getListeners()
void setLatch(boolean isLatch)
void setIndexLatch(double indexLatch)
String getVitaminVariant(String name)
void setUpperVelocity(double upperVelocity)
double getUpperVelocity()
void setPassive(boolean passive)
void setVitaminVariant(String name, String tagValue2)
PIDConfiguration getPidConfiguration()
LinkConfiguration(int home, int llimit, int ulimit, double d)
String deviceScriptingName
void setShaftType(String shaftType)
void setHomingTicksPerSecond(int homingTicksPerSecond)
int getTotlaNumberOfLinks()
HashMap< String, String > vitaminVariant
void setVitamin(String name, String type, String id)
double deviceTheoreticalMax
void setCenterOfMassFromCentroid(TransformNR com)
void setElectroMechanicalSize(String electroMechanicalSize)
void setLowerLimit(double lowerLimit)
void setDeviceTheoreticalMax(double deviceTheoreticalMax)
void setSlaveLinks(ArrayList< LinkConfiguration > slaveLinks)
void setTypeString(String typeString)
void clearChangeListener()
ArrayList< LinkConfiguration > getSlaveLinks()
void setHardwareIndex(int index)
void getVitamins(Element doc)
void removeChangeListener(ILinkConfigurationChangeListener l)
void setMassKg(double mass)
boolean invertLimitVelocityPolarity
LinkConfiguration(Element eElement)
void setVitamins(HashMap< String, String[]> vitamins)
ArrayList< LinkConfiguration > slaveLinks
HashMap< String, String[]> vitamins
int getHomingTicksPerSecond()
void setStopOnLatch(boolean isStopOnLatch)
void setDeviceScriptingName(String deviceScriptingName)
TransformNR centerOfMassFromCentroid
void setLinkIndex(int linkIndex)
void event(TransformNR changed)
void setInvertLimitVelocityPolarity(boolean invertVelocity)
static String getTagValue(String sTag, Element eElement)
static void info(String message)
static void error(String message)
void setInverted(boolean inverted)
void setIndexLatch(double latch)
void setStopOnIndex(boolean stopOnIndex)
static LinkType fromString(String name)
PIDConfiguration getPIDConfiguration(int group)