From 1f2a3555037b1e5e32134b5d5fe4d13a03417997 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 12 Sep 2026 08:04:36 +0000 Subject: [PATCH] fix(atomic-action): stabilize tutorial obstacle recovery Clear current articulation velocities during resets, reduce tutorial sun intensity, and prevent the animated planning obstacle from physically knocking the robot over while preserving planner collision geometry. Adjust short-path triggering and add focused regression coverage. --- embodichain/lab/sim/objects/articulation.py | 2 +- .../dynamic_obstacle_recovery.py | 50 +++++++++++++++++-- .../tutorials/atomic_action/tutorial_utils.py | 4 +- .../sim/atomic_actions/test_tutorial_utils.py | 24 ++++++++- tests/sim/objects/test_articulation.py | 20 ++++++++ 5 files changed, 94 insertions(+), 6 deletions(-) diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index 88f70be7c..1e0d86229 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -2074,7 +2074,7 @@ def clear_dynamics(self, env_ids: Sequence[int] | None = None) -> None: """ local_env_ids = self._all_indices if env_ids is None else env_ids zeros = torch.zeros((len(local_env_ids), self.dof), device=self.device) - self.set_qvel(zeros, env_ids=local_env_ids) + self.set_qvel(zeros, env_ids=local_env_ids, target=False) self.set_qvel(zeros, env_ids=local_env_ids, target=True) self.set_qf(zeros, env_ids=local_env_ids) diff --git a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py index 2a9eb3b8e..7b7ccadcb 100644 --- a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py +++ b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py @@ -78,22 +78,53 @@ SAMPLE_COUNT = 80 COMMAND_CYCLE_TIME = 0.1 COLLISION_SPHERE_FIT_DENSITY = 0.3 +# Keep the fitted sphere set unpadded: extra padding makes the initial pose +# infeasible for this compact tutorial scene. ROBOT_COLLISION_BUFFER = 0.0 MOVE_AFTER_COMMAND = 12 OBSTACLE_SIZE = (0.08, 0.08, 0.10) OBSTACLE_START_POSITION = (0.59, -0.20, 0.455) BLOCKING_PATH_FRACTION = 0.50 +OBSTACLE_TRIGGER_PATH_FRACTION = 0.10 OBSTACLE_MOVE_DURATION = 0.6 AUTO_PLAY_LEAD_IN_DURATION = 0.75 POST_EXECUTION_HOLD_DURATION = 1.0 TRACKING_ERROR_THRESHOLD = 0.1 MINIMUM_REPLAN_DETOUR = 0.04 MAXIMUM_BLOCKED_PATH_CLEARANCE = 0.0 -MINIMUM_REPLAN_CLEARANCE = 0.01 +# The replanned trajectory is sampled at control waypoints; 5 mm leaves a +# positive geometric margin without rejecting valid paths due to interpolation +# between those samples. +MINIMUM_REPLAN_CLEARANCE = 0.005 MAXIMUM_FINAL_EEF_ERROR = 0.04 TRAJECTORY_MARKER_STRIDE = 8 +def _obstacle_motion_trigger_command( + path_segment_count: int, + *, + configured_trigger: int = MOVE_AFTER_COMMAND, + path_fraction: float = BLOCKING_PATH_FRACTION, +) -> int: + """Choose an in-flight obstacle trigger for the planned path. + + The trigger is derived from the path instead of assuming that every + planner emits the same number of control commands. It remains bounded by + the tutorial's configured trigger so longer trajectories keep the original + pacing, while short trajectories still move the obstacle before completion. + """ + if path_segment_count < 1: + raise ValueError("path_segment_count must be at least one.") + if configured_trigger < 1: + raise ValueError("configured_trigger must be at least one.") + if not math.isfinite(path_fraction) or not 0.0 < path_fraction < 1.0: + raise ValueError("path_fraction must be finite and lie in (0, 1).") + return min( + configured_trigger, + max(1, round(path_segment_count * path_fraction)), + ) + + def _animate_obstacle_to_pose( obstacle: RigidObject, adapter: SimulationExecutionAdapter, @@ -423,7 +454,11 @@ def main() -> None: roughness=0.35, ), ), - attrs=RigidBodyAttributesCfg(), + # The obstacle remains in cuRobo's collision world, but its visual + # animation must not generate a physical impulse that knocks the + # robot out of its planned trajectory before replanning observes + # the scene revision. + attrs=RigidBodyAttributesCfg(enable_collision=False), body_type="kinematic", init_pos=list(OBSTACLE_START_POSITION), init_rot=[0.0, 0.0, 0.0], @@ -498,6 +533,15 @@ def main() -> None: session.active_commands, control_part=CONTROL_PART, ) + move_after_command = _obstacle_motion_trigger_command( + initial_eef_path.shape[1] - 1, + path_fraction=OBSTACLE_TRIGGER_PATH_FRACTION, + ) + logger.log_info( + "Obstacle motion trigger set to " + f"command {move_after_command} for {initial_eef_path.shape[1] - 1} " + "planned path segments." + ) blocking_obstacle_pose, blocking_waypoint_index = _blocking_obstacle_pose( obstacle.get_local_pose(to_matrix=True), initial_eef_path, @@ -568,7 +612,7 @@ def on_step(step: RunnerStep) -> None: if ( not args.no_obstacle_motion and not obstacle_moved - and step.command_count >= MOVE_AFTER_COMMAND + and step.command_count >= move_after_command ): start_pose = obstacle.get_local_pose(to_matrix=True).clone() logger.log_warning( diff --git a/scripts/tutorials/atomic_action/tutorial_utils.py b/scripts/tutorials/atomic_action/tutorial_utils.py index 2791acc42..910db9091 100644 --- a/scripts/tutorials/atomic_action/tutorial_utils.py +++ b/scripts/tutorials/atomic_action/tutorial_utils.py @@ -95,6 +95,7 @@ ) DEFAULT_GRIPPER_CLOSE_QPOS = 0.036 DEFAULT_TUTORIAL_SUN_DIRECTION = (0.0, 0.0, -1.0) +DEFAULT_TUTORIAL_SUN_INTENSITY = 5.0 _FRANKA_TUTORIAL_BASE_ROTATION = (0.0, 0.0, 180.0) _DEFAULT_GRIPPER_TCP_Z = 0.17 _GRIPPER_TCP = ( @@ -256,7 +257,7 @@ def create_tutorial_simulation( uid="main_light", light_type="sun", color=(0.6, 0.6, 0.6), - intensity=30.0, + intensity=DEFAULT_TUTORIAL_SUN_INTENSITY, direction=tuple(sun_direction), ) ) @@ -1227,6 +1228,7 @@ def create_tutorial_robot_cfg( "DEFAULT_AXIS_SIZE", "DEFAULT_GRIPPER_CLOSE_QPOS", "DEFAULT_TUTORIAL_SUN_DIRECTION", + "DEFAULT_TUTORIAL_SUN_INTENSITY", "GRIPPER_HAND_JOINT_PATTERN", "GRIPPER_URDF_PATH", "ROBOTIQ_2F_140_TCP", diff --git a/tests/sim/atomic_actions/test_tutorial_utils.py b/tests/sim/atomic_actions/test_tutorial_utils.py index 88bd4a33c..fa63120d6 100644 --- a/tests/sim/atomic_actions/test_tutorial_utils.py +++ b/tests/sim/atomic_actions/test_tutorial_utils.py @@ -40,6 +40,7 @@ _blocking_obstacle_pose, _maximum_path_deviation, _minimum_cuboid_clearance, + _obstacle_motion_trigger_command, ) from scripts.tutorials.atomic_action.coordinated_pickment import ( compute_left_to_right_arm_direction, @@ -49,6 +50,7 @@ ) from scripts.tutorials.atomic_action.tutorial_utils import ( DEFAULT_TUTORIAL_SUN_DIRECTION, + DEFAULT_TUTORIAL_SUN_INTENSITY, ROBOTIQ_2F_140_TCP, ROBOTIQ_HAND_JOINT_PATTERN, TUTORIAL_PLANNERS, @@ -80,7 +82,7 @@ CUBOID_SIZE = (0.2, 0.2, 0.2) STRICT_RECOVERY_TRACKING_ERROR = 0.1 STRICT_RECOVERY_SPHERE_DENSITY = 0.3 -STRICT_RECOVERY_MINIMUM_CLEARANCE = 0.01 +STRICT_RECOVERY_MINIMUM_CLEARANCE = 0.005 FRANKA_TUTORIAL_BASE_ROTATION = (0.0, 0.0, 180.0) DUAL_FRANKA_MOUNT_X_AXIS = torch.tensor([0.0, -1.0, 0.0]) UR_RUNTIME_QPOS_LIMITS = torch.tensor([[-2.0 * math.pi, 2.0 * math.pi]] * 6) @@ -766,10 +768,18 @@ def test_tutorial_simulation_uses_one_global_sun_light() -> None: light_kwargs = light_cfg.call_args.kwargs assert light_kwargs["uid"] == "main_light" assert light_kwargs["light_type"] == "sun" + assert light_kwargs["intensity"] == DEFAULT_TUTORIAL_SUN_INTENSITY assert light_kwargs["direction"] == DEFAULT_TUTORIAL_SUN_DIRECTION assert "init_pos" not in light_kwargs +@pytest.mark.parametrize("robot_type", ("ur5", "franka", "ur10")) +def test_tutorial_robot_configs_keep_gravity_enabled(robot_type: str) -> None: + cfg = create_tutorial_robot_cfg(robot_type) + + assert cfg.enable_gravity is True + + def test_shared_robot_selection_keeps_ur5_default_and_accepts_all_variants() -> None: parser = create_tutorial_argument_parser("test parser") default_args = parser.parse_args([]) @@ -1047,10 +1057,22 @@ def test_dynamic_obstacle_recovery_keeps_strict_collision_contract() -> None: assert "fit_type=" not in main_source assert "sphere_density=COLLISION_SPHERE_FIT_DENSITY" in main_source assert "collision_sphere_buffer=ROBOT_COLLISION_BUFFER" in main_source + assert "RigidBodyAttributesCfg(enable_collision=False)" in main_source assert "blocked_path_clearance > MAXIMUM_BLOCKED_PATH_CLEARANCE" in main_source assert "replan_clearance < MINIMUM_REPLAN_CLEARANCE" in main_source +def test_dynamic_obstacle_trigger_scales_down_for_short_paths() -> None: + assert _obstacle_motion_trigger_command(10, path_fraction=0.10) == 1 + assert _obstacle_motion_trigger_command(3, path_fraction=0.10) == 1 + assert _obstacle_motion_trigger_command(40) == 12 + + +def test_dynamic_obstacle_trigger_rejects_invalid_paths() -> None: + with pytest.raises(ValueError, match="path_segment_count"): + _obstacle_motion_trigger_command(0) + + def test_maximum_path_deviation_measures_detour_from_reference_polyline() -> None: reference_path = torch.tensor([[[0.0, 0.0, 0.0], [0.5, 0.0, 0.0], [1.0, 0.0, 0.0]]]) detour_path = torch.tensor([[[0.0, 0.0, 0.0], [0.5, 0.2, 0.0], [1.0, 0.0, 0.0]]]) diff --git a/tests/sim/objects/test_articulation.py b/tests/sim/objects/test_articulation.py index 0471f8119..4cdeaa4b3 100644 --- a/tests/sim/objects/test_articulation.py +++ b/tests/sim/objects/test_articulation.py @@ -85,6 +85,26 @@ def test_set_gravity_updates_all_environments_by_default() -> None: ] +@pytest.mark.no_sim +def test_clear_dynamics_clears_current_and_target_joint_velocities() -> None: + """Clearing dynamics resets both velocity state and drive targets.""" + articulation = object.__new__(Articulation) + articulation._all_indices = torch.arange(2, dtype=torch.int32) + articulation._data = SimpleNamespace(dof=3) + articulation.device = torch.device("cpu") + articulation.set_qvel = MagicMock() + articulation.set_qf = MagicMock() + + articulation.clear_dynamics() + + assert [call.kwargs["target"] for call in articulation.set_qvel.call_args_list] == [ + False, + True, + ] + for call in articulation.set_qvel.call_args_list: + assert torch.equal(call.args[0], torch.zeros((2, 3))) + + def test_get_qf_returns_all_articulation_joint_efforts(): expected_qf = torch.tensor([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]], dtype=torch.float32) articulation = object.__new__(Articulation)