diff --git a/mani_skill/agents/controllers/pd_joint_pos_vel.py b/mani_skill/agents/controllers/pd_joint_pos_vel.py
index 96890ebec..3bc7baff3 100644
--- a/mani_skill/agents/controllers/pd_joint_pos_vel.py
+++ b/mani_skill/agents/controllers/pd_joint_pos_vel.py
@@ -23,9 +23,12 @@ def _initialize_action_space(self):
 
     def reset(self):
         super().reset()
-        self._target_qvel[self.scene._reset_mask] = torch.zeros_like(
-            self._target_qpos[self.scene._reset_mask], device=self.device
-        )
+        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
+            )
 
     def set_drive_velocity_targets(self, targets):
         self.articulation.set_joint_drive_velocity_targets(
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])