Get dynamic World Pose of a link [closed]

edit

Hey guys

i am struggling to get the dynamic world pose of a link. I have written a plugin, which computes the distance to certain objects in the world. Therefore i need to know the World pose of different links in a model which is moving. To get the world Pose of a link i have done this in the OnUpdate() function:

physics::Link_V links= this->world->ModelByName("desk_0")->GetLinks();
for (physics::Link_V::iterator iter = links.begin(); iter !=links.end(); ++iter){
  if((*iter)->GetName().find(this->anchorPrefix) == 0){
      physics::LinkPtr anchor = *iter;
      ignition::math::Pose3d anchorPose = anchor->GetWorldPose();
  }
 }
RCLCPP_INFO(ros_node->get_logger(), "Anchor Pose: %s: x: %f, y: %f, z: %f", (*iter)->GetName().c_str(), anchorPose.Pos().X(), anchorPose.Pos().Y(), anchorPose.Pos().Z());

As expected, i get the world pose of the different links, but the pose is not changing if i move the model around in the world. The model is not static, only the gravity for those specific "anchor"-links is disabled. I also disabled the auto disable function (<allow_auto_disable>0</allow_auto_disable>) for the model.

I hope somebody could give me a hint what i could have done wrong or an alternative approach to get the World pose.

Thanks in advance Jannis