no collision between gripper and table exists in a custom gym environment using custom robot model

[erdisayar]

Ask for help

Hello,

I built a Franka Robot model in MuJoco and would like to use it like the default gym robots such as "FetchPickAndPlace, FetchPush, FetchSlide...". However, I faced some collision problems and couldn't overcome this problem for days.

As you can see from the below video, the gripper of the robot goes through the table and collision doesn't exists. [ To be able to reproduce this problem, you can download and extract the zip file and then run the script franka_pick_and_place.py

https://user-images.githubusercontent.com/44807161/187040917-289c1ebb-bf07-4f9e-8ef8-96d2acc3646b.mp4

Here are the source files franka_pick_and_place.zip

However, If I use the default FetchPickAndPlace environment, the collision prevents end-effector going through the table.

https://user-images.githubusercontent.com/44807161/187041172-dce56037-8d4a-4866-b184-fec12c8c67c6.mp4

If I start the Franka model in Mujoco (without using any python/gym scripts), then collision exists

https://user-images.githubusercontent.com/44807161/187041286-161fa172-2187-4118-8481-352e1930151e.mp4

[To reproduce the video above, plese disable/comment out the <weld> in line 109 in franka_shared.xml which you can find in the attached zip file.

The issue seems to code related (more specially mocap body related) , but I haven't figure out what causes this issue and how to fix it.

Thank you very much for your help and reply in advance.

[erez-tom]

Since you mention mocap bodies, here's a quick sanity check before we delve into the details: Did you consider that if you specify a mocap position that forces a child body into penetration, the mocap position trumps the solver and contact forces cannot prevent penetration? Even if another body is merely welded to a mocap body whose position demands penetration, the constraint resolution solver may prefer to violate penetration than violate the weld constraint (depending on solver parameters). Perhaps one of these cases applies to your model?

[erdisayar]

@erez-tom Thanks for your reply and listing the possible problems. I believe the issue is related to the one of the option you described.

I checked the every possible mocap body related codes in the script. (https://github.com/openai/gym/blob/0.15.4/gym/envs/robotics/fetch_env.py#L70)

I am using the same scripts as the openai-gym. Unfortunately, I havent pinpoint cause of this issue even if I use the same scripts to move the robot. Somehow, mocap body in the default FetchPickAndPlace is stopped moving when the gripper collides with table but the same code doesn't stop moving the mocap body for my custom robot environment. :astonished:

Note To workaround this issue, I also tried the control mocap body manually. In other words, If the mocap body is lower than some specific threshold when the robot takes action, then I fixed it in that threshold. However, the algorithm doesnt learn well with this workaround

if self.sim.data.mocap_pos[0][-1] < 0.398:
            self.sim.data.mocap_pos[0][-1] = 0.398

[nimrod-gileadi]

Apologies if this is still an issue for you. Closing since there hasn't been any activity on this issue in a while.