From ab7d96156ddf918ddd601b509b8b2df6b7644af4 Mon Sep 17 00:00:00 2001 From: Hz_Zhang <47402297+HaozheZhang6@users.noreply.github.com> Date: Fri, 12 Jun 2026 16:12:01 -0700 Subject: [PATCH 1/2] [BUG FIX] Restore the carried object's pose when plan_path fails Fixes #2715 --- genesis/utils/path_planning.py | 4 ++++ tests/test_rigid_physics.py | 37 ++++++++++++++++++++++++++++++++++ 2 files changed, 41 insertions(+) diff --git a/genesis/utils/path_planning.py b/genesis/utils/path_planning.py index b0b9fb5c7a..ffd62b557a 100644 --- a/genesis/utils/path_planning.py +++ b/genesis/utils/path_planning.py @@ -590,6 +590,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.update_object(ee_link_idx, obj_link_idx, _pos, _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 @@ -994,6 +996,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.update_object(ee_link_idx, obj_link_idx, _pos, _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 a7d44573b1..c4091d8cae 100644 --- a/tests/test_rigid_physics.py +++ b/tests/test_rigid_physics.py @@ -2992,6 +2992,43 @@ 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]) +def test_plan_path_with_entity_preserves_state_on_failure(backend, 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. + 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 + + cube_pos = cube.get_pos().clone() + cube_quat = cube.get_quat().clone() + _, valid = franka.plan_path( + qpos_goal=qpos_goal, + 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( From 4af09db2782d71cd5660e52d1f5d2c4102c013a4 Mon Sep 17 00:00:00 2001 From: haozhe Date: Mon, 15 Jun 2026 19:17:09 -0700 Subject: [PATCH 2/2] Restore the carried object by its captured world pose, covering non-current qpos_start The grasp transform is captured relative to qpos_start (the robot is moved there by get_exclude_geom_pairs before capture), so re-deriving the object pose at qpos_cur on failure teleported it whenever qpos_start differed from the current pose. Snapshot the object's world pose up front and restore it directly; extend the regression test with a custom-qpos_start case that fails on the old restore. --- genesis/utils/path_planning.py | 31 +++++++++++++++++++++++++++++-- tests/test_rigid_physics.py | 12 +++++++++++- 2 files changed, 40 insertions(+), 3 deletions(-) diff --git a/genesis/utils/path_planning.py b/genesis/utils/path_planning.py index ffd62b557a..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() @@ -591,7 +617,7 @@ def plan( else: self._entity.set_qpos(qpos_cur, zero_velocity=False) if is_plan_with_obj: - self.update_object(ee_link_idx, obj_link_idx, _pos, _quat, envs_idx) + 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 @@ -912,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() @@ -997,7 +1024,7 @@ def plan( else: self._entity.set_qpos(qpos_cur, zero_velocity=False) if is_plan_with_obj: - self.update_object(ee_link_idx, obj_link_idx, _pos, _quat, envs_idx) + 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 c4091d8cae..98d43bf2ff 100644 --- a/tests/test_rigid_physics.py +++ b/tests/test_rigid_physics.py @@ -2994,11 +2994,15 @@ def test_path_planning_avoidance(backend, n_envs, show_viewer, tol): @pytest.mark.required @pytest.mark.parametrize("backend", [gs.cpu]) -def test_plan_path_with_entity_preserves_state_on_failure(backend, tol): +@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), @@ -3013,10 +3017,16 @@ def test_plan_path_with_entity_preserves_state_on_failure(backend, tol): 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,