Robotics StackExchange | Archived questions

URDF: Gazebo Joint Offset Calculation vs RViz

I am trying to simulate a system with multiple revolute joints using URDF, Gazebo joint state publisher, and Gazebo ros2 control. Currently,

I am struggling to get Gazebo to render the same description I was able to get with rviz2. I think the issue comes from the way that Gazebo is calculating frames of reference of child links of revolute joints. It seems that the position of all children of revolute joints are being calculated with respect to global 0 and original world frame X,Y,Z directions.

With the following code

   <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}" />
      <limit lower="0" upper="${4*pi}" effort="10" velocity="3"/>
  </joint>

I get something that renders as follows image description

You can see ${prefix}robotrotor highlighed sitting at the origin. The rviz2 tool renders this at the end of the motor on top of the large cylinder.

Whereas if I use this code

  <joint name="${prefix}_body_to_rotor" type="fixed">
      <parent link="${prefix}_robot_body" />
      <child link="${prefix}_robot_rotor" />
     <origin  xyz="${x} ${y} ${z}" rpy="${phi} ${theta} ${psi}" />
  </joint>

I get something that looks like this

image description

where the rotor appears to be properly positioned.

My top level xacro

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

  <xacro:include filename="./polou_4801.xacro" />
  <xacro:include filename="./base_wide.xacro" />
  <xacro:include filename="./shoulder.xacro" />  
  <xacro:include filename="./pcb_holder.xacro" />  
  <xacro:include filename="./lower_arm.xacro" />  
  <xacro:include filename="./forearm.xacro" />  
  <xacro:include filename="./claw.xacro" />  

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



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

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

  <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 0" />


</joint>

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


  <joint name="b_body_to_a_rotor" type="fixed">
      <parent link="a_robot_body" />
      <child link="b_robot_body" />
      <origin  rpy="0 ${-pi/2} 0" xyz="0 0 2" />
  </joint> 

  <xacro:polou_4801 prefix="b" x="0.0" y="0.0" z="0.0" phi="0.0" theta="0.0" psi="0.0" />
  <xacro:include filename="./gazebo.xacro" />  
</robot>

This is polou4801.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/workspace/src/frye_arm_1/meshes/polou_4801_body_x100.dae"/>
      </geometry>
      <material name="blue"/>
</visual>
<collision>
  <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/workspace/src/frye_arm_1/meshes/polou_4801_body_x100.dae"/>
  </geometry>
</collision>
<inertial>
    <origin xyz="${x} ${y} ${z}" rpy="${phi} ${theta} ${psi}" />
    <mass value="940" />
    <inertia ixx="0.242833" ixy="0.0" ixz="0.0" iyy="0.242833" iyz="0.0" izz="0.29375" />      
</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}" />
      <limit lower="0" upper="${4*pi}" effort="10" velocity="3"/>
  </joint>
-->  
  <joint name="${prefix}_body_to_rotor" type="fixed">
      <parent link="${prefix}_robot_body" />
      <child link="${prefix}_robot_rotor" />
      <origin  xyz="${x} ${y} ${z}" rpy="${phi} ${theta} ${psi}" />
  </joint>
  <link name="${prefix}_robot_rotor">
    <visual>
      <origin xyz="${x} ${y} ${z+6.5}" rpy="${phi} ${theta} ${psi}" />
      <geometry>
      <mesh filename="file:///home/jackfrye/Documents/Robotics/workspace/src/frye_arm_1/meshes/polou_4801_rotor_x100.dae"/>
      </geometry>




/xacro:macro

I am struggling to post the rest of the code and the site won't allow me to attach XACRO. Hopefully there is enough URDF above to help you get a view of the structure of the robot.

Really the question revolves (no pun intended) around how coordinate frames are established for joints of each type. Perhaps for revolute joints, something unexpected (or expected) is happening in the URDF->SDF conversion. I do not understand why the child links of revolute joints are not being rendered in the coordinate frame/orientation established for that joint while they appear to be for fixed joints. It should be noted this behavior is inconsistent with rviz2.

Asked by jfrye11 on 2022-12-28 17:02:02 UTC

Comments

Answers