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)