コード例 #1
0
ファイル: URDF2SRDL.java プロジェクト: RoboWGT/robo_groovy
  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);
    }
  }
コード例 #2
0
ファイル: URDF2SRDL.java プロジェクト: RoboWGT/robo_groovy
  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();
  }