URDF to SDF Frame of Reference Calculation for Base Joint Fixed vs Floating

Question

I am trying to simulate a robot I do not want fixed to the Gazebo world I am spawning it into. However, when I make the joint between my base link and the world floating, I get unexpected behavior in the way children frames of reference are established.

To preface, here is all the relevant XACRO

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="frye_arm_1">

  <xacro:include filename="/home/jackfrye/Documents/Robotics/workspace/src/frye_arm_1/urdf/base_wide.xacro" />
  <xacro:include filename="/home/jackfrye/Documents/Robotics/workspace/src/frye_arm_1/urdf/polou_4801.xacro" />

  <link name="world"></link>

  <joint name="world_to_base" type="floating">
      <parent link="world" />
  <child link="a_base_wide" />
  <origin  rpy="0 0 0" xyz="0 0 0" />
  </joint>

  <xacro:base_wide prefix="a" x="0.0" y="0.0" z="0.374" />

  <joint name="base_to_motor_body" type="fixed">
      <parent link="a_base_wide" />
      <child link="a_robot_body" />
      <origin  rpy="0 0 0" xyz="0 0 .359" />
  </joint>

  <xacro:polou_4801 prefix="a" x="0.0" y="0.0" z="0.0" phi="0.0" theta="0.0" psi="0.0" />
</robot>

base_wide.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="frye_arm_1">
  <xacro:macro name="base_wide" params="prefix x y z">
  
  <link name="${prefix}_base_wide">
    <visual>
      <origin xyz="${x} ${y} ${z}" rpy="0 0 0" />
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/base_widen/base_widen_x10.dae"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <origin xyz="${x} ${y} ${z}" rpy="0 0 0"/>    
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/base_widen/base_widen_x10.dae"/>
      </geometry>
    </collision>
    <inertial>
        <origin xyz="${x} ${y} ${z}" rpy="0 0 0"/>
        <mass value="131.0971" />
        <inertia ixx="0.1811" ixy="0.0" ixz="0.0" iyy="0.1809" iyz="0.0002" izz="0.18" />      
    </inertial>     
  </link>
  </xacro:macro>
</robot>

polou_4801.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="frye_arm_1">
  <xacro:macro name="polou_4801" params="prefix x y z phi theta psi">
  
    <link name="${prefix}_robot_body">
    <visual>
      <origin xyz="${x} ${y} ${z}" rpy="${phi} ${theta} ${psi}" />
      <geometry>
        <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_body/polou_4801_body_x10_no_wire.dae"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <origin xyz="${x} ${y} ${z}" rpy="${phi} ${theta} ${psi}" />    
      <geometry>
          <mesh filename="file://file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_body/polou_4801_body_x10_no_wire.dae"/>
      </geometry>
    </collision>
    <inertial>
        <origin xyz="${x} ${y} ${z}" rpy="${phi} ${theta} ${psi}" />
        <mass value="94" />
        <inertia ixx="3.4438" ixy="0.0" ixz="0.0" iyy="3.4438" iyz="0.0" izz="0.7338" />      
    </inertial>     
  </link>

  <joint name="${prefix}_body_to_rotor" type="revolute">
      <parent link="${prefix}_robot_body" />
      <child link="${prefix}_robot_rotor" />
      <axis xyz="0 0 1" />
<!--      <origin  xyz="${x} ${y} ${z}" rpy="${phi} ${theta} ${psi}" /> -->
         <origin  xyz="0 0 0.374" rpy="0 0 0" /> 
      <limit lower="0" upper="${4*pi}" effort="10" velocity="3"/>
  </joint>
  
  <link name="${prefix}_robot_rotor">
    <visual>
<!--      <origin xyz="${x} ${y} ${z+0.75}" rpy="${phi} ${theta} ${psi}" /> -->
      <origin xyz="${x} ${y} ${z}" rpy="0 0 0" /> 
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_shaft/polou_4801_shaft_x10.dae"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
<!--      <origin xyz="${x} ${y} ${z+0.75}" rpy="${phi} ${theta} ${psi}" /> -->
      <origin xyz="${x} ${y} ${z}" rpy="0 0 0" /> 
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_shaft/polou_4801_shaft_x10.dae"/>
      </geometry>
    </collision>
    <inertial>
<!--      <origin xyz="${x} ${y} ${z+0.75}" rpy="${phi} ${theta} ${psi}" /> -->
      <origin xyz="${x} ${y} ${z}" rpy="0 0 0" /> 
        <mass value=".05" />
        <inertia ixx="0.00000" ixy="0.0" ixz="0.0" iyy="0.0000" iyz="0.0" izz="0.0000" />      
    </inertial>       
   </link>
  </xacro:macro>

</robot>

Now in the top level for the joint "world_to_base", when I used floating, which is the setting I want to use, it seems like the transforms are not being done as expected. If I say I want to make a joint at 0.374 +Z, that means I want that joint at +.374 from the parent's frame of reference. If I create a floating joint the move the child +Z .359, that should create the frame of reference from which the joint is created. In that case, the joint would then be at +.359+.374 in the "world" frame. When I do a floating joint, I do not see this behavior.

However, when I used fixed for the connection between my base link and world, I do see this relationship in the link-joint tree.

It makes it difficult to write portable XACRO if I cannot assume that my make first joint in the XACRO (eg the motor body joint) is going to be offset WRT to its parent not and not world. Maybe I just need to refactor and add logic into the polou xacro that takes into account that all joints are being layed out wrt the world frame, but I would prefer not to and just have it calculate like it does for the "fixed" case.

Answer 1

I think there is an issue with the URDF-to-SDFormat converter in libsdformat's parser_urdf.cc file since it currently ignores both floating and planar joint types. I've opened the following feature request to sdformat to properly parse the floating joint origin tags.

• https://github.com/gazebosim/sdformat/issues/1282

Answer 2

In my experience, I've only seen the pattern where you add a world link to the URDF to affix the robot to the world. If you want it to be floating, you don't need the world link at all, just start from a_base_wide. There is no floating joint in SDF since that is equivalent to not having a joint (https://answers.gazebosim.org/question/14728/what-is-a-floating-joint-physically/?answer=14737#post-id-14737). If you still need to offset the whole robot when it's spawned, you can use the <gazebo> tag to directly insert a <pose> tag to the resulting model.

  <gazebo>
    <pose>1 0 0  0 0 0</pose>
  </gazebo>

I haven't tested that so it may or may not work. Alternatively, you can change your launch script to spawn the robot at a desired pose.