How to get the WorldPose of Collision Objects?
Short question:
In a custom plugin, I want to retrieve the current pose of a collision object with respect to the world frame. I use the following code for this:
physics::CollisionPtr collision;
math::Pose pose;
...
pose = collision->GetWorldPose();
When I run the simulation, I get a pose. However, the pose is always the same, even when I move the robot to which the collision object is attached to. It seems to me that the returned pose is the initial pose. How can I get the updated pose? More details:
The collision objects I get from
collision = link->GetCollision("collision_name");
When I use the link itself for getting the pose, like:
pose = link->GetWorldPose();
the pose is correctly updated, but for the link and not for the collision object. The reason why I want to have the pose of a collision object is that I added some Points Of Interests (POIs) on the robot structure with fixed joints. I want to know the pose of these POIs. Since they are linked to the robot with fixed joints, gazebo does not create extra links for them, but adds just an additional collision object as a child to the link. Hence, an extra question:
Is there a smarter way to get pose information of POIs distributed on the robot's structure?
I'm using gazebo 7.
Possibly related? https://bitbucket.org/osrf/gazebo/pull-requests/2702