Issue in UrdfExporter.to_urdf_string for parsing of non required SDF attributes

[xela-95]

While trying to convert my rod.Sdf model to urdf I got some exception related to missing fields in my Sdf model, in particular in my case they are related to the link->invertial->pose attributes:

rod/src/rod/urdf/exporter.py

Lines 246 to 247 in c67a77d

	"@xyz": " ".join(map(str, l.inertial.pose.xyz)),
	"@rpy": " ".join(map(str, l.inertial.pose.rpy)),

However, from the SDF specification they are not required: http://sdformat.org/spec?ver=1.7&elem=link#link_pose

CC @flferretti

[flferretti]

Hi @xela-95, thanks for reporting this! I looked into the URDF documentation (http://wiki.ros.org/urdf/XML/link) and you're right! I suspect that there might a different issue related to the extraction logic of xyz and rpy in

rod/src/rod/sdf/common.py

Lines 65 to 71 in c67a77d

	@property
	def xyz(self) -> list[float]:
	return self.pose[0:3]
	@property
	def rpy(self) -> list[float]:
	return self.pose[3:6]

. Would you mind reporting the exact error log?

[xela-95]

Sure!

This is the SDF I'm trying to convert to mjcf format:

SDF model

<?xml version="1.0" ?>
<sdf version="1.11">
  <model name="4_bar">
    <self_collide>false</self_collide>

    <!-- Link AB -->
    <link name="AB">
      <pose>0 0 0 0 0 0</pose>
      <inertial>
        <mass>1.0</mass>
        <inertia>
          <ixx>0.02167</ixx>
          <iyy>0.00167</iyy>
          <izz>0.02167</izz>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyz>0.0</iyz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <box>
            <size>0.1 0.5 0.1</size>
          </box>
        </geometry>
      </collision>
      <visual name="visual">
        <geometry>
          <box>
            <size>0.1 0.5 0.1</size>
          </box>
        </geometry>
      </visual>
    </link>

    <!-- Joint B -->
    <joint name="B" type="revolute">
      <parent>AB</parent>
      <child>BC</child>
      <pose relative_to="AB">0 0.25 0 0 0 -1.57</pose>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-inf</lower>
          <upper>inf</upper>
        </limit>
      </axis>
    </joint>

    <!-- Link BC -->
    <link name="BC">
      <pose relative_to="B">0 0.25 0 0 0 0</pose>
      <inertial>
        <mass>1.0</mass>
        <inertia>
          <ixx>0.02167</ixx>
          <iyy>0.00167</iyy>
          <izz>0.02167</izz>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyz>0.0</iyz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <box>
            <size>0.1 0.5 0.1</size>
          </box>
        </geometry>
      </collision>
      <visual name="visual">
        <geometry>
          <box>
            <size>0.1 0.5 0.1</size>
          </box>
        </geometry>
      </visual>
    </link>

    <!-- Joint C -->
    <joint name="C" type="revolute">
      <parent>BC</parent>
      <child>CD</child>
      <pose relative_to="BC">0 0.25 0 0 0 -1.57</pose>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-inf</lower>
          <upper>inf</upper>
        </limit>
      </axis>
    </joint>


    <!-- Link CD -->
    <link name="CD">
      <pose relative_to="C">0 0.25 0 0 0 0</pose>
      <inertial>
        <mass>1.0</mass>
        <inertia>
          <ixx>0.02167</ixx>
          <iyy>0.00167</iyy>
          <izz>0.02167</izz>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyz>0.0</iyz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <box>
            <size>0.1 0.5 0.1</size>
          </box>
        </geometry>
      </collision>
      <visual name="visual">
        <geometry>
          <box>
            <size>0.1 0.5 0.1</size>
          </box>
        </geometry>
      </visual>
    </link>

    <!-- Joint D -->
    <joint name="D" type="revolute">
      <parent>CD</parent>
      <child>DA</child>
      <pose relative_to="CD">0 0.25 0 0 0 -1.57</pose>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-inf</lower>
          <upper>inf</upper>
        </limit>
      </axis>
    </joint>

    <!-- Link DA -->
    <link name="DA">
      <pose relative_to="D">0 0.25 0 0 0 0</pose>
      <inertial>
        <mass>1.0</mass>
        <inertia>
          <ixx>0.02167</ixx>
          <iyy>0.00167</iyy>
          <izz>0.02167</izz>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyz>0.0</iyz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <box>
            <size>0.1 0.5 0.1</size>
          </box>
        </geometry>
      </collision>
      <visual name="visual">
        <geometry>
          <box>
            <size>0.1 0.5 0.1</size>
          </box>
        </geometry>
      </visual>
    </link>

    <!-- Joint A -->
    <joint name="A" type="revolute">
      <parent>DA</parent>
      <child>AB</child>
      <pose relative_to="DA">0 0.25 0 0 0 0</pose>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-inf</lower>
          <upper>inf</upper>
        </limit>
      </axis>
    </joint>


  </model>
</sdf>

I'm passing through jaxsim mujoco helper module:

    mjcf_string, assets = jaxsim.mujoco.SdfToMjcf.convert(
        sdf=model.built_from,
    )

Here's the error log:

rod[229531] DEBUG Converting model '4_bar' to URDF
rod[229531] DEBUG Detected 'AB' as root link
rod[229531] WARNING Ignoring non-trivial pose of link 'BC'
rod[229531] WARNING Ignoring non-trivial pose of link 'CD'
rod[229531] WARNING Ignoring non-trivial pose of link 'DA'
Traceback (most recent call last):
  File "/home/acroci/repos/component_darwin/src/closed_chain.py", line 90, in <module>
    viz.visualize_model(model=model, data=data, mjcf_path=mjcf_path)
    ~~~~~~~~~~~~~~~~~~~^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/home/acroci/repos/component_darwin/src/viz.py", line 36, in visualize_model
    mjcf_string, assets = jaxsim.mujoco.SdfToMjcf.convert(
                          ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^
        sdf=model.built_from,
        ^^^^^^^^^^^^^^^^^^^^^
    )
    ^
  File "/home/acroci/repos/jaxsim/src/jaxsim/mujoco/loaders.py", line 599, in convert
    return RodModelToMjcf.convert(
           ~~~~~~~~~~~~~~~~~~~~~~^
        rod_model=rod_model,
        ^^^^^^^^^^^^^^^^^^^^
    ...<3 lines>...
        cameras=cameras,
        ^^^^^^^^^^^^^^^^
    )
    ^
  File "/home/acroci/repos/jaxsim/src/jaxsim/mujoco/loaders.py", line 216, in convert
    ).to_urdf_string(
      ~~~~~~~~~~~~~~^
        sdf=rod.Sdf(model=rod_model, version="1.7"),
        ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    )
    ^
  File "/home/acroci/repos/rod/src/rod/urdf/exporter.py", line 247, in to_urdf_string
    "@xyz": " ".join(map(str, l.inertial.pose.xyz)),
                              ^^^^^^^^^^^^^^^^^^^
AttributeError: 'NoneType' object has no attribute 'xyz'

[flferretti]

I'm getting a different error, have you perhaps done any modification to either rod or jaxsim?

Full Log

AssertionError                            Traceback (most recent call last)
Cell In[22], line 1
----> 1 mjcf_string, assets = jaxsim.mujoco.SdfToMjcf.convert(
      2     sdf=path,
      3 )

File ~/jaxsim/src/jaxsim/mujoco/loaders.py:599, in SdfToMjcf.convert(sdf, considered_joints, model_name, plane_normal, heightmap, cameras)
    592 rod_model = load_rod_model(
    593     model_description=sdf,
    594     is_urdf=False,
    595     model_name=model_name,
    596 )
    598 # Convert the ROD model to MJCF.
--> 599 return RodModelToMjcf.convert(
    600     rod_model=rod_model,
    601     considered_joints=considered_joints,
    602     plane_normal=plane_normal,
    603     heightmap=heightmap,
    604     cameras=cameras,
    605 )

File ~/jaxsim/src/jaxsim/mujoco/loaders.py:216, in RodModelToMjcf.convert(rod_model, considered_joints, plane_normal, heightmap, heightmap_samples_xy, cameras)
    211     joints_dict[joint_name].type = "fixed"
    213 # Convert the ROD model to URDF.
    214 urdf_string = rod.urdf.exporter.UrdfExporter(
    215     gazebo_preserve_fixed_joints=False, pretty=True
--> 216 ).to_urdf_string(
    217     sdf=rod.Sdf(model=rod_model, version="1.7"),
    218 )
    220 # -------------------------------------
    221 # Add a floating joint if floating-base
    222 # -------------------------------------
    224 if not rod_model.is_fixed_base():

File ~/git/rod/src/rod/urdf/exporter.py:128, in UrdfExporter.to_urdf_string(self, sdf)
    122     canonical_link.pose = None
    124 # Convert all poses to use the Urdf frames convention.
    125 # This process drastically simplifies extracting compatible kinematic transforms.
    126 # Furthermore, it post-processes frames such that they get directly attached to
    127 # a real link (instead of being attached to other frames).
--> 128 model.switch_frame_convention(
    129     frame_convention=rod.FrameConvention.Urdf,
    130     explicit_frames=True,
    131     attach_frames_to_links=True,
    132 )
    134 # ============================================
    135 # Convert SDF frames to URDF equivalent chains
    136 # ============================================
    137 
    138 # Initialize the containers of extra links and joints
    139 extra_links_from_frames: list[dict[str, Any]] = []

File ~/git/rod/src/rod/sdf/model.py:166, in Model.switch_frame_convention(self, frame_convention, is_top_level, explicit_frames, attach_frames_to_links)
    157 def switch_frame_convention(
    158     self,
    159     frame_convention: rod.FrameConvention,
   (...)
    162     attach_frames_to_links: bool = True,
    163 ) -> None:
    164     from rod.utils.frame_convention import switch_frame_convention
--> 166     switch_frame_convention(
    167         model=self,
    168         frame_convention=frame_convention,
    169         is_top_level=is_top_level,
    170         attach_frames_to_links=attach_frames_to_links,
    171     )
    173     self.resolve_frames(is_top_level=is_top_level, explicit_frames=explicit_frames)

File ~/git/rod/src/rod/utils/frame_convention.py:148, in switch_frame_convention(model, frame_convention, is_top_level, attach_frames_to_links)
    144         link_name_to_parent_joint_names[j.child].append(j.name)
    145     else:
    146         # The pose of the canonical link is used to define the origin of
    147         # the URDF joint connecting the world to the robot
--> 148         assert model.is_fixed_base()
    149         link_name_to_parent_joint_names[j.child].append("world")
    151 reference_frame_model = lambda m: "world"

AssertionError: