Parsing a urdf model in Rosjava

edit

As per above, I was able to parse urdf files into the java environment. I created a urdf class that contains the same methods and structures as the Cpp urdf class.

import java.io.File;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import org.xml.sax.*;

import javax.xml.parsers.SAXParserFactory;
import javax.xml.parsers.ParserConfigurationException;
import javax.xml.parsers.SAXParser;
import org.w3c.dom.*;

import android.util.Log;


import javax.xml.parsers.*;
public class ModelInterface 
{

    private List<Joint> Joints_;
    private List<Link> Links_;

    public List<Joint> getJoints()
    {
        return Joints_;
    }

    public List<Link> getLinks()
    {
        return Links_;
    }

    public void initXml(String filename) throws SAXException, IOException, ParserConfigurationException
    {
        //This is the constructor of the object. Search for and load YAML file for robot parameters. This method uses the YAML beans to do so.
                Joints_= new ArrayList<Joint>();
                Links_= new ArrayList<Link>();
                Joints_.clear();
                Links_.clear();


                boolean validating=true;

                DocumentBuilderFactory factory= DocumentBuilderFactory.newInstance();

                //factory.setValidating(validating);

                DocumentBuilder builder = factory.newDocumentBuilder();
                File tempfile = new File(filename);

                Document document = builder.parse(tempfile);


                NodeList jointTags = document.getElementsByTagName("joint");
                NodeList linkTags = document.getElementsByTagName("link");

                for(int i =0; i<jointTags.getLength();i++)
                {
                    Joint tempjoint=new Joint();


                    tempjoint.setName(jointTags.item(i).getAttributes().getNamedItem("name").getNodeValue());
                    tempjoint.setType(jointTags.item(i).getAttributes().getNamedItem("type").getNodeValue());
                    tempjoint.setLowerLimit(Double.parseDouble(jointTags.item(i).getChildNodes().item(9).getAttributes().getNamedItem("lower").getNodeValue()));
                    tempjoint.setUpperLimit(Double.parseDouble(jointTags.item(i).getChildNodes().item(9).getAttributes().getNamedItem("upper").getNodeValue()));

                    Joints_.add(tempjoint);
                }

                for(int i =0; i<linkTags.getLength();i++)
                {
                    Link templink=new Link();
                    templink.setName(linkTags.item(i).getAttributes().getNamedItem("name").getNodeValue());

                    Links_.add(templink);
                }
    }

 }

In the initXML (filename) the urdf file is parsed using a DOM reader. It required a little debugging to ensure that the correct elements were read into the correct spots. It also doesn't check the formatting of the urdf file (ie if you change the order of how a link's elements are written) but works off the order I had used. It only gets the link name, and the joint name, types and limits. Conceivably you could parse the entire file if you liked. For reference an example of my urdf order for a link is here:

   <link name = "Tool" >
    <visual>
        <geometry>
        <cylinder length="0.15" radius="0.1"/>
            </geometry>
        <origin rpy= "1.57 0 0" xyz="0 0 0"/>
        <material name="LightGreen">
        </material>
       </visual>
    <collision>
        <geometry>
            <cylinder length="0.15" radius="0.1"/>
                </geometry>
            <origin rpy= "1.57 0 0" xyz="0 0.10 0"/>
    </collision>
    <inertial>
            <mass value="13.414"/>
            <inertia ixx="0.34" ixy="0.000039" iyy="0.0649" ixz="-0.001386" iyz="-0.000114" izz="0.296312"/>
            <origin rpy="0 0 0" xyz="0.000 0.075 0.0" />
    </inertial>
    </link>

and joints:

<joint name="joint7" type="revolute">
   <parent link="Wrist_X"/>
   <child link="Tool"/>
   <origin xyz = "0 0.38575 0" rpy = "0 0 0"/>
   <axis xyz="0 1 0" />
   <limit effort="1000" lower="-2.36" upper="2.36" velocity="1.571"/>
</joint>