protected static void collectCheckedLinksAndJoints(
Element docElement,
ArrayList<Element> jointList,
ArrayList<Element> linkList,
HashMap<String, ArrayList<String>> succJoints,
HashMap<String, String> precMatrices,
HashMap<Integer, String> jointPoses) {
// list of all joints
jointList.clear();
// list of all links
linkList.clear();
// mapping of link to succeeding joint(s)
succJoints.clear();
// mapping of link to preceding rotational matrix
precMatrices.clear();
// counter for naming rotational matrices
int mCount = 1;
// get all joints, check for needed attributes and elements
NodeList joints = docElement.getElementsByTagName("joint");
for (int n = 0; n < joints.getLength(); n++) {
Element joint = (Element) joints.item(n);
if (!joint.getParentNode().isSameNode(docElement)) {
continue;
}
String jointName = joint.getAttribute("name");
if (jointName.isEmpty()) {
throw new RuntimeException(
"Attribute 'name' not specified for joint " + n + " (URDF document order)");
}
String type = joint.getAttribute("type");
if (type.isEmpty()) {
throw new RuntimeException("Attribute 'type' not specified for joint '" + jointName + "'");
} else {
try {
UrdfJointType.valueOf(type.toLowerCase());
} catch (IllegalArgumentException iae) {
throw new RuntimeException(
"Unknown joint type '" + type + "' in joint '" + jointName + "'");
}
}
NodeList children = joint.getElementsByTagName("child");
if (children.getLength() == 0) {
throw new RuntimeException("Element 'child' not specified for joint '" + jointName + "'");
} else if (children.getLength() > 1) {
throw new RuntimeException(
"Element 'child' specified "
+ children.getLength()
+ " times for joint '"
+ jointName
+ "'");
}
Element child = (Element) children.item(0);
String childLink = child.getAttribute("link");
if (childLink.isEmpty()) {
throw new RuntimeException(
"Attribute 'link' not specified for element 'child' in joint '" + jointName + "'");
}
NodeList parents = joint.getElementsByTagName("parent");
if (parents.getLength() == 0) {
throw new RuntimeException("Element 'parent' not specified for joint '" + jointName + "'");
} else if (parents.getLength() > 1) {
throw new RuntimeException(
"Element 'parent' specified "
+ parents.getLength()
+ " times for joint '"
+ jointName
+ "'");
}
Element parent = (Element) parents.item(0);
String parentLink = parent.getAttribute("link");
if (parentLink.isEmpty()) {
throw new RuntimeException(
"Attribute 'link' not specified for element 'parent' in joint '" + jointName + "'");
}
NodeList origins = joint.getElementsByTagName("origin");
if (origins.getLength() == 0) {
Element newOrigin = urdf.createElement("origin");
newOrigin.setAttribute("xyz", "0 0 0");
newOrigin.setAttribute("rpy", "0 0 0");
joint.appendChild(newOrigin);
origins = joint.getElementsByTagName("origin");
System.out.println(
"[INFO]\tElement 'origin' not specified for joint '"
+ jointName
+ "' -> using zero vectors for 'xyz' and 'rpy'");
} else if (origins.getLength() > 1) {
throw new RuntimeException(
"Element 'origin' specified "
+ origins.getLength()
+ " times for joint '"
+ jointName
+ "'");
} else {
Element origin = (Element) origins.item(0);
String originXYZ = origin.getAttribute("xyz");
if (originXYZ.isEmpty()) {
origin.setAttribute("xyz", "0 0 0");
System.out.println(
"[INFO]\tAttribute 'xyz' not specified for element 'origin' in joint '"
+ jointName
+ "' -> using zero vector");
} else {
StringTokenizer t = new StringTokenizer(originXYZ, " ");
if (t.countTokens() != 3) {
throw new RuntimeException(
"Wrong format in attribute 'xyz' of element 'origin' in joint '" + jointName + "'");
} else {
try {
Double.valueOf(t.nextToken());
Double.valueOf(t.nextToken());
Double.valueOf(t.nextToken());
} catch (NumberFormatException nfe) {
throw new RuntimeException(
"Wrong format in attribute 'xyz' of element 'origin' in joint '"
+ jointName
+ "'");
}
}
}
String originRPY = origin.getAttribute("rpy");
if (originRPY.isEmpty()) {
origin.setAttribute("rpy", "0 0 0");
System.out.println(
"[INFO]\tAttribute 'rpy' not specified for element 'origin' in joint '"
+ jointName
+ "' -> using zero vector");
} else {
StringTokenizer t = new StringTokenizer(originRPY, " ");
if (t.countTokens() != 3) {
throw new RuntimeException(
"Wrong format in attribute 'rpy' of element 'origin' in joint '" + jointName + "'");
} else {
try {
Double.valueOf(t.nextToken());
Double.valueOf(t.nextToken());
Double.valueOf(t.nextToken());
} catch (NumberFormatException nfe) {
throw new RuntimeException(
"Wrong format in attribute 'rpy' of element 'origin' in joint '"
+ jointName
+ "'");
}
}
}
}
jointList.add(joint);
// fill mapping of link to succeeding joint
if (!succJoints.containsKey(parentLink)) {
ArrayList<String> succs = new ArrayList<String>(2);
succs.add(jointName);
succJoints.put(parentLink, succs);
} else {
succJoints.get(parentLink).add(jointName);
}
// fill mapping of link to preceding joint's rotational matrix
jointPoses.put(mCount, OWLThing.getUniqueID("&robot;RotationMatrix3D"));
precMatrices.put(childLink, jointPoses.get(mCount++));
}
// get all links, check for needed attributes and elements
NodeList links = docElement.getElementsByTagName("link");
for (int n = 0; n < links.getLength(); n++) {
Element link = (Element) links.item(n);
if (!link.getParentNode().isSameNode(docElement)) {
continue;
}
String linkName = link.getAttribute("name").trim();
if (linkName.isEmpty()) {
throw new RuntimeException(
"Attribute 'name' not specified for link " + n + " (URDF document order)");
}
linkList.add(link);
}
}
protected static String getLinksAndJoints(
Element docElement, String targetURI, String robotName) {
String i1 = indent;
String i2 = i1 + indent;
StringBuffer s = new StringBuffer();
ArrayList<Element> jointList = new ArrayList<Element>();
ArrayList<Element> linkList = new ArrayList<Element>();
// mapping of link to succeeding joint(s)
HashMap<String, ArrayList<String>> succJoints;
succJoints = new HashMap<String, ArrayList<String>>();
// mapping of link to preceding rotational matrix
HashMap<String, String> precJoints;
precJoints = new HashMap<String, String>();
// mapping from matrix index to matrix IRI
HashMap<Integer, String> jointPoses = new HashMap<Integer, String>();
// check if urdf description is complete and fill convenience data structures
collectCheckedLinksAndJoints(
docElement, jointList, linkList, succJoints, precJoints, jointPoses);
// Read Gazebo Tags of docElement file
GazeboInformationReader gazeboReader = new GazeboInformationReader(docElement);
GeneralInformationReader generalReader = new GeneralInformationReader();
Element rootLink = findRootLinkOfJointTree(jointList, linkList);
String rootLinkName = rootLink.getAttribute("name");
System.out.println("[INFO]\tRoot link of robot is '" + rootLinkName + "'");
// add robot instance
s.append(getRobotInstance(robotName, rootLinkName));
s.append("\n\n");
s.append(i1 + "<!-- =========================================== -->\n");
s.append(i1 + "<!-- | Robot Joints | -->\n");
s.append(i1 + "<!-- =========================================== -->\n");
// add joints
int mCount = 0;
String prefix = robotName + "_";
double[] rotTransMat = new double[16];
for (Element joint : jointList) {
String jointName = joint.getAttribute("name");
String jointType = joint.getAttribute("type");
Element parent = (Element) joint.getElementsByTagName("parent").item(0);
String parentLinkName = parent.getAttribute("link");
Element child = (Element) joint.getElementsByTagName("child").item(0);
String childLinkName = child.getAttribute("link");
s.append("\n\n");
s.append(i1 + "<!-- &robot;" + prefix + jointName + " -->\n\n");
s.append(i1 + "<owl:NamedIndividual rdf:about=\"&robot;" + prefix + jointName + "\">\n");
s.append(i2 + "<rdf:type rdf:resource=\"&srdl2-comp;");
switch (UrdfJointType.valueOf(jointType.toLowerCase())) {
case continuous:
s.append("ContinuousUrdfJoint");
break;
case fixed:
s.append("FixedUrdfJoint");
break;
case floating:
s.append("FloatingUrdfJoint");
break;
case planar:
s.append("PlanarUrdfJoint");
break;
case prismatic:
s.append("PrismaticUrdfJoint");
break;
case revolute:
s.append("RevoluteUrdfJoint");
break;
}
s.append("\"/>\n");
s.append(i2 + "<srdl2-comp:urdfName>" + jointName + "</srdl2-comp:urdfName>\n");
// s.append(i2+"<srdl2-comp:precedingLink rdf:resource=\"&robot;" + prefix + parentLinkName
// + "\"/>\n");
s.append(
i2
+ "<srdl2-comp:succeedingLink rdf:resource=\"&robot;"
+ prefix
+ childLinkName
+ "\"/>\n");
// Include hasAttribute-Tags for gazebo information
s.append(generalReader.getAttributeIncludesFor(i2, prefix, joint));
// Include hasAttribute-Tags for gazebo information
s.append(gazeboReader.getAttributeIncludesFor(i2, prefix, jointName));
// s.append(i2+"<knowrob:orientation rdf:resource=\""+ jointPoses.get(++mCount) + "\"/>\n");
s.append(i1 + "</owl:NamedIndividual>\n");
// add related rotation matrix
Element origin = (Element) joint.getElementsByTagName("origin").item(0);
String originXYZ = origin.getAttribute("xyz");
StringTokenizer t = new StringTokenizer(originXYZ, " ");
double x = Double.valueOf(t.nextToken().trim());
double y = Double.valueOf(t.nextToken().trim());
double z = Double.valueOf(t.nextToken().trim());
String originRPY = origin.getAttribute("rpy");
t = new StringTokenizer(originRPY, " ");
double roll = Double.valueOf(t.nextToken().trim());
double pitch = Double.valueOf(t.nextToken().trim());
double yaw = Double.valueOf(t.nextToken().trim());
rotTransMat = calcRotTransMatrix3D(x, y, z, roll, pitch, yaw, rotTransMat);
// preceding joint, used for 'knowrob:relativeTo' object property
String precMatrix = precJoints.get(parentLinkName);
// current joint
String currMatrix = precJoints.get(childLinkName);
// create Proprioception instance for this joint
String proprio = OWLThing.getUniqueID("&robot;Proprioception");
s.append("\n\n");
s.append(i1 + "<!-- " + proprio + " -->\n\n");
s.append(i1 + "<owl:NamedIndividual rdf:about=\"" + proprio + "\">\n");
s.append(i2 + "<rdf:type rdf:resource=\"&knowrob;Proprioception\"/>\n");
s.append(i2 + "<knowrob:eventOccursAt rdf:resource=\"" + currMatrix + "\"/>\n");
s.append(i2 + "<knowrob:startTime rdf:resource=\"&robot;timepoint_1357020000\"/>\n");
s.append(
i2 + "<knowrob:objectActedOn rdf:resource=\"&robot;" + prefix + jointName + "\"/>\n");
s.append(i1 + "</owl:NamedIndividual>\n\n");
// create RotationMatrix3D for the default orientation
s.append("\n\n");
s.append(i1 + "<!-- " + currMatrix + " -->\n\n");
s.append(i1 + "<owl:NamedIndividual rdf:about=\"" + currMatrix + "\">\n");
s.append(i2 + "<rdf:type rdf:resource=\"&knowrob;RotationMatrix3D\"/>\n");
if (precMatrix != null) {
s.append(i2 + "<knowrob:relativeTo rdf:resource=\"" + precMatrix + "\"/>\n");
}
for (int r = 0; r < 4; r++) { // 4 rows
for (int c = 0; c < 4; c++) { // 4 columns
double value = rotTransMat[c + r * 4];
String id = r + "" + c;
s.append(
i2
+ "<knowrob:m"
+ id
+ " rdf:datatype=\"&xsd;double\">"
+ value
+ "</knowrob:m"
+ id
+ ">\n");
}
}
s.append(i1 + "</owl:NamedIndividual>\n\n");
}
s.append("\n\n");
s.append(i1 + "<!-- =========================================== -->\n");
s.append(i1 + "<!-- | Robot Links | -->\n");
s.append(i1 + "<!-- =========================================== -->\n");
// add links
for (Element link : linkList) {
String linkName = link.getAttribute("name");
s.append("\n\n");
s.append(i1 + "<!-- &robot;" + prefix + linkName + " -->\n\n");
s.append(i1 + "<owl:NamedIndividual rdf:about=\"&robot;" + prefix + linkName + "\">\n");
s.append(i2 + "<rdf:type rdf:resource=\"&srdl2-comp;UrdfLink\"/>\n");
s.append(i2 + "<srdl2-comp:urdfName>" + linkName + "</srdl2-comp:urdfName>\n");
// Include hasAttribute-Tags for gazebo information
s.append(generalReader.getAttributeIncludesFor(i2, prefix, link));
ArrayList<String> succs = succJoints.get(linkName);
if (succs != null) {
for (String succ : succs) {
s.append(
i2 + "<srdl2-comp:succeedingJoint rdf:resource=\"&robot;" + prefix + succ + "\"/>\n");
}
}
// Include hasAttribute-Tags for gazebo information
s.append(gazeboReader.getAttributeIncludesFor(i2, prefix, linkName));
s.append(i1 + "</owl:NamedIndividual>\n");
}
return s.toString();
}
