Inaccurate state setting in MJ simulation

[laikhtewari]

Type of Issue

Select the type of issue:

• Bug report (to report a bug)
• Feature request (to request an additional feature)
• Tracker (I am just using this as a tracker)
• Refactor request
• Documentation Ask

Description

While creating a mirroring simulation feature in Polymetis, a set_state method was added to all manipulator environments. To do so, the simulation is reset to the desired state / robot position and gravity compensation torques are added. A unit test was implemented to verify that the measured position and velocity matches the desired position and velocity. This unit test passes for the bullet env (within a tolerance of 1e-3), but fails for the MJ env.

Current Behavior

Setting the position of a robot in MJ env using set_state yields a relatively good position but non-zero velocity and acceleration causing the position to grow increasingly inaccurate.

Expected Behavior

Position and velocity should match with low error after setting state

Steps to reproduce

The test_mirror_env test in test_env.py illustrates the issue.