From 7ca316ab74d503847e2de0f3554f0369e1859c8c Mon Sep 17 00:00:00 2001 From: sean1295 Date: Tue, 16 Jul 2024 21:13:43 -0400 Subject: [PATCH 1/3] reset qvel when None --- mani_skill/agents/controllers/pd_joint_pos_vel.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/mani_skill/agents/controllers/pd_joint_pos_vel.py b/mani_skill/agents/controllers/pd_joint_pos_vel.py index 96890ebec..8dd336325 100644 --- a/mani_skill/agents/controllers/pd_joint_pos_vel.py +++ b/mani_skill/agents/controllers/pd_joint_pos_vel.py @@ -23,7 +23,10 @@ def _initialize_action_space(self): def reset(self): super().reset() - self._target_qvel[self.scene._reset_mask] = torch.zeros_like( + if self._target_qvel is None: + self._target_qvel = self.qvel.clone() + else: + self._target_qvel[self.scene._reset_mask] = torch.zeros_like( self._target_qpos[self.scene._reset_mask], device=self.device ) From dde6c0f967a7c4b8e81554be2979875bf315b2be Mon Sep 17 00:00:00 2001 From: sean1295 Date: Tue, 16 Jul 2024 21:39:39 -0400 Subject: [PATCH 2/3] formatting --- mani_skill/agents/controllers/pd_joint_pos_vel.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mani_skill/agents/controllers/pd_joint_pos_vel.py b/mani_skill/agents/controllers/pd_joint_pos_vel.py index 8dd336325..3bc7baff3 100644 --- a/mani_skill/agents/controllers/pd_joint_pos_vel.py +++ b/mani_skill/agents/controllers/pd_joint_pos_vel.py @@ -27,8 +27,8 @@ def reset(self): self._target_qvel = self.qvel.clone() else: self._target_qvel[self.scene._reset_mask] = torch.zeros_like( - self._target_qpos[self.scene._reset_mask], device=self.device - ) + self._target_qpos[self.scene._reset_mask], device=self.device + ) def set_drive_velocity_targets(self, targets): self.articulation.set_joint_drive_velocity_targets( From 42f1a8a9bba6c78a9000c057fd38411907d6fefd Mon Sep 17 00:00:00 2001 From: sean1295 Date: Wed, 17 Jul 2024 00:03:39 -0400 Subject: [PATCH 3/3] make motion planning to work with control_mode == pd_joint_pos_vel --- mani_skill/examples/motionplanning/panda/motionplanner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mani_skill/examples/motionplanning/panda/motionplanner.py b/mani_skill/examples/motionplanning/panda/motionplanner.py index 52fe24f6c..f28e06300 100644 --- a/mani_skill/examples/motionplanning/panda/motionplanner.py +++ b/mani_skill/examples/motionplanning/panda/motionplanner.py @@ -86,7 +86,7 @@ def follow_path(self, result, refine_steps: int = 0): for i in range(n_step + refine_steps): qpos = result["position"][min(i, n_step - 1)] if self.control_mode == "pd_joint_pos_vel": - qvel = result["velocity"][(i, n_step - 1)] + qvel = result["velocity"][min(i, n_step - 1)] action = np.hstack([qpos, qvel, self.gripper_state]) else: action = np.hstack([qpos, self.gripper_state])