URDF: Gazebo Joint Offset Calculation vs RViz

asked 2022-12-28 16:02:02 -0600

jfrye11 gravatar image

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}_robot_rotor 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 ...
(more)
edit retag flag offensive close merge delete