Gazebo | Ignition | Community
Ask Your Question
0

Get dynamic World Pose of a link [closed]

asked 2022-03-07 11:39:38 -0500

jl gravatar image

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

edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by jl
close date 2022-03-08 04:35:14.563943

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-03-08 04:34:32 -0500

jl gravatar image

I finally found it out myself: Problem was that i didn't connected the links through fixed joints with the base link of the model. So they were not moving if i move the model.

Hope this helps someone sometime.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2022-03-07 11:39:38 -0500

Seen: 44 times

Last updated: Mar 07