[BUG FIX] Restore the carried object's pose when plan_path fails

genesis/utils/path_planning.py

@@ -78,6 +78,31 @@ def update_object(self, ee_link_idx, obj_link_idx, _pos, _quat, envs_idx):
             self._solver.set_base_links_pos(trans, obj_link_idx)
             self._solver.set_base_links_quat(quat, obj_link_idx)
 
+    def get_object_pose(self, obj_link_idx, envs_idx):
+        """
+        Snapshot the carried object's world pose so a failed plan can restore it exactly.
+
+        The grasp transform returned by ``get_link_pose`` is relative to the robot at ``qpos_start``, so re-deriving
+        the object pose from it after the robot is reset to its current configuration would misplace the object
+        whenever ``qpos_start`` differs from the current pose. The planner restores this snapshot instead.
+        """
+        if self._solver.n_envs > 0:
+            pos = self._solver.get_links_pos(links_idx=obj_link_idx, envs_idx=envs_idx)
+            quat = self._solver.get_links_quat(links_idx=obj_link_idx, envs_idx=envs_idx)
+        else:
+            pos = self._solver.get_links_pos(links_idx=obj_link_idx)
+            quat = self._solver.get_links_quat(links_idx=obj_link_idx)
+        return pos.clone(), quat.clone()
+
+    def reset_object_pose(self, obj_link_idx, pos, quat, envs_idx):
+        """Restore the carried object to a world pose previously captured by ``get_object_pose``."""
+        if self._solver.n_envs > 0:
+            self._solver.set_base_links_pos(pos, obj_link_idx, envs_idx=envs_idx)
+            self._solver.set_base_links_quat(quat, obj_link_idx, envs_idx=envs_idx)
+        else:
+            self._solver.set_base_links_pos(pos, obj_link_idx)
+            self._solver.set_base_links_quat(quat, obj_link_idx)
+
     # ------------------------------------------------------------------------------------
     # ------------------------------ util funcs ------------------------------------------
     # ------------------------------------------------------------------------------------
@@ -523,6 +548,7 @@ def plan(
             obj_geom_end = obj_entity.geom_end
             obj_link_idx = obj_entity._links[0].idx
             _pos, _quat = self.get_link_pose(ee_link_idx, obj_link_idx, envs_idx)
+            obj_pos, obj_quat = self.get_object_pose(obj_link_idx, envs_idx)
 
         self._init_rrt_fields(max_nodes=max_nodes, max_step_size=resolution)
         self._reset_rrt_fields()
@@ -590,6 +616,8 @@ def plan(
                 self._entity.set_qpos(qpos_cur, envs_idx=envs_idx, zero_velocity=False)
             else:
                 self._entity.set_qpos(qpos_cur, zero_velocity=False)
+            if is_plan_with_obj:
+                self.reset_object_pose(obj_link_idx, obj_pos, obj_quat, envs_idx)
             sol = torch.zeros((num_waypoints, len(envs_idx), sol.shape[-1]), dtype=gs.tc_float, device=gs.device)
             return sol, is_invalid
 
@@ -910,6 +938,7 @@ def plan(
             obj_geom_end = obj_entity.geom_end
             obj_link_idx = obj_entity._links[0].idx
             _pos, _quat = self.get_link_pose(ee_link_idx, obj_link_idx, envs_idx)
+            obj_pos, obj_quat = self.get_object_pose(obj_link_idx, envs_idx)
 
         self._init_rrt_connect_fields(max_nodes=max_nodes, max_step_size=resolution)
         self._reset_rrt_connect_fields()
@@ -994,6 +1023,8 @@ def plan(
                 self._entity.set_qpos(qpos_cur, envs_idx=envs_idx, zero_velocity=False)
             else:
                 self._entity.set_qpos(qpos_cur, zero_velocity=False)
+            if is_plan_with_obj:
+                self.reset_object_pose(obj_link_idx, obj_pos, obj_quat, envs_idx)
             return torch.zeros(num_waypoints, len(envs_idx), sol.shape[-1], device=gs.device), is_invalid
 
         mask = rrt_connect_valid_mask(res_idx)

tests/test_rigid_physics.py

@@ -3102,6 +3102,53 @@ def test_path_planning_avoidance(backend, n_envs, show_viewer, tol):
         assert_allclose(theta, 0.0, tol=5e-3)
 
 
+@pytest.mark.required
+@pytest.mark.parametrize("backend", [gs.cpu])
+@pytest.mark.parametrize("custom_start", [False, True])
+def test_plan_path_with_entity_preserves_state_on_failure(backend, custom_start, tol):
+    # A failed plan_path(with_entity=...) must not move the attached object (#2715): the
+    # failure early-return restored the robot qpos but not the carried object. The goal
+    # below targets a pose under the floor, so the goal config is in collision and the
+    # single planning attempt fails deterministically regardless of the planner's RNG.
+    # When ``custom_start`` passes a qpos_start that differs from the robot's current pose,
+    # the carry grasp transform is captured relative to qpos_start, so restoring the object
+    # requires its captured world pose rather than re-deriving it at the current pose.
+    scene = gs.Scene(
+        sim_options=gs.options.SimOptions(dt=0.01),
+        rigid_options=gs.options.RigidOptions(box_box_detection=True),
+        show_viewer=False,
+    )
+    scene.add_entity(gs.morphs.Plane())
+    cube = scene.add_entity(gs.morphs.Box(size=(0.05, 0.05, 0.05), pos=(0.3, 0.1, 0.35)))
+    franka = scene.add_entity(gs.morphs.MJCF(file="xml/franka_emika_panda/panda.xml"))
+    scene.build()
+
+    hand = franka.get_link("hand")
+    qpos_goal = franka.inverse_kinematics(link=hand, pos=np.array([0.4, 0.0, -0.25]), quat=np.array([0, 1, 0, 0]))
+    qpos_goal[-2:] = 0.0
+
+    qpos_start = None
+    if custom_start:
+        qpos_start = franka.get_qpos().clone()
+        qpos_start[1] += 0.5  # a start config that differs from the robot's current pose
+
+    cube_pos = cube.get_pos().clone()
+    cube_quat = cube.get_quat().clone()
+    _, valid = franka.plan_path(
+        qpos_goal=qpos_goal,
+        qpos_start=qpos_start,
+        num_waypoints=50,
+        max_retry=0,
+        max_nodes=100,
+        ee_link_name="hand",
+        with_entity=cube,
+        return_valid_mask=True,
+    )
+    assert not bool(valid)  # the scenario under test is a failed plan
+    assert_allclose(cube.get_pos(), cube_pos, tol=tol)
+    assert_allclose(cube.get_quat(), cube_quat, tol=tol)
+
+
 @pytest.mark.required
 def test_all_fixed(show_viewer):
     scene = gs.Scene(