RelativePose() does not return pose relative to parent

[FiIipe]

Environment

• OS Version: Ubuntu 20.04
• Installation: Apt installation of Gazebo 11

Description

• Expected behavior: As described in the documentation, I was expecting this method to return the pose of the entity relative to its parent.
• Actual behavior: This method returns the pose of the entity relative to the base link of the robot.

Steps to reproduce

1. Create a new link

<!-- Robot Base -->
  <link name="base_link">
    <visual>
      <geometry>
        <mesh filename="file://$(find description_motherduck)/meshes/body.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>

2. Create another link which has the base link as parent

<!-- Motors Platform -->
  <link name="base_motors_platform_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find description_motherduck)/meshes/motors_platform.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${base_motors_platform_length} ${base_motors_platform_width} ${base_motors_platform_height}"/>
      </geometry>
    </collision>

  <joint name="motors_platform_joint" type="revolute">
    <parent link="base_link"/>
    <child link="base_motors_platform_link"/>
    <origin xyz="0.25 0.0 -0.092" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>

    <calibration rising="0.0"/>
    <dynamics damping="0.0" friction="0.5"/>
    <limit lower="-1" upper="1" effort="0" velocity="1"/>
    <safety_controller k_velocity="10" k_position="15" soft_lower_limit="-2.0" soft_upper_limit="0.5" />
  </joint>

3. Create another link that has the previous created link as parent

    <link name="wheel_link">
      <visual>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
            <mesh filename="file://$(find description_motherduck)/meshes/${wheel_file_name}"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
      </collision>

      <xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}"/>
    </link>

    <joint name="wheel_joint" type="continuous">
      <parent link="base_motors_platform_link"/>
      <child link="wheel_link"/>
      <origin xyz="0 ${y_reflect*(base_motors_platform_width/2+wheel_ygap)} 0" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
    </joint>

5. Then write a small example to get the position of the wheel relative to the platform using the following method:
   joints_wheels_->GetChild()->RelativePose();. This will return the pose relative to the base link of the robot instead of the previous link (which is the "base_motors_platform_link" in the example above).

Output

As result of this, the transformation of the wheels is incorrect and the representation on RVIZ looks like this:

The representation on Gazebo looks fine, so I assume my URDF is good

As per the URDF that I defined, the relative pose in X and Z should be 0.0 0.0 when the model spawns, however this is not the case.
[gazebo-1] Relative Pose: 0.242962 0.157838 -0.091999 -0.000238 -0.039842 0.044572

I hope my explanation is clear. If something else is necessary, I can provide more information.

Thank you.

[traversaro]

I am afraid that the documentation is not clear. The RelativePose() method is a method of the Entity class. that is inherited by the Link. So, the "parent" to which the RelativePose docs are referring to is not the "parent" link induced by a spanning tree representation of the simulated multibody system, but rather to the "parent" entity of the link, see https://github.com/gazebosim/gazebo-classic/blob/cc87a6731ceae895da863a8f8187369cba81ee54/gazebo/physics/Entity.cc#LL251C64-L251C64 .

In case of links, I think the "parent" entity of the link is the parent model, but I am not sure, probably you can find out this by looking for the use of the Link constructor in the Gazebo codebase.

If that is the case, do you have any suggestions on how to improve the docs?

[FiIipe]

@traversaro Thank you very much for the explanation. Since there is no bug, we can close this post.

Regarding the documentation, I would say that we can keep it for now. I tried to find posts describing the same problem I was having and the only one I found was this: https://answers.gazebosim.org//question/14824/get-a-pose-of-nested-model-relative-to-its-parent-model/

So, this leads me to believe that for most of the developers, the documentation is clear. For the ones that the documentation is not clear, I hope they find your explanation useful :)

To find the transformation between the child and the parent link, this did the trick for me:

auto pose_wheel = joint->GetChild()->RelativePose() - joint->GetParent()->RelativePose();

Thank you!

[traversaro]

Great! Just a small comment, the Pose3::operator- method has been deprecated in ign-math7 as it was not super intuitive its semantics, see gazebosim/gz-math#60 . This is not a super big problem in the context of Gazebo Classic, as Gazebo Classic will never move to another version of ign/gz math. Anyhow, if you want to write the code in a way that is more intuitive to people used to homogeneous matrix operation, you can write:

// parent_H_child = (base_H_parent)^{-1} * base_H_child = parent_H_base * base_H_child
auto pose_wheel = joint->GetParent()->RelativePose().inverse()*joint->GetChild()->RelativePose();