For the XML here, Line 86 to 96: https://github.com/openai/gym/blob/.../gym/envs/robotics/assets/fetch/robot.xml#L86 I would like to calculate the distance from the site to the edge of the gripper so that I can use the gripper site xpos and an object xpos and detect whether the object is being touched by the gripper at all.
If you want to know if objects are touching, the best way to do it is to use the collision detector. You can also rely on the distance between objects of course, if you happen to know it, but computing distances in the general case can only be done by the collision detector. If you want to detect touch without generating contact force, you can use the gap parameter of contacts (set it to a large value). This will register the contact in mjData.contact but will not generate forces. You can then write code that examines mjData.contact and finds the contact you care about the checking the geom ids of each contact. Note that each contact record contains the distance between the two geoms.
I found the distances in self.sim.data.contact However, I am trying to compute the collisions in an off-policy way. I need to be able to take certain snapshots of the simulator and recombine parts of the state to recompute the collision. So, I need to save some representation of object1, object2, and have some separately callable collision function collision(object1, object2) that gives me the distance. How can I do that?
The collision detection doesn't seem to be working here. https://snag.gy/fbq1hP.jpg You can see in the above pic the block must clearly be colliding with either the right or left finger in order to be held in the air. However, no collisions are detected. My code: https://gist.github.com/richardrl/b81cc5bb9c83d5e79a8dda582f96026d
Okay I figured it out. Limit the contacts to the first ncon contacts and simply check if gripper <-> block contacts are present within the first ncon contacts. It would be really helpful if the 'active' contacts being the first ncon contacts that was mentioned in the documentation. But, I still think it's strange that my solution didn't work. Even if the contacts aren't 'active', the contact.dist distances should still be accurate right? If so, my solution should've worked