Wrong gazebo::physics::Model::WorldPose() for nested models

[osrf-migration]

Original report (archived issue) by Nicola Piga (Bitbucket: xenvre).

The original report had attachments: ModelPosePublisher.cc, ModelPosePublisher.hh, model.sdf, model.config

---

I'm trying to write a Gazebo Model plugin that simply gets the absolute pose of a model nested within another.

The sdf looks like this

<sdf version="1.5">

  <model name="root_model">
    <pose> 1 1 0 0 0 0 </pose>
    <link name="root_model_root_link">
      <!-- standard sdf content here -->
    </link>

    <!-- nested model -->
    <model name="nested_model">
      <!-- pose relative to the parent model -->
      <pose> 0 0 1 0 0 0</pose>
      <link name="nested_model_root_link">
        <!-- standard sdf content here -->
      </link>

      <!-- plugin inserted inside the nested model -->
      <plugin name='plugin' filename='plugin_name.so'></plugin>
    </model>

  </model>

</sdf>

Let's say I have a little box (the nested model) on top of another box (the root model) and I want to know the absolute position of the little box.

With this code I'm getting the pointer to the model

void PluginClass::Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf)                                                       
{        
   // m_model is a member of the class                                                                                                                                                                                                                                                                 
    m_model = _parent;  
}

and then I try to read the absolute pose with respect to the Gazebo world frame in a method that was binded to the World update event using gazebo::event::Events::ConnectWorldUpdateBegin

...
ignition::math::Pose3d currentPose;
currentPose = m_model->WorldPose();
...

When I start Gazebo and load the sdf I obtain for the positional part of the nested model

x = 1
y = 1
z = 1

and that is correct. However if I try to interact with the root model and with the nested model strange things happen. For example if I move the big box upward using the Translation Mode and then I release it the box falls due to gravity, together with the little box. The new position obtained is like (it depends on how much the box was moved upward)

x = 1
y = 1
z = 1.92

which is clearly wrong since the two objects, after the transient, are again in the initial position. If I repeat the experiment the z coordinates increases further.

Also if I try to move the little box relative to the big box, e.g. a robot pushes the little box on a plane parallel to the x-y plane of the inertial reference frame, the position is not updated at all.

Is this behavior normal?

[osrf-migration]

Original comment by Nicola Piga (Bitbucket: xenvre).

---

• Edited issue description

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

Thanks for the report. I'd like to help, but it's hard without more details. If you can upload a complete model and example of your plugin, we can try to reproduce your issue.

[osrf-migration]

Original comment by Nicola Piga (Bitbucket: xenvre).

---

Thank you Steven. Sure I can upload them.

I will upload two files

• ModelPosePublisher.hh
• ModelPosePublisher.cc

containing the plugin that prints the position of the model (I think it's pretty standard code)

and two files for the model

• model.config
• model.sdf

[osrf-migration]

Original comment by Nicola Piga (Bitbucket: xenvre).

---

• set attachment to "ModelPosePublisher.cc"

Implementation of plugin

[osrf-migration]

Original comment by Nicola Piga (Bitbucket: xenvre).

---

• set attachment to "ModelPosePublisher.hh"

Header of plugin

[osrf-migration]

Original comment by Nicola Piga (Bitbucket: xenvre).

---

• set attachment to "model.sdf"

Model .config and .sdf

[osrf-migration]

Original comment by Nicola Piga (Bitbucket: xenvre).

---

The first thing that you could do to reproduce the problem is to select the whole model and use the Translation Mode to move it upward. When the selection is released the box and the little box on top of it falls due to gravity and then the 'z' coordinate printed in the terminal will have a very strange value. Since the plugin is attached in the nested model, inside the SDF, the position printed by the plugin should be that of the nested model. Am I right?

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

I was able to reproduce the error with the following CMakeLists.txt:

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(issue_2410)

# Find packages

find_package(Gazebo REQUIRED)

# include appropriate directories
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")

# Create libraries and executables

add_library(gazebo_modelPosePublisher SHARED ModelPosePublisher.cc)
target_link_libraries(gazebo_modelPosePublisher ${GAZEBO_LIBRARIES})

I can't see that you've done anything wrong. I tried setting gravity to up and the box floats upward, but the printed pose doesn't change at all. This is probably a bug in updating the world pose of a nested model. I last fixed a bug in this logic in Entity.cc in pull request #2702 when fixing a bug in getting the world pose of a Collision. I'm guessing that's where this bug is coming from.

[osrf-migration]

Original comment by Nicola Piga (Bitbucket: xenvre).

---

Hi @scpeters thank you for taking time to try the code that I have attached.

I tried to read the code of the class gazebo::physics::Entity and I have some doubts regarding the way in which the pose of a nested model is updated.

In the method gazebo::physics::Entity::Load the initial world pose of the nested model is assigned like this

void Entity::Load(sdf::ElementPtr _sdf)
{
...
    if (this->parent && this->parentEntity)
    {
      this->worldPose = this->sdf->Get<ignition::math::Pose3d>("pose") +
                        this->parentEntity->worldPose;
    }
...
}

i.e. it is equal to the initial pose of the parent model + the static transform between the parent model and the nested model. Using symbols

nested_pose_0 = parent_pose_0 + sdf_pose

Suppose now that the nested model moves relative to the parent model but the parent model does not move. Then when the physics engine updates the world pose of the parent model the method gazebo::physics::Entity::SetWorldPoseModel is called and the world pose of the nested model is updated within the for cycle in the else if (entity->HasType(MODEL)) branch as

void Entity::SetWorldPoseModel(
    const ignition::math::Pose3d &_pose,
    const bool _notify, const bool _publish)
{
  ignition::math::Pose3d oldModelWorldPose = this->worldPose;

  // initialization: (no children?) set own worldPose
  this->worldPose = _pose;
  this->worldPose.Correct();

  ...

  // force an update of all children
  // update all children pose, moving them with the model.
  // The outer loop updates all the links.
  for (Base_V::iterator iter = this->children.begin();
      iter != this->children.end(); ++iter)
  {
    if ((*iter)->HasType(ENTITY))
    {
      EntityPtr entity = boost::static_pointer_cast<Entity>(*iter);

      if (entity->HasType(LINK))
      {
        ...
      }
      else if (entity->HasType(MODEL))
      {
        // set pose of nested models
        entity->SetWorldPoseModel(
            (entity->worldPose - oldModelWorldPose) + _pose, _notify, _publish);
      }
      else
      {
        ...
      }
    }
  }
}

hence the world pose of a nested model is updated like

nested_pose_1 = (nested_pose_0 - parent_pose_0) + parent_pose_1
              =  (parent_pose_0 + sdf_pose - parent_pose_0) + parent_pose_1
              = sdf_pose + parent_pose_1
              = (since the parent model does not move) sdf_pose + parent_pose_0
              = nested_pose_0

i.e. relative motions between the nested and parent model is not considered at all.

Is the reasoning right or am I missing something?