How to convert world contact positions to robot's local coordinate?

Question

I am implementing contact plugin on PR2. I found the contact sensor returns contact positions based on world coordinate.

Since robot can move around and my controller only accepts contact positions based of torso_lift_link coordinate, I need to convert the position from world to local. Is there any method for it?

Thanks

---

Originally posted by Daehyung Park on Gazebo Answers with karma: 47 on 2013-01-09

Post score: 0

Answer 1

Take the difference between the robot pose and the contact pose.

---

Originally posted by nkoenig with karma: 7676 on 2013-01-09

This answer was ACCEPTED on the original site

Post score: 0

---

Original comments

Comment by Daehyung Park on 2013-01-13:
I found example from ros gazebo bumper plugin. Finally, it works. Thanks.