diff --git a/genesis/utils/path_planning.py b/genesis/utils/path_planning.py index b0b9fb5c7a..428f03602e 100644 --- a/genesis/utils/path_planning.py +++ b/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) diff --git a/tests/test_rigid_physics.py b/tests/test_rigid_physics.py index feddbd84b9..1b306409f9 100644 --- a/tests/test_rigid_physics.py +++ b/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(