URDFTools can take a Collection<InputStream> as the first parameters to allow for splitting the URDF up into multiple files

scs2-definition/src/main/java/us/ihmc/scs2/definition/robot/urdf/URDFTools.java

@@ -166,7 +166,7 @@ public static URDFModel loadURDFModel(File urdfFile, Collection<String> resource
       try
       {
          // Internally, the unmarshaller does "new BufferedInputStream(new FileInputStream(urdfFile)" (see AbstractUnmarshallerImpl), no need to have 2 distinct implementations.
-         return loadURDFModel(new BufferedInputStream(new FileInputStream(urdfFile)), resourceDirectories, null, parserProperties);
+         return loadURDFModel(Collections.singletonList(new BufferedInputStream(new FileInputStream(urdfFile))), resourceDirectories, null, parserProperties);
       }
       catch (FileNotFoundException e)
       {
@@ -189,14 +189,19 @@ public static URDFModel loadURDFModel(File urdfFile, Collection<String> resource
     * @return the model.
     */
    public static URDFModel loadURDFModel(InputStream inputStream, Collection<String> resourceDirectories, ClassLoader resourceClassLoader) throws JAXBException
+   {
+      return loadURDFModel(Collections.singletonList(inputStream), resourceDirectories, resourceClassLoader, DEFAULT_URDF_PARSER_PROPERTIES);
+   }
+
+   public static URDFModel loadURDFModel(Collection<InputStream> inputStream, Collection<String> resourceDirectories, ClassLoader resourceClassLoader) throws JAXBException
    {
       return loadURDFModel(inputStream, resourceDirectories, resourceClassLoader, DEFAULT_URDF_PARSER_PROPERTIES);
    }
 
    /**
     * Parse a {@link URDFModel} from the given input stream.
     *
-    * @param inputStream         the stream to be loaded.
+    * @param inputStreams         the stream to be loaded.
     * @param resourceDirectories paths to resource directories. This allows to search for resources
     *                            that are defined outside the {@code inputStream}.
     * @param resourceClassLoader the class loader is used to retrieve the resources. If the resources
@@ -209,66 +214,188 @@ public static URDFModel loadURDFModel(InputStream inputStream, Collection<String
     *                            done.
     * @return the model.
     */
-   public static URDFModel loadURDFModel(InputStream inputStream,
+   public static URDFModel loadURDFModel(Collection<InputStream> inputStreams,
                                          Collection<String> resourceDirectories,
                                          ClassLoader resourceClassLoader,
                                          URDFParserProperties parserProperties) throws JAXBException
    {
+      Set<String> allResourceDirectories = new HashSet<>(resourceDirectories);
+      JAXBContext context = JAXBContext.newInstance(URDFModel.class);
+      Unmarshaller um = context.createUnmarshaller();
+
+      URDFModel combinedURDFModel = new URDFModel();
+
       try
       {
-         Set<String> allResourceDirectories = new HashSet<>(resourceDirectories);
-         URDFModel urdfModel;
-         JAXBContext context = JAXBContext.newInstance(URDFModel.class);
-         Unmarshaller um = context.createUnmarshaller();
-
-         if (!parserProperties.ignoreNamespace)
+         for (InputStream inputStream : inputStreams)
          {
-            urdfModel = (URDFModel) um.unmarshal(inputStream);
+            URDFModel urdfModel;
+            if (!parserProperties.ignoreNamespace)
+            {
+               urdfModel = (URDFModel) um.unmarshal(inputStream);
+            }
+            else
+            {
+               InputSource is = new InputSource(inputStream);
+               SAXParserFactory sax = SAXParserFactory.newInstance();
+               sax.setNamespaceAware(false);
+
+               XMLReader reader;
+               try
+               {
+                  reader = sax.newSAXParser().getXMLReader();
+               }
+               catch (ParserConfigurationException | SAXException e)
+               {
+                  throw new JAXBException(e);
+               }
+
+               SAXSource source = new SAXSource(reader, is);
+               urdfModel = (URDFModel) um.unmarshal(source);
+            }
+
+            resolvePaths(urdfModel, allResourceDirectories, resourceClassLoader);
+
+            if (!parserProperties.linksToIgnore.isEmpty() && urdfModel.getLinks() != null)
+            {
+               urdfModel.getLinks().removeIf(link -> parserProperties.linksToIgnore.contains(link.getName()));
+            }
+
+            if (!parserProperties.jointsToIgnore.isEmpty() && urdfModel.getJoints() != null)
+            {
+               urdfModel.getJoints().removeIf(joint -> parserProperties.jointsToIgnore.contains(joint.getName()));
+            }
+
+            if (!parserProperties.parseSensors && urdfModel.getGazebos() != null)
+            {
+               urdfModel.getGazebos().removeIf(gazebo -> gazebo.getSensor() != null);
+            }
+
+            if (parserProperties.handleImplicitJointDefinitions)
+               handleImplicitJointDefinitions(urdfModel);
+
+            // Merge the current URDFModel with the combined URDFModel
+            combinedURDFModel = mergeURDFModels(combinedURDFModel, urdfModel);
          }
-         else
+      }
+      finally
+      {
+         for (InputStream inputStream : inputStreams)
          {
-            InputSource is = new InputSource(inputStream);
-            SAXParserFactory sax = SAXParserFactory.newInstance();
-            sax.setNamespaceAware(false);
-            XMLReader reader;
-
             try
             {
-               reader = sax.newSAXParser().getXMLReader();
+               inputStream.close();
             }
-            catch (SAXException | ParserConfigurationException e)
+            catch (IOException e)
             {
-               throw new JAXBException(e);
+               LogTools.error(e.getMessage());
             }
-
-            SAXSource source = new SAXSource(reader, is);
-            urdfModel = (URDFModel) um.unmarshal(source);
          }
-         resolvePaths(urdfModel, allResourceDirectories, resourceClassLoader);
+      }
 
-         if (!parserProperties.linksToIgnore.isEmpty() && urdfModel.getLinks() != null)
-            urdfModel.getLinks().removeIf(urdfLink -> parserProperties.linksToIgnore.contains(urdfLink.getName()));
-         if (!parserProperties.jointsToIgnore.isEmpty() && urdfModel.getJoints() != null)
-            urdfModel.getJoints().removeIf(urdfJoint -> parserProperties.jointsToIgnore.contains(urdfJoint.getName()));
-         if (!parserProperties.parseSensors)
-            urdfModel.getGazebos().removeIf(gazebo -> gazebo.getSensor() != null);
+      return combinedURDFModel;
+   }
 
-         if (parserProperties.handleImplicitJointDefinitions)
-            handleImplicitJointDefinitions(urdfModel);
+   private static URDFModel mergeURDFModels(URDFModel baseModel, URDFModel additionalModel)
+   {
+      // Create a new URDFModel to hold the merged components
+      URDFModel mergedModel = new URDFModel();
+      List<String> mergedLinkNames = new ArrayList<>();
+      List<String> mergedJointNames = new ArrayList<>();
+      List<String> mergedGazeboNames = new ArrayList<>();
 
-         return urdfModel;
+      // Set name of merged urdf
+      if (baseModel.getName() == null)
+      {
+         mergedModel.setName(additionalModel.getName());
       }
-      finally
+      else
       {
-         try
+         mergedModel.setName(baseModel.getName());
+      }
+
+      // Merge links that haven't already been added
+      List<URDFLink> mergedLinks = new ArrayList<>();
+      if (baseModel.getLinks() != null)
+      {
+         mergedLinks.addAll(baseModel.getLinks());
+         for (URDFLink link : baseModel.getLinks())
          {
-            inputStream.close();
+            mergedLinkNames.add(link.getName());
          }
-         catch (IOException e)
+      }
+      if (additionalModel.getLinks() != null)
+      {
+         for (URDFLink link : additionalModel.getLinks())
          {
-            LogTools.error(e.getMessage());
+            if (!mergedLinkNames.contains(link.getName()))
+            {
+               mergedLinks.add(link);
+               mergedLinkNames.add(link.getName());
+            }
+            else
+            {
+               LogTools.warn("Link ({}) has already been added to urdf, skipping link", link.getName());
+            }
+         }
+      }
+      mergedModel.setLinks(new ArrayList<>(mergedLinks));
+
+      // Merge joints that haven't already been added
+      List<URDFJoint> mergedJoints = new ArrayList<>();
+      if (baseModel.getJoints() != null)
+      {
+         mergedJoints.addAll(baseModel.getJoints());
+         for (URDFJoint joint : baseModel.getJoints())
+         {
+            mergedJointNames.add(joint.getName());
+         }
+      }
+      if (additionalModel.getJoints() != null)
+      {
+         for (URDFJoint joint : additionalModel.getJoints())
+         {
+            if (!mergedJointNames.contains(joint.getName()))
+            {
+               mergedJoints.add(joint);
+               mergedJointNames.add(joint.getName());
+            }
+            else
+            {
+               LogTools.warn("Joint ({}) has already been added to urdf, skipping joint", joint.getName());
+            }
+         }
+      }
+      mergedModel.setJoints(new ArrayList<>(mergedJoints));
+
+      // Merge Gazebos that haven't already been added
+      List<URDFGazebo> mergedGazebos = new ArrayList<>();
+      if (baseModel.getGazebos() != null)
+      {
+         mergedGazebos.addAll(baseModel.getGazebos());
+         for (URDFGazebo gazebo : baseModel.getGazebos())
+         {
+            mergedGazeboNames.add(gazebo.getReference());
          }
       }
+      if (additionalModel.getGazebos() != null)
+      {
+         for (URDFGazebo gazebo : additionalModel.getGazebos())
+         {
+            if (!mergedGazeboNames.contains(gazebo.getReference()))
+            {
+               mergedGazebos.add(gazebo);
+               mergedGazeboNames.add(gazebo.getReference());
+            }
+            else
+            {
+               LogTools.warn("Gazebo ({}) has already been added to urdf, skipping gazebo", gazebo.getReference());
+            }
+         }
+      }
+      mergedModel.setGazebos(new ArrayList<>(mergedGazebos));
+
+      return mergedModel;
    }
 
    /**
@@ -2398,7 +2525,7 @@ public static URDFLinkReference toURDFLinkReference(RigidBodyDefinition rigidBod
    /**
     * This class provides extra properties for tweaking operations when parsing a URDF file. It is used
     * in both
-    * {@link URDFTools#loadURDFModel(InputStream, Collection, ClassLoader, URDFParserProperties)} and
+    * {@link URDFTools#loadURDFModel(Collection, Collection, ClassLoader, URDFParserProperties)} and
     * {@link URDFTools#toRobotDefinition(URDFModel, URDFParserProperties)}.
     *
     * @author Sylvain Bertrand
@@ -2425,7 +2552,7 @@ public static class URDFParserProperties
        * </p>
        *
        * @param ignoreNamespace {@code true} to ignore namespaces. Recommended value {@code false}.
-       * @see URDFTools#loadURDFModel(InputStream, Collection, ClassLoader, URDFParserProperties)
+       * @see URDFTools#loadURDFModel(Collection, Collection, ClassLoader, URDFParserProperties)
        */
       public void setIgnoreNamespace(boolean ignoreNamespace)
       {
@@ -2439,7 +2566,7 @@ public void setIgnoreNamespace(boolean ignoreNamespace)
        * URDF files.
        *
        * @param nameOfJointToIgnore the name of a joint to be ignored when parsing the URDF file.
-       * @see URDFTools#loadURDFModel(InputStream, Collection, ClassLoader, URDFParserProperties)
+       * @see URDFTools#loadURDFModel(Collection, Collection, ClassLoader, URDFParserProperties)
        */
       public void addJointToIgnore(String nameOfJointToIgnore)
       {
@@ -2453,7 +2580,7 @@ public void addJointToIgnore(String nameOfJointToIgnore)
        * URDF files.
        *
        * @param nameOfLinkToIgnore the name of a link to be ignored when parsing the URDF file.
-       * @see URDFTools#loadURDFModel(InputStream, Collection, ClassLoader, URDFParserProperties)
+       * @see URDFTools#loadURDFModel(Collection, Collection, ClassLoader, URDFParserProperties)
        */
       public void addLinkToIgnore(String nameOfLinkToIgnore)
       {

scs2-examples/src/main/resources/urdf/limbA.urdf

@@ -0,0 +1,69 @@
+<?xml version="1.0" ?>
+<robot name="limbA" xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <joint name="jointA" type="revolute">
+        <axis xyz="0 1 0"/>
+        <origin rpy="0 0 0" xyz="0.1 0 0.2"/>
+        <parent link="baseLink"/>
+        <child link="bodyAB"/>
+        <limit effort="190" lower="-1.1" upper="0.4141" velocity="5.89"/>
+    </joint>
+    <link name="bodyAB">
+        <inertial>
+            <mass value="0.025"/>
+            <origin rpy="0 0 0" xyz="0  0  0.1"/>
+            <inertia ixx="1.0e-8" ixy="0" ixz="0" iyy="1.0e-8" iyz="0" izz="1.0e-8"/>
+        </inertial>
+        <visual>
+            <geometry>
+                <cylinder radius="0.0125" length="0.4"/>
+            </geometry>
+            <material>
+                <color rgba="0.92 0.85 0.2 1"/>
+            </material>
+            <origin rpy="0 0 0" xyz="0  0  0.1"/>
+        </visual>
+    </link>
+    <joint name="jointB" type="revolute">
+        <axis xyz="0 1 0"/>
+        <origin rpy="0 0 0" xyz="0 0 0.2"/>
+        <parent link="bodyAB"/>
+        <child link="endEffectorLink"/>
+        <limit effort="190" lower="-1.1" upper="0.4141" velocity="5.89"/>
+        <mimic joint="jointA" multiplier="1.25" offset="0"/>
+    </joint>
+    <link name="endEffectorLink">
+        <inertial>
+            <mass value="5"/>
+            <origin rpy="0 0 0" xyz="0  0  0.1"/>
+            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.005"/>
+        </inertial>
+        <visual>
+            <geometry>
+                <box size="0.0125 0.0125 0.2"/>
+            </geometry>
+            <material>
+                <color rgba="0.2 0.7 0.9 1"/>
+            </material>
+            <origin rpy="0 0 0" xyz="0 0 0.1"/>
+        </visual>
+    </link>
+    <joint name="jointC" type="revolute">
+        <axis xyz="1 0 0"/>
+        <origin rpy="0 0 0" xyz="0.1 0 0.0"/>
+        <parent link="baseLink"/>
+        <child link="head"/>
+        <limit effort="190" lower="-1.1" upper="0.4141" velocity="5.89"/>
+    </joint>
+    <link name="head">
+        <visual>
+            <geometry>
+                <sphere radius="0.1"/>
+            </geometry>
+            <material>
+                <color rgba="0.2 0.7 0.9 1"/>
+            </material>
+            <origin rpy="0 0 0" xyz="0.1 0 0.3"/>
+        </visual>
+    </link>
+</robot>