Fail to completely execute the desirable action in sapien env using mplib.Planner

[piao-0429]

System: OS version: Ubuntu 20.04 Python version (if applicable): Python 3.8 SAPIEN version (pip freeze | grep sapien): 2.2.2 Environment: Desktop

Describe the bug I create an env with sapien but when I execute a planned action with env.step(action), where action is the target pose of the end effector (8-dim: 3 for pos, 4 for quat, 1 for gripper), it cannot reach my desired pose (but it manages to move in the correct direction). I guess it may be some problem with my simulation time or planning timestep, etc. But I cannot discover how to fix this problem.

Additional context Some reference code here:

self._sim_freq = 500
self._control_freq = 100
self._sim_steps_per_control = self._sim_freq // self._control_freq
self.dt = 1.0 / (self._control_freq / self._sim_freq)

    def step(self, action): 
        self.current_step += 1
        self.success_planned = self.move_to_pose(action)

    def move_to_pose(self, action):
        pose = action[:-1]   
        open_gripper = action[-1]  
        self.plan_timestep = 0.1
        result = self.planner.plan_screw(pose, self.agent.get_qpos(), time_step=self.plan_timestep) # mplib.Planner
        if result["status"] != "Success":
            self.scene.step()
            return 1
        self.follow_path(result)
        return 0

    def follow_path(self, result):
        n_step = result["position"].shape[0]
        assert n_step > 0, "no path"
        min_step = int(self._sim_freq * self.plan_timestep * 2)
        for i in range(min(min_step, n_step)):
            for j in range(7):
                self.active_joints[j].set_drive_target(result["position"][i][j])
                self.active_joints[j].set_drive_velocity_target(result["velocity"][i][j])
            self.scene.step()

[Lexseal]

Not an expert on this issue. But have you tried a smaller planning timestep like 1/250? ref

If that doesn't work, maybe @fbxiang can help

[piao-0429]

It seems not work as well.

[Lexseal]

I just pinged fanbo and he can probably give you a better answer. Meanwhile, can you attach the trajectory in the result returned by plan? This does sound like a simulation issue

[piao-0429]

result['position'] result['time'] I have attached two txt files above. The result is based on self.plan_timestep = 1/250 Thanks!

[fbxiang]

Are you balancing passive forces as describe in the tutorial here https://sapien.ucsd.edu/docs/2.2/tutorial/robotics/basic_robot.html#compensate-passive-forces-e-g-gravity ?

If the forces are balanced, the issue may be the robot reached certain join limit that you should be able to check in the viewer.

[piao-0429]

I've fixed this problem by balancing passive force! Thanks for your help!

[Lexseal]

Thanks for the follow-up! Closing for now.