From 7f3e1efa518cc652a37f3e1567fa034ab572158c Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 12 Sep 2026 04:02:34 +0000 Subject: [PATCH 1/3] feat(atomic-actions): select motion planners in tutorials Expose TOPPRA, trapezoidal, and cuRobo selection across the runnable atomic-action tutorials while keeping NeuralPlanner out of the shared selector. Integrate trapezoidal native timing with composite action phases and update the corresponding tutorials, planner docs, tests, and project context. --- agent_context/MAP.yaml | 2 +- .../topics/atomic-actions/execution.md | 6 +- .../topics/motion-planning/motion-planning.md | 6 +- .../topics/motion-planning/planner-details.md | 5 + .../sim/atomic_actions/builtin_actions.md | 4 + .../overview/sim/motion/motion_generator.md | 6 +- .../motion/planners/trapezoidal_planner.md | 4 + docs/source/tutorial/atomic_actions.rst | 60 +++++ docs/source/tutorial/motion_gen.rst | 5 +- embodichain/compute/trajectory/resampling.py | 213 ++++++++++++------ .../sim/atomic_actions/primitives/_helpers.py | 59 ++++- .../atomic_actions/primitives/axis_align.py | 5 +- .../atomic_actions/primitives/open_door.py | 9 +- .../sim/atomic_actions/primitives/pick_up.py | 3 + .../sim/atomic_actions/primitives/place.py | 3 + .../sim/atomic_actions/primitives/press.py | 9 +- .../atomic_actions/primitives/push_object.py | 9 +- .../sim/atomic_actions/primitives/slide.py | 9 +- .../sim/atomic_actions/primitives/twist.py | 9 +- .../lab/sim/motion/motion_generator.py | 22 +- .../lab/sim/motion/planners/base_planner.py | 12 +- scripts/tutorials/atomic_action/assemble.py | 6 +- scripts/tutorials/atomic_action/axis_align.py | 5 +- scripts/tutorials/atomic_action/control_dt.py | 7 +- .../atomic_action/coordinated_pickment.py | 5 +- .../atomic_action/coordinated_placement.py | 5 +- .../dynamic_obstacle_recovery.py | 8 +- scripts/tutorials/atomic_action/hand_over.py | 5 +- .../atomic_action/move_end_effector.py | 6 +- .../atomic_action/move_held_object.py | 6 +- .../tutorials/atomic_action/move_joints.py | 6 +- .../atomic_action/moving_target_recovery.py | 8 +- scripts/tutorials/atomic_action/open_door.py | 10 +- scripts/tutorials/atomic_action/pickup.py | 6 +- scripts/tutorials/atomic_action/place.py | 6 +- scripts/tutorials/atomic_action/pour.py | 32 ++- scripts/tutorials/atomic_action/press.py | 10 +- scripts/tutorials/atomic_action/slide.py | 10 +- .../tutorials/atomic_action/tutorial_utils.py | 131 +++++++++-- scripts/tutorials/atomic_action/twist.py | 10 +- tests/compute/test_trajectory.py | 16 ++ tests/sim/atomic_actions/test_actions.py | 14 +- .../atomic_actions/test_primitives_helpers.py | 14 ++ .../sim/atomic_actions/test_tutorial_utils.py | 46 +++- .../motion/test_motion_generator_batched.py | 43 +++- 45 files changed, 735 insertions(+), 140 deletions(-) diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index e3d4dfa5d..dcf16677d 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -144,7 +144,7 @@ topics: aliases: [motion planning, trajectory planning, trajectory playback, fixed-cadence trajectory playback, motion expansion, trajectory expansion, trajectory augmentation, fixed scene trajectory augmentation, 轨迹播放, 轨迹扩增, 运动规划, 轨迹规划] - keywords: [BasePlanner, PlanState, PlanResult, MotionGenerator, ToppraPlanner, CuroboPlanner, NeuralPlanner, + keywords: [BasePlanner, PlanState, PlanResult, MotionGenerator, ToppraPlanner, TrapezoidalPlanner, CuroboPlanner, NeuralPlanner, expansion, GenerationSession, TrajectoryAugmentationCfg, CandidateTrajectoryBatch, collision world, compute trajectory, trajectory resampling, trajectory warping, retime_to_control_grid, JointTrajectoryPlaybackCfg, play_joint_trajectory] diff --git a/agent_context/topics/atomic-actions/execution.md b/agent_context/topics/atomic-actions/execution.md index 94641a7ca..413722a7c 100644 --- a/agent_context/topics/atomic-actions/execution.md +++ b/agent_context/topics/atomic-actions/execution.md @@ -112,7 +112,11 @@ resample fractional durations. Primitive planner results are explicitly retimed before their controlled-joint paths are embedded into a full-robot `TimedTrajectory`. Off-grid duration rounds up to a whole control interval, qvel is recomputed from the executed samples, -and now-invalid native acceleration samples are discarded. +and now-invalid native acceleration samples are discarded. Composite primitives +that combine planner output with fixed-length hand or multi-part phases may +also resample the planner's position path to the phase count before assembly; +the final composite trajectory then derives its velocity targets from the +assembled positions and timing. Tracking recovery is separate from task-level semantic recovery: diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index 6295dc6a9..a79c1e9f4 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -69,7 +69,8 @@ Focused augmentation tests live under `tests/sim/motion/expansion/`. ## Choose the owning layer - `BasePlanner` and `PlanState` / `PlanResult` define planning interfaces. -- `ToppraPlanner` owns time parameterization; `CuroboPlanner` owns collision-aware planning. +- `ToppraPlanner` and `TrapezoidalPlanner` own joint-path time parameterization; + `CuroboPlanner` owns collision-aware planning. - `MotionGenerator` composes motion commands and trajectory helpers; `NeuralPlanner` is experimental. - [Planner details](planner-details.md) cover process/memory behavior, registration and validation. - [Collision worlds](collision-worlds.md) cover snapshots, pose updates, provenance and cache boundaries. @@ -198,7 +199,8 @@ retains compatibility aliases; compute does not import simulation modules. `embodichain.compute.trajectory` owns pure interpolation, path resampling, time-domain differentiation/resampling, and keyframe-based warping. `interpolate_with_distance` retains keyframes; -`resample_with_distance` treats interior points as optional path samples. +`resample_with_distance` treats interior points as optional path samples and +falls back to pure Torch when the Warp runtime cannot launch. MotionGenerator and atomic trajectory helpers import the compute API directly. `lab.sim.utility.action_utils` retains solver-dependent pose/IK adaptation and re-exports pure functions for compatibility. Warp implementations live in diff --git a/agent_context/topics/motion-planning/planner-details.md b/agent_context/topics/motion-planning/planner-details.md index 9ebd14ac1..78cb45585 100644 --- a/agent_context/topics/motion-planning/planner-details.md +++ b/agent_context/topics/motion-planning/planner-details.md @@ -38,6 +38,11 @@ waypoints, with optional quintic corner blending. `TIME` sampling pads shorter batch rows at their exact final position with zero velocity and acceleration, starting at each row's actual endpoint. Padding has zero arrival intervals. +When dispatched through `MotionGenerator`, `TrapezoidalPlanner` owns sparse +joint-goal timing: the facade prepends the observed `start_qpos` when the +caller supplies one and preserves the planner's native samples and +derivatives. + With `stop_at_waypoints=False`, straight runs are compressed using normalized edge directions and a cosine tolerance relative to the first edge of each retained run. Small waypoint spacing does not change the angular test, and diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index 40addd658..c06664132 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -1063,5 +1063,9 @@ python scripts/tutorials/atomic_action/pickup.py --headless --auto_play --device python scripts/tutorials/atomic_action/hand_over.py --headless --auto_play --device cpu ``` +The scripts share a `--planner` selector for the non-neural backends +(`toppra`, `trapezoidal`, or `curobo`). See {doc}`/tutorial/atomic_actions` for +backend-specific caveats and examples. + See {doc}`/tutorial/atomic_actions` for engine setup, static compilation, closed-loop execution, effect verification, and custom-action guidance. diff --git a/docs/source/overview/sim/motion/motion_generator.md b/docs/source/overview/sim/motion/motion_generator.md index 4f8534fc9..7cff019d4 100644 --- a/docs/source/overview/sim/motion/motion_generator.md +++ b/docs/source/overview/sim/motion/motion_generator.md @@ -44,7 +44,7 @@ The built-in declarations are: * TOPPRA: `JOINT_MOVE`; * TrapezoidalPlanner: `JOINT_MOVE`, sparse joint waypoints, and preserved native - samples; + samples. During quantity sampling it retains every converted waypoint; * NeuralPlanner: `EEF_MOVE`; * cuRobo: `EEF_MOVE` and `JOINT_MOVE`. @@ -54,6 +54,10 @@ TrapezoidalPlanner declares both `uses_sparse_joint_waypoints=True` and returns the planner's native `positions`, `velocities`, `accelerations`, and `dt` without normalizing them to `MotionGenOptions.sample_count`. See the [TrapezoidalPlanner guide](planners/trapezoidal_planner.md). +When options are automatically resolved from a backend-neutral request, a +requested quantity is treated as a lower bound if Cartesian-to-joint conversion +produces more waypoints; an explicit `TrapezoidalPlanOptions.sample_interval` +remains authoritative. ## Usage diff --git a/docs/source/overview/sim/motion/planners/trapezoidal_planner.md b/docs/source/overview/sim/motion/planners/trapezoidal_planner.md index ca9e26d68..ec6d7062b 100644 --- a/docs/source/overview/sim/motion/planners/trapezoidal_planner.md +++ b/docs/source/overview/sim/motion/planners/trapezoidal_planner.md @@ -87,6 +87,10 @@ start, and does not run generic joint pre-interpolation. It also preserves the planner's native sample grid and its analytical velocity and acceleration outputs. `MotionGenOptions.sample_count` therefore does not replace an explicit `TrapezoidalPlanOptions.sample_interval`. +When options are automatically resolved from a backend-neutral request, the +requested quantity is treated as a lower bound if Cartesian-to-joint conversion +produces more required waypoints; an explicit +`TrapezoidalPlanOptions.sample_interval` remains authoritative. Every successful result with positions contains: diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index 5e6e1ea3e..ce7300f97 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -159,6 +159,66 @@ The ``motion_generator`` variable in the snippets below is a configured :class:`~embodichain.lab.sim.motion.motion_generator.MotionGenerator`; its robot, planner, device, cache, and collision world become the resources owned by the engine. +Selecting a motion planner +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Every runnable atomic-action tutorial accepts the shared ``--planner`` switch. +It selects the non-neural backend used to construct its +``MotionGenerator``: + +.. list-table:: + :header-rows: 1 + :widths: 18 30 52 + + * - Value + - Backend + - Behavior and availability + * - ``toppra`` + - :class:`~embodichain.lab.sim.motion.planners.ToppraPlanner` + - Time-optimal joint-space timing. This remains the default for tutorials + that previously used TOPPRA. + * - ``trapezoidal`` + - :class:`~embodichain.lab.sim.motion.planners.TrapezoidalPlanner` + - Deterministic trapezoidal joint-space timing. Cartesian targets are + converted through the shared IK/interpolation path before planning; + quantity sampling retains every converted waypoint. When options are + resolved from the backend-neutral request, ``MotionGenerator`` treats + the requested count as a lower bound; an explicitly supplied + ``TrapezoidalPlanOptions.sample_interval`` remains authoritative. + Fixed-length composite skill phases resample that position path back to + their requested phase count before combining arm and hand commands. + * - ``curobo`` + - :class:`~embodichain.lab.sim.motion.planners.CuroboPlanner` + - CUDA-backed Cartesian/joint planning with collision-world support when + the tutorial supplies one. This remains the default for the tutorials + that previously used cuRobo. + +``NeuralPlanner`` is intentionally not a choice here: it requires a +tutorial-specific ONNX model and frame configuration rather than being a +drop-in backend for these examples. For example, the same pose tutorial can +be run with each supported backend as follows: + +.. code-block:: bash + + python scripts/tutorials/atomic_action/move_end_effector.py --device cpu --planner toppra + python scripts/tutorials/atomic_action/move_end_effector.py --device cpu --planner trapezoidal + python scripts/tutorials/atomic_action/move_end_effector.py --device cuda --planner curobo + +The selector does not override a skill's explicit motion contract. In +particular, ``dynamic_obstacle_recovery.py`` is deliberately cuRobo-only because +it updates a live collision world; ``coordinated_pickment.py``, +``coordinated_placement.py``, and ``hand_over.py`` use dual-arm planning that +currently rejects cuRobo. ``control_dt.py`` intentionally uses +``strategy="ik_interp"`` to compare control periods, so its planner choice is +constructed for consistency but does not change that interpolation experiment. +``coordinated_pickment.py`` likewise retains its synchronized multi-arm +IK/keyframe implementation; its selector validates the backend but does not +replace those custom synchronized phases. +The exact Cartesian-linear portions of ``OpenDoor``, ``Press``, and ``Slide`` +likewise remain IK-grounded; their planner choice applies to the other +motion-generation portions. ``Twist`` uses the selected planner for each of +its pose segments. + Control-part commands --------------------- diff --git a/docs/source/tutorial/motion_gen.rst b/docs/source/tutorial/motion_gen.rst index 314034d5b..51b65fddf 100644 --- a/docs/source/tutorial/motion_gen.rst +++ b/docs/source/tutorial/motion_gen.rst @@ -5,7 +5,7 @@ Motion Generator .. currentmodule:: embodichain.lab.sim.motion.motion_generator -The ``MotionGenerator`` class in EmbodiChain provides a unified and extensible interface for robot trajectory planning. It supports time-optimal trajectory generation (currently via TOPPRA), joint/Cartesian interpolation, and is designed for easy integration with RL, imitation learning, and classical control scenarios. +The ``MotionGenerator`` class in EmbodiChain provides a unified and extensible interface for robot trajectory planning. It supports time-optimal trajectory generation via TOPPRA, deterministic trapezoidal joint-space timing, joint/Cartesian interpolation, and is designed for easy integration with RL, imitation learning, and classical control scenarios. Key Features ------------ @@ -193,7 +193,8 @@ API Reference Notes & Best Practices ~~~~~~~~~~~~~~~~~~~~~~ -- TOPPRA and NeuralPlanner do not maintain a collision world. Select the optional +- TOPPRA, TrapezoidalPlanner, and NeuralPlanner do not maintain a collision + world. Select the optional cuRobo V2 backend for collision-aware planning and exact joint-trajectory collision validation; see :doc:`/overview/sim/motion/planners/curobo_planner`. - Planning inputs and outputs use environment-batched PyTorch tensors. diff --git a/embodichain/compute/trajectory/resampling.py b/embodichain/compute/trajectory/resampling.py index 70eaa8285..e629f55ea 100644 --- a/embodichain/compute/trajectory/resampling.py +++ b/embodichain/compute/trajectory/resampling.py @@ -32,6 +32,74 @@ __all__ = ["resample_with_distance"] +def _resample_with_distance_torch( + trajectory: torch.Tensor, + sample_count: int, +) -> torch.Tensor: + """Resample a normalized path with pure Torch operations. + + This is the reference fallback used when the Warp runtime is unavailable + or cannot launch its kernels. ``trajectory`` is already validated and + normalized to a floating-point tensor by :func:`resample_with_distance`. + """ + batch_size, point_count, dimension = trajectory.shape + if batch_size == 0 or sample_count == 0: + return trajectory.new_empty((batch_size, sample_count, dimension)) + if sample_count == 1: + return trajectory[:, :1].clone() + if point_count == 1: + return trajectory.expand(-1, sample_count, -1).clone() + + segment_lengths = torch.linalg.vector_norm( + trajectory[:, 1:] - trajectory[:, :-1], dim=-1 + ) + cumulative = torch.cat( + [ + torch.zeros( + (batch_size, 1), dtype=trajectory.dtype, device=trajectory.device + ), + segment_lengths.cumsum(dim=1), + ], + dim=1, + ) + total_length = cumulative[:, -1:] + fractions = torch.linspace( + 0.0, + 1.0, + sample_count, + dtype=trajectory.dtype, + device=trajectory.device, + ) + distances = total_length * fractions[None, :] + upper_index = torch.searchsorted( + cumulative.contiguous(), distances.contiguous(), right=True + ).clamp_(1, point_count - 1) + lower_index = upper_index - 1 + lower_distance = cumulative.gather(1, lower_index) + upper_distance = cumulative.gather(1, upper_index) + denominator = upper_distance - lower_distance + safe_denominator = denominator.clamp_min(torch.finfo(trajectory.dtype).eps) + alpha = torch.where( + denominator > 0.0, + (distances - lower_distance) / safe_denominator, + torch.zeros_like(distances), + ) + lower = trajectory.gather( + 1, + lower_index.unsqueeze(-1).expand(-1, -1, dimension), + ) + upper = trajectory.gather( + 1, + upper_index.unsqueeze(-1).expand(-1, -1, dimension), + ) + result = torch.lerp(lower, upper, alpha.unsqueeze(-1)) + # Keep boundaries exact, including for paths whose final segment has zero + # length or whose cumulative distance is rounded in low precision. + result[:, 0] = trajectory[:, 0] + result[:, -1] = trajectory[:, -1] + return result + + def resample_with_distance( trajectory: torch.Tensor, interp_num: int, @@ -44,6 +112,10 @@ def resample_with_distance( downsampling. It is intended for dense planner paths rather than required waypoint sequences. + The Warp implementation is used when available. A pure Torch reference + path is used automatically when Warp has not been initialized, which keeps + CPU-only callers and lightweight action tests functional. + Args: trajectory: Path tensor with shape ``(B, N, M)``. interp_num: Target number of samples ``T``. @@ -74,69 +146,82 @@ def resample_with_distance( if batch_size == 0 or sample_count == 0: return trajectory.new_empty((batch_size, sample_count, dimension)) - # Flatten input trajectory for Warp kernels (avoids multidimensional - # wp.array interop issues). - trajectory_flat = trajectory.view(-1) - points = wp.from_torch(trajectory_flat) - - out = wp.empty( - (batch_size * sample_count * dimension,), - dtype=wp.float32, - device=standardize_device_string(device), - ) - - if point_count == 1: - wp.launch( - kernel=repeat_first_point, - dim=batch_size * sample_count, - inputs=[ - points, - out, - batch_size, - sample_count, - dimension, - point_count, - ], - device=standardize_device_string(device), + try: + # Flatten input trajectory for Warp kernels (avoids multidimensional + # wp.array interop issues). + trajectory_flat = trajectory.view(-1) + points = wp.from_torch(trajectory_flat) + warp_device = standardize_device_string(device) + + out = wp.empty( + (batch_size * sample_count * dimension,), + dtype=wp.float32, + device=warp_device, ) - return wp.to_torch(out).view(batch_size, sample_count, dimension) - dists = wp.empty( - (batch_size * (point_count - 1),), - dtype=wp.float32, - device=standardize_device_string(device), - ) - wp.launch( - kernel=pairwise_distances, - dim=batch_size * (point_count - 1), - inputs=[points, dists, batch_size, point_count, dimension], - device=standardize_device_string(device), - ) - - cumulative = wp.empty( - (batch_size * point_count,), - dtype=wp.float32, - device=standardize_device_string(device), - ) - wp.launch( - kernel=cumsum_distances, - dim=batch_size, - inputs=[dists, cumulative, batch_size, point_count], - device=standardize_device_string(device), - ) - - wp.launch( - kernel=interpolate_along_distance, - dim=batch_size * sample_count, - inputs=[ - points, - cumulative, - out, - batch_size, - point_count, - dimension, - sample_count, - ], - device=standardize_device_string(device), - ) - return wp.to_torch(out).view(batch_size, sample_count, dimension) + if point_count == 1: + wp.launch( + kernel=repeat_first_point, + dim=batch_size * sample_count, + inputs=[ + points, + out, + batch_size, + sample_count, + dimension, + point_count, + ], + device=warp_device, + ) + result = wp.to_torch(out).view(batch_size, sample_count, dimension) + else: + dists = wp.empty( + (batch_size * (point_count - 1),), + dtype=wp.float32, + device=warp_device, + ) + wp.launch( + kernel=pairwise_distances, + dim=batch_size * (point_count - 1), + inputs=[points, dists, batch_size, point_count, dimension], + device=warp_device, + ) + + cumulative = wp.empty( + (batch_size * point_count,), + dtype=wp.float32, + device=warp_device, + ) + wp.launch( + kernel=cumsum_distances, + dim=batch_size, + inputs=[dists, cumulative, batch_size, point_count], + device=warp_device, + ) + + wp.launch( + kernel=interpolate_along_distance, + dim=batch_size * sample_count, + inputs=[ + points, + cumulative, + out, + batch_size, + point_count, + dimension, + sample_count, + ], + device=warp_device, + ) + result = wp.to_torch(out).view(batch_size, sample_count, dimension) + except (AttributeError, RuntimeError): + result = _resample_with_distance_torch(trajectory, sample_count) + + # Warp uses float32 storage; preserve exact boundaries and the normalized + # output dtype for callers that supplied another floating-point type. + result = result.to(dtype=trajectory.dtype).clone() + if sample_count > 0: + result[:, 0] = trajectory[:, 0] + if sample_count > 1: + result[:, -1] = trajectory[:, -1] + return result diff --git a/embodichain/lab/sim/atomic_actions/primitives/_helpers.py b/embodichain/lab/sim/atomic_actions/primitives/_helpers.py index 4574d28ee..73768172f 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/_helpers.py +++ b/embodichain/lab/sim/atomic_actions/primitives/_helpers.py @@ -95,6 +95,62 @@ def repeat_qpos(qpos: torch.Tensor, n_waypoints: int) -> torch.Tensor: return qpos.unsqueeze(1).repeat(1, n_waypoints, 1) +def resample_planned_trajectory( + trajectory: torch.Tensor, + sample_count: int, +) -> torch.Tensor: + """Fit a planner path to a composite action's fixed phase length. + + Some planners preserve their native samples instead of honoring the + backend-neutral requested count. Composite atomic actions allocate arm and + hand phases together, so they need a common count at this boundary. The + position-only resampling intentionally happens after planning; native + planner derivatives are not used by these position-composed phases. + + Args: + trajectory: Batched joint path with shape ``(B, N, D)``. + sample_count: Required number of output samples. + + Returns: + The original path when its length already matches, otherwise a + cumulative-distance resampling with exactly ``sample_count`` samples. + + Raises: + TypeError: If ``trajectory`` is not a floating-point tensor. + ValueError: If the path or requested sample count is invalid. + """ + if not isinstance(trajectory, torch.Tensor): + raise TypeError("trajectory must be a torch.Tensor.") + if trajectory.ndim != 3: + raise ValueError("trajectory must have shape (B, N, D).") + if not torch.is_floating_point(trajectory): + raise TypeError("trajectory must be a floating-point tensor.") + if trajectory.shape[1] == 0 or trajectory.shape[2] == 0: + raise ValueError("trajectory must have non-zero N and D dimensions.") + if isinstance(sample_count, bool) or not isinstance(sample_count, int): + raise TypeError("sample_count must be an integer.") + if sample_count < 1: + raise ValueError("sample_count must be positive.") + if trajectory.shape[0] == 0: + return trajectory.new_empty((0, sample_count, trajectory.shape[2])) + if trajectory.shape[1] == sample_count: + return trajectory + if trajectory.shape[1] == 1: + return trajectory.expand(-1, sample_count, -1).clone() + resampled = resample_with_distance( + trajectory, + sample_count, + device=trajectory.device, + ) + # Preserve the planner's exact boundary states even when the Warp path + # performs its arithmetic in float32 or the Torch fallback rounds a + # cumulative-distance fraction at an endpoint. + resampled = resampled.to(dtype=trajectory.dtype).clone() + resampled[:, 0] = trajectory[:, 0] + resampled[:, -1] = trajectory[:, -1] + return resampled + + def assemble_full_robot_trajectory( base_qpos: torch.Tensor, part_trajectories: Sequence[tuple[Sequence[int], torch.Tensor]], @@ -138,7 +194,7 @@ def plan_named_arm_trajectory( raise TypeError("Motion planning success must be a torch.Tensor.") if result.positions is None: raise ValueError("Motion planning result must contain joint positions.") - return result.success, result.positions + return result.success, resample_planned_trajectory(result.positions, n_waypoints) def arm_qpos_from_state( @@ -240,6 +296,7 @@ def split_joint_trajectory_at_pose( "arm_qpos_from_state", "assemble_full_robot_trajectory", "plan_named_arm_trajectory", + "resample_planned_trajectory", "require_shared_task_state_key", "repeat_qpos", "resolve_batched_pose", diff --git a/embodichain/lab/sim/atomic_actions/primitives/axis_align.py b/embodichain/lab/sim/atomic_actions/primitives/axis_align.py index e8ba2ae5c..7ad66acb0 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/axis_align.py +++ b/embodichain/lab/sim/atomic_actions/primitives/axis_align.py @@ -58,6 +58,7 @@ ) from embodichain.lab.sim.atomic_actions.primitives._helpers import ( arm_qpos_from_state, + resample_planned_trajectory, require_shared_task_state_key, ) from embodichain.lab.sim.atomic_actions.primitives.pick_up import PickUpOptions @@ -512,7 +513,9 @@ def _plan_pose_phase( ) assert isinstance(result.success, torch.Tensor) assert result.positions is not None - return result.success, result.positions + return result.success, resample_planned_trajectory( + result.positions, sample_count + ) def _axis_alignment_eef_keyframes( self, diff --git a/embodichain/lab/sim/atomic_actions/primitives/open_door.py b/embodichain/lab/sim/atomic_actions/primitives/open_door.py index 2a97ec796..0ace8b213 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/open_door.py +++ b/embodichain/lab/sim/atomic_actions/primitives/open_door.py @@ -53,7 +53,10 @@ from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( make_manipulation_slot, ) -from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state +from embodichain.lab.sim.atomic_actions.primitives._helpers import ( + arm_qpos_from_state, + resample_planned_trajectory, +) from embodichain.lab.sim.atomic_actions.requirements import ( CARTESIAN_POSE_CAPABILITY, SkillBindingContract, @@ -663,7 +666,9 @@ def _plan_pose_segment( ) assert isinstance(result.success, torch.Tensor) assert result.positions is not None - return result.success, result.positions + return result.success, resample_planned_trajectory( + result.positions, sample_count + ) def _opened_link_and_eef_poses( self, diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index 1e396c788..45da6bd30 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -342,6 +342,9 @@ def _get_full_pickup_trajectory( interpolation_dt=interpolation_dt, ) if motion_policy.strategy == "motion_gen": + # Keep the native combined path for the pose-based split. The + # motion generator resolves backend defaults when no explicit + # planner options were supplied. motion_options.sample_count = None motion_result = self.motion_generator.generate( build_pose_plan_states( diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index c75557903..b35425a07 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -293,6 +293,9 @@ def _plan( interpolation_dt=context.control_dt, ) if request.motion_policy.strategy == "motion_gen": + # Keep the native combined path for the pose-based split. The + # motion generator resolves backend defaults when no explicit + # planner options were supplied. motion_options.sample_count = None motion_result = self.motion_generator.generate( build_pose_plan_states(torch.cat([down_xpos, back_xpos], dim=1)), diff --git a/embodichain/lab/sim/atomic_actions/primitives/press.py b/embodichain/lab/sim/atomic_actions/primitives/press.py index 7498dcffd..6d167adaa 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/press.py +++ b/embodichain/lab/sim/atomic_actions/primitives/press.py @@ -24,7 +24,10 @@ import torch -from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state +from embodichain.lab.sim.atomic_actions.primitives._helpers import ( + arm_qpos_from_state, + resample_planned_trajectory, +) from embodichain.lab.sim.atomic_actions.affordance import PressAffordance from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget from embodichain.lab.sim.atomic_actions.control import ( @@ -359,7 +362,9 @@ def _plan_pose_segment( ) assert isinstance(result.success, torch.Tensor) assert result.positions is not None - return result.success, result.positions + return result.success, resample_planned_trajectory( + result.positions, sample_count + ) __all__ = ["Press", "PressGoal", "PressOptions"] diff --git a/embodichain/lab/sim/atomic_actions/primitives/push_object.py b/embodichain/lab/sim/atomic_actions/primitives/push_object.py index 963ce4f9b..56843eeb2 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/push_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/push_object.py @@ -47,7 +47,10 @@ from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( make_manipulation_slot, ) -from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state +from embodichain.lab.sim.atomic_actions.primitives._helpers import ( + arm_qpos_from_state, + resample_planned_trajectory, +) from embodichain.lab.sim.atomic_actions.requirements import ( CARTESIAN_POSE_CAPABILITY, SkillBindingContract, @@ -626,7 +629,9 @@ def _plan_pose_segment( ) assert isinstance(result.success, torch.Tensor) assert result.positions is not None - return result.success, result.positions + return result.success, resample_planned_trajectory( + result.positions, sample_count + ) __all__ = [ diff --git a/embodichain/lab/sim/atomic_actions/primitives/slide.py b/embodichain/lab/sim/atomic_actions/primitives/slide.py index 53235392a..e0111d20c 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/slide.py +++ b/embodichain/lab/sim/atomic_actions/primitives/slide.py @@ -51,7 +51,10 @@ from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( make_manipulation_slot, ) -from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state +from embodichain.lab.sim.atomic_actions.primitives._helpers import ( + arm_qpos_from_state, + resample_planned_trajectory, +) from embodichain.lab.sim.atomic_actions.requirements import ( CARTESIAN_POSE_CAPABILITY, SkillBindingContract, @@ -402,7 +405,9 @@ def _plan_pose_segment( ) assert isinstance(result.success, torch.Tensor) assert result.positions is not None - return result.success, result.positions + return result.success, resample_planned_trajectory( + result.positions, sample_count + ) __all__ = [ diff --git a/embodichain/lab/sim/atomic_actions/primitives/twist.py b/embodichain/lab/sim/atomic_actions/primitives/twist.py index 73a0d5532..fc22da998 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/twist.py +++ b/embodichain/lab/sim/atomic_actions/primitives/twist.py @@ -53,7 +53,10 @@ from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( make_manipulation_slot, ) -from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state +from embodichain.lab.sim.atomic_actions.primitives._helpers import ( + arm_qpos_from_state, + resample_planned_trajectory, +) from embodichain.lab.sim.atomic_actions.requirements import ( CARTESIAN_POSE_CAPABILITY, FORWARD_KINEMATICS_CAPABILITY, @@ -373,7 +376,9 @@ def _plan_pose_segment( ) assert isinstance(result.success, torch.Tensor) assert result.positions is not None - return result.success, result.positions + return result.success, resample_planned_trajectory( + result.positions, sample_count + ) def _twisted_grasp_poses( self, diff --git a/embodichain/lab/sim/motion/motion_generator.py b/embodichain/lab/sim/motion/motion_generator.py index 6d07b398d..7c9e74d1a 100644 --- a/embodichain/lab/sim/motion/motion_generator.py +++ b/embodichain/lab/sim/motion/motion_generator.py @@ -488,6 +488,10 @@ def generate( (``"motion_gen"``) or deterministic waypoint IK followed by joint-space interpolation (``"ik_interp"``). Joint targets fall back to interpolation when the configured backend cannot consume :class:`MoveType.JOINT_MOVE`. + Joint-only planners that own sparse waypoints may retain every converted + waypoint when quantity sampling is requested. For automatically resolved + trapezoidal options, the requested count is then treated as a lower bound; + an explicitly supplied planner option remains authoritative. Args: target_states: Batched planner waypoints. @@ -546,8 +550,8 @@ def _generate_with_planner( move_type = target_states[0].move_type uses_sparse_joint_waypoints = ( len(move_types) == 1 - and move_type == MoveType.JOINT_MOVE - and self.planner.uses_sparse_joint_waypoints + and move_type is MoveType.JOINT_MOVE + and getattr(self.planner, "uses_sparse_joint_waypoints", False) is True ) should_preinterpolate = ( len(move_types) == 1 @@ -646,12 +650,26 @@ def _generate_with_planner( ValueError, ) + explicit_plan_options = options.plan_opts is not None plan_opts = self.resolve_plan_options( options.plan_opts, sample_count=options.sample_count, velocity_limit=options.velocity_limit, acceleration_limit=options.acceleration_limit, ) + if ( + should_preinterpolate + and not explicit_plan_options + and isinstance(plan_opts, TrapezoidalPlanOptions) + and plan_opts.sample_method is TrajectorySampleMethod.QUANTITY + and int(plan_opts.sample_interval) < len(target_plan_states) + ): + # Trapezoidal quantity sampling retains every supplied waypoint. + # Cartesian targets may have been converted to a denser IK path. + # For backend-neutral defaults, make the requested count a safe + # lower bound instead of rejecting an otherwise valid path. An + # explicit TrapezoidalPlanOptions remains caller-authoritative. + plan_opts.sample_interval = len(target_plan_states) plan_opts = self.planner.with_motion_context( plan_opts, start_qpos=options.start_qpos, diff --git a/embodichain/lab/sim/motion/planners/base_planner.py b/embodichain/lab/sim/motion/planners/base_planner.py index 5feb5720a..a32accc9e 100644 --- a/embodichain/lab/sim/motion/planners/base_planner.py +++ b/embodichain/lab/sim/motion/planners/base_planner.py @@ -230,17 +230,19 @@ def __init__(self, cfg: BasePlannerCfg): uses_sparse_joint_waypoints: bool = False """Whether joint targets are sparse waypoints owned by the planner. - When ``True``, :class:`MotionGenerator` bypasses its generic joint-space - interpolation and prepends ``start_qpos`` as the first waypoint. This lets - the planner own both path timing and derivative generation. + When ``True``, :class:`~embodichain.lab.sim.motion.motion_generator.MotionGenerator` + prepends the observed ``start_qpos`` before dispatching a joint target. This + lets the planner own both path timing and derivative generation without + making every caller materialize the current state as a waypoint. """ preserve_plan_samples: bool = False """Whether callers must retain this planner's returned sample points exactly. When ``True``, :class:`MotionGenerator` returns the planner's trajectory - without resampling, preserving collision-checked samples. When ``False`` - (the default), the generator may normalize the trajectory to a requested + without resampling, preserving planner-owned samples and derivatives + (including collision-checked samples where applicable). When ``False`` (the + default), the generator may normalize the trajectory to a requested waypoint count. """ diff --git a/scripts/tutorials/atomic_action/assemble.py b/scripts/tutorials/atomic_action/assemble.py index c664c90fe..8c10d7cab 100644 --- a/scripts/tutorials/atomic_action/assemble.py +++ b/scripts/tutorials/atomic_action/assemble.py @@ -134,6 +134,7 @@ def parse_arguments() -> argparse.Namespace: "visualize_axes", ), default_renderer="hybrid", + default_planner="curobo", ) return parser.parse_args() @@ -257,7 +258,10 @@ def run_assemble_demo( can, label="soda_can", ) - motion_gen = create_curobo_motion_generator(robot) + motion_gen = create_curobo_motion_generator( + robot, + planner=getattr(args, "planner", "curobo"), + ) left_open, left_close = get_hand_open_close_qpos( robot, hand_control_part="left_hand", close_qpos=HAND_CLOSE_QPOS ) diff --git a/scripts/tutorials/atomic_action/axis_align.py b/scripts/tutorials/atomic_action/axis_align.py index 8685f3f6c..a41466329 100644 --- a/scripts/tutorials/atomic_action/axis_align.py +++ b/scripts/tutorials/atomic_action/axis_align.py @@ -150,7 +150,10 @@ def main() -> None: obj = create_align_object(sim) hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, obj, hand_open) - motion_gen = create_toppra_motion_generator(robot) + motion_gen = create_toppra_motion_generator( + robot, + planner=getattr(args, "planner", "toppra"), + ) engine = create_simulation_atomic_action_engine( motion_generator=motion_gen, diff --git a/scripts/tutorials/atomic_action/control_dt.py b/scripts/tutorials/atomic_action/control_dt.py index 617567cdc..3e062a32d 100644 --- a/scripts/tutorials/atomic_action/control_dt.py +++ b/scripts/tutorials/atomic_action/control_dt.py @@ -64,7 +64,12 @@ def main() -> None: args = parse_arguments() sim = create_tutorial_simulation(args) robot = add_tutorial_robot(sim, args.robot) - engine = AtomicActionEngine(motion_generator=create_toppra_motion_generator(robot)) + engine = AtomicActionEngine( + motion_generator=create_toppra_motion_generator( + robot, + planner=getattr(args, "planner", "toppra"), + ) + ) initial_qpos = robot.get_qpos().clone() start_arm_qpos = robot.get_qpos(name="arm")[0] diff --git a/scripts/tutorials/atomic_action/coordinated_pickment.py b/scripts/tutorials/atomic_action/coordinated_pickment.py index b3ea01d1f..98fa7bdbf 100644 --- a/scripts/tutorials/atomic_action/coordinated_pickment.py +++ b/scripts/tutorials/atomic_action/coordinated_pickment.py @@ -384,7 +384,10 @@ def run_coordinated_pickment_demo( label=preset.label, ) left_to_right_arm_direction = compute_left_to_right_arm_direction(robot, sim.device) - motion_gen = create_toppra_motion_generator(robot) + motion_gen = create_toppra_motion_generator( + robot, + planner=getattr(args, "planner", "toppra"), + ) hand_close_qpos = ( ROBOTIQ_2F_140_CLOSE_QPOS if args.robot == "ur10" else preset.hand_close_qpos diff --git a/scripts/tutorials/atomic_action/coordinated_placement.py b/scripts/tutorials/atomic_action/coordinated_placement.py index e78034e5b..36db35f0b 100644 --- a/scripts/tutorials/atomic_action/coordinated_placement.py +++ b/scripts/tutorials/atomic_action/coordinated_placement.py @@ -550,7 +550,10 @@ def run_coordinated_placement_demo( log_scene_targets(bread_pose, pan_pose) bread_semantics = create_manual_object_semantics(bread, BREAD_LABEL) pan_semantics = create_manual_object_semantics(pan, PAN_LABEL) - motion_gen = create_toppra_motion_generator(robot) + motion_gen = create_toppra_motion_generator( + robot, + planner=getattr(args, "planner", "toppra"), + ) right_open, right_close = get_hand_open_close_qpos( robot, diff --git a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py index d2209423a..2a9eb3b8e 100644 --- a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py +++ b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py @@ -390,7 +390,8 @@ def _publish_path_overlays( def parse_arguments() -> argparse.Namespace: """Parse command-line arguments for the dynamic-obstacle tutorial.""" parser = create_tutorial_argument_parser( - "Demonstrate collision-world revision recovery with cuRobo." + "Demonstrate collision-world revision recovery with cuRobo.", + default_planner="curobo", ) parser.add_argument( "--no_obstacle_motion", @@ -403,6 +404,11 @@ def parse_arguments() -> argparse.Namespace: def main() -> None: """Move an obstacle during execution and replan from the latest snapshot.""" args = parse_arguments() + if getattr(args, "planner", "curobo") != "curobo": + raise ValueError( + "dynamic_obstacle_recovery requires --planner curobo because " + "the demo updates a live collision world during execution." + ) sim = create_tutorial_simulation(args) robot = add_tutorial_robot(sim, args.robot) obstacle = sim.add_rigid_object( diff --git a/scripts/tutorials/atomic_action/hand_over.py b/scripts/tutorials/atomic_action/hand_over.py index c01c5b80e..7d6d5b1ec 100644 --- a/scripts/tutorials/atomic_action/hand_over.py +++ b/scripts/tutorials/atomic_action/hand_over.py @@ -190,7 +190,10 @@ def run_handover_demo( obj.clear_dynamics() publish_tutorial_scene(sim, args) object_semantics = create_antipodal_semantics(obj, label="handover") - motion_gen = create_toppra_motion_generator(robot) + motion_gen = create_toppra_motion_generator( + robot, + planner=getattr(args, "planner", "toppra"), + ) left_open, left_close = get_hand_open_close_qpos( robot, hand_control_part="left_hand", close_qpos=HAND_CLOSE_QPOS diff --git a/scripts/tutorials/atomic_action/move_end_effector.py b/scripts/tutorials/atomic_action/move_end_effector.py index 9993916e1..321923746 100644 --- a/scripts/tutorials/atomic_action/move_end_effector.py +++ b/scripts/tutorials/atomic_action/move_end_effector.py @@ -56,6 +56,7 @@ def parse_arguments() -> argparse.Namespace: parser = create_tutorial_argument_parser( "Demonstrate MoveEndEffector with a multi-waypoint pose trajectory.", features=("visualize_axes",), + default_planner="curobo", ) return parser.parse_args() @@ -65,7 +66,10 @@ def main() -> None: args = parse_arguments() sim = create_tutorial_simulation(args) robot = add_tutorial_robot(sim, args.robot) - motion_gen = create_curobo_motion_generator(robot) + motion_gen = create_curobo_motion_generator( + robot, + planner=getattr(args, "planner", "curobo"), + ) engine = AtomicActionEngine(motion_generator=motion_gen) diff --git a/scripts/tutorials/atomic_action/move_held_object.py b/scripts/tutorials/atomic_action/move_held_object.py index efc553ac1..ba266630b 100644 --- a/scripts/tutorials/atomic_action/move_held_object.py +++ b/scripts/tutorials/atomic_action/move_held_object.py @@ -74,6 +74,7 @@ def parse_arguments() -> argparse.Namespace: parser = create_tutorial_argument_parser( "Pick up a paper cup and move it by object pose.", features=("grasp_sampling", "visualize_axes"), + default_planner="curobo", ) return parser.parse_args() @@ -118,7 +119,10 @@ def main() -> None: sim = create_tutorial_simulation(args) robot = add_tutorial_robot(sim, args.robot) obj = create_pick_object(sim) - motion_gen = create_curobo_motion_generator(robot) + motion_gen = create_curobo_motion_generator( + robot, + planner=getattr(args, "planner", "curobo"), + ) hand_open, hand_close = get_hand_open_close_qpos(robot) engine = create_simulation_atomic_action_engine( diff --git a/scripts/tutorials/atomic_action/move_joints.py b/scripts/tutorials/atomic_action/move_joints.py index 0a35a5b9f..60ed84f71 100644 --- a/scripts/tutorials/atomic_action/move_joints.py +++ b/scripts/tutorials/atomic_action/move_joints.py @@ -55,6 +55,7 @@ def parse_arguments() -> argparse.Namespace: parser = create_tutorial_argument_parser( "Demonstrate MoveJoints with named and explicit qpos targets.", features=("visualize_axes",), + default_planner="curobo", ) return parser.parse_args() @@ -64,7 +65,10 @@ def main() -> None: args = parse_arguments() sim = create_tutorial_simulation(args) robot = add_tutorial_robot(sim, args.robot) - motion_gen = create_curobo_motion_generator(robot) + motion_gen = create_curobo_motion_generator( + robot, + planner=getattr(args, "planner", "curobo"), + ) home = robot.get_qpos(name="arm")[0].clone() limits = robot.get_qpos_limits(name="arm")[0] diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index 3e007477a..5b53ea20c 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -229,7 +229,8 @@ def _compose_goal_pose( def parse_arguments() -> argparse.Namespace: """Parse command-line arguments for the moving-target tutorial.""" parser = create_tutorial_argument_parser( - "Demonstrate ExecutionRunner replanning after a visible target move." + "Demonstrate ExecutionRunner replanning after a visible target move.", + default_planner="curobo", ) parser.add_argument( "--no_target_motion", @@ -253,7 +254,10 @@ def main() -> None: control_dt=2.0 * sim.sim_config.physics_dt, scene_supplier=target_scene.snapshot, ) - motion_gen = create_curobo_motion_generator(robot) + motion_gen = create_curobo_motion_generator( + robot, + planner=getattr(args, "planner", "curobo"), + ) hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, target, hand_open) if args.no_target_motion: diff --git a/scripts/tutorials/atomic_action/open_door.py b/scripts/tutorials/atomic_action/open_door.py index cab6cfebb..af2c971ed 100644 --- a/scripts/tutorials/atomic_action/open_door.py +++ b/scripts/tutorials/atomic_action/open_door.py @@ -161,7 +161,10 @@ def main() -> None: draw_axis_marker(sim, "door_handle_link_pose", handle_pose) engine = AtomicActionEngine( - motion_generator=create_toppra_motion_generator(robot), + motion_generator=create_toppra_motion_generator( + robot, + planner=getattr(args, "planner", "toppra"), + ), control_profiles={ "hand": ControlPartCommandProfile.joint_positions( open=hand_open, @@ -190,7 +193,10 @@ def main() -> None: open_fraction=open_fraction, ), control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, - motion_policy=MotionPolicy(sample_count=TRAJECTORY_SAMPLE_COUNT), + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=TRAJECTORY_SAMPLE_COUNT, + ), skill_options=OpenDoorOptions( hand_interp_steps=HAND_INTERP_STEPS, door_waypoint_count=args.door_waypoint_count, diff --git a/scripts/tutorials/atomic_action/pickup.py b/scripts/tutorials/atomic_action/pickup.py index c931092b6..88df9092f 100644 --- a/scripts/tutorials/atomic_action/pickup.py +++ b/scripts/tutorials/atomic_action/pickup.py @@ -73,6 +73,7 @@ def parse_arguments() -> argparse.Namespace: parser = create_tutorial_argument_parser( "Demonstrate PickUp on a cube.", features=("grasp_sampling", "visualize_axes"), + default_planner="curobo", ) parser.add_argument( "--approach", choices=[*APPROACH_DIRECTIONS, "custom"], default="top" @@ -129,7 +130,10 @@ def main() -> None: obj = create_pick_object(sim) hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, obj, hand_open) - motion_gen = create_curobo_motion_generator(robot) + motion_gen = create_curobo_motion_generator( + robot, + planner=getattr(args, "planner", "curobo"), + ) engine = create_simulation_atomic_action_engine( motion_generator=motion_gen, scene_entities=(obj,), diff --git a/scripts/tutorials/atomic_action/place.py b/scripts/tutorials/atomic_action/place.py index f4e0b22ed..96d34ff01 100644 --- a/scripts/tutorials/atomic_action/place.py +++ b/scripts/tutorials/atomic_action/place.py @@ -74,6 +74,7 @@ def parse_arguments() -> argparse.Namespace: parser = create_tutorial_argument_parser( "Pick up a cube and place it at a target pose.", features=("grasp_sampling", "visualize_axes"), + default_planner="curobo", ) return parser.parse_args() @@ -125,7 +126,10 @@ def main() -> None: sim = create_tutorial_simulation(args) robot = add_tutorial_robot(sim, args.robot, tcp_z=0.15) obj = create_pick_object(sim) - motion_gen = create_curobo_motion_generator(robot) + motion_gen = create_curobo_motion_generator( + robot, + planner=getattr(args, "planner", "curobo"), + ) hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, obj, hand_open) diff --git a/scripts/tutorials/atomic_action/pour.py b/scripts/tutorials/atomic_action/pour.py index b749d8ef2..5b46f791e 100644 --- a/scripts/tutorials/atomic_action/pour.py +++ b/scripts/tutorials/atomic_action/pour.py @@ -39,6 +39,7 @@ PourOptions, ) from embodichain.lab.sim.motion.planners import ( + TrapezoidalPlanOptions, ToppraPlanOptions, TrajectorySampleMethod, ) @@ -60,6 +61,7 @@ prepare_tutorial_scene, replay_trajectory, run_tutorial, + TutorialPlanner, ) POUR_INTERNAL_AXIS = (1.0, 0.0, 0.0) @@ -92,15 +94,28 @@ def parse_arguments() -> argparse.Namespace: return parser.parse_args() -def _create_pick_motion_policy() -> MotionPolicy: - """Create the PickUp policy with an explicit valid TOPPRA sample count.""" +def _create_pick_motion_policy(planner: TutorialPlanner = "toppra") -> MotionPolicy: + """Create a planner-specific PickUp policy with a fixed motion sample count.""" + if planner == "toppra": + plan_opts = ToppraPlanOptions( + sample_method=TrajectorySampleMethod.QUANTITY, + sample_interval=PICK_MOTION_SAMPLE_COUNT, + ) + elif planner == "trapezoidal": + plan_opts = TrapezoidalPlanOptions( + sample_method=TrajectorySampleMethod.QUANTITY, + sample_interval=PICK_MOTION_SAMPLE_COUNT, + ) + elif planner == "curobo": + # cuRobo owns its native sampling and does not use the joint-space + # sample-count options above. + plan_opts = None + else: + raise ValueError(f"Unsupported tutorial planner {planner!r}.") return MotionPolicy( strategy="motion_gen", sample_count=PICK_SAMPLE_INTERVAL, - plan_opts=ToppraPlanOptions( - sample_method=TrajectorySampleMethod.QUANTITY, - sample_interval=PICK_MOTION_SAMPLE_COUNT, - ), + plan_opts=plan_opts, ) @@ -115,7 +130,8 @@ def main() -> None: ) hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, obj, hand_open) - motion_gen = create_toppra_motion_generator(robot) + planner = getattr(args, "planner", "toppra") + motion_gen = create_toppra_motion_generator(robot, planner=planner) engine = create_simulation_atomic_action_engine( motion_generator=motion_gen, @@ -149,7 +165,7 @@ def main() -> None: "pick_up", GraspGoal(semantics), control_parts=control_parts, - motion_policy=_create_pick_motion_policy(), + motion_policy=_create_pick_motion_policy(planner), skill_options=PickUpOptions( approach_direction=torch.tensor( APPROACH_DIRECTION, diff --git a/scripts/tutorials/atomic_action/press.py b/scripts/tutorials/atomic_action/press.py index e9bbcbf53..b920a21f8 100644 --- a/scripts/tutorials/atomic_action/press.py +++ b/scripts/tutorials/atomic_action/press.py @@ -185,7 +185,10 @@ def main() -> None: ) target = create_rigid_button(sim) if args.rigid_object else create_microwave(sim) hand_open, hand_close = get_hand_open_close_qpos(robot, close_qpos=0.040) - motion_gen = create_toppra_motion_generator(robot) + motion_gen = create_toppra_motion_generator( + robot, + planner=getattr(args, "planner", "toppra"), + ) semantics, target_pose = create_button_semantics(target) affordance = semantics.affordance assert isinstance(affordance, PressAffordance) @@ -214,7 +217,10 @@ def main() -> None: target_pose, ), control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, - motion_policy=MotionPolicy(sample_count=PRESS_SAMPLE_INTERVAL), + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=PRESS_SAMPLE_INTERVAL, + ), skill_options=PressOptions( hand_interp_steps=HAND_INTERP_STEPS, approach_distance=0.12, diff --git a/scripts/tutorials/atomic_action/slide.py b/scripts/tutorials/atomic_action/slide.py index 3a70cd517..09b753df5 100644 --- a/scripts/tutorials/atomic_action/slide.py +++ b/scripts/tutorials/atomic_action/slide.py @@ -162,7 +162,10 @@ def create_invocation( target_pose, ), control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, - motion_policy=MotionPolicy(sample_count=TRAJECTORY_SAMPLE_COUNT), + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=TRAJECTORY_SAMPLE_COUNT, + ), skill_options=SlideOptions( direction=direction, hand_interp_steps=HAND_INTERP_STEPS, @@ -188,7 +191,10 @@ def main() -> None: ) drawer = create_drawer(sim) hand_open, hand_close = get_hand_open_close_qpos(robot) - motion_gen = create_toppra_motion_generator(robot) + motion_gen = create_toppra_motion_generator( + robot, + planner=getattr(args, "planner", "toppra"), + ) semantics = create_drawer_semantics(drawer) affordance = semantics.affordance assert isinstance(affordance, SlideAffordance) diff --git a/scripts/tutorials/atomic_action/tutorial_utils.py b/scripts/tutorials/atomic_action/tutorial_utils.py index 17b5e6918..b7c9d4666 100644 --- a/scripts/tutorials/atomic_action/tutorial_utils.py +++ b/scripts/tutorials/atomic_action/tutorial_utils.py @@ -39,7 +39,11 @@ from embodichain.lab.sim.cfg import LightCfg, MarkerCfg, RenderCfg, RobotCfg from embodichain.lab.sim.objects import RigidObject, Robot from embodichain.lab.sim.motion.motion_generator import MotionGenCfg, MotionGenerator -from embodichain.lab.sim.motion.planners import CuroboPlannerCfg, ToppraPlannerCfg +from embodichain.lab.sim.motion.planners import ( + CuroboPlannerCfg, + ToppraPlannerCfg, + TrapezoidalPlannerCfg, +) from embodichain.lab.sim.robots import FrankaPandaCfg, URRobotCfg from embodichain.toolkits.graspkit.pg_grasp import ( AntipodalGraspPoseGenerator, @@ -118,6 +122,12 @@ "franka", "ur10", ) +TutorialPlanner = Literal["toppra", "trapezoidal", "curobo"] +TUTORIAL_PLANNERS: tuple[TutorialPlanner, ...] = ( + "toppra", + "trapezoidal", + "curobo", +) def create_tutorial_argument_parser( @@ -126,8 +136,28 @@ def create_tutorial_argument_parser( features: Collection[TutorialCliFeature] = (), default_device: str | None = None, default_renderer: str | None = None, + default_planner: TutorialPlanner = "toppra", ) -> argparse.ArgumentParser: - """Create a launcher parser with the shared atomic-tutorial switches.""" + """Create a launcher parser with the shared atomic-tutorial switches. + + Args: + description: Command-line program description. + features: Optional groups of tutorial-specific shared arguments. + default_device: Optional device override for the launcher arguments. + default_renderer: Optional renderer override for the launcher arguments. + default_planner: Planner selected when ``--planner`` is omitted. + + Returns: + The configured argument parser. + + Raises: + ValueError: If ``default_planner`` is not a supported tutorial backend. + """ + if default_planner not in TUTORIAL_PLANNERS: + raise ValueError( + f"default_planner must be one of {TUTORIAL_PLANNERS}, " + f"got {default_planner!r}." + ) parser = argparse.ArgumentParser(description=description) add_env_launcher_args_to_parser(parser) defaults = {} @@ -149,6 +179,16 @@ def create_tutorial_argument_parser( default="ur5", help="Robot construction to use (default: ur5).", ) + parser.add_argument( + "--planner", + choices=TUTORIAL_PLANNERS, + default=default_planner, + help=( + "Motion-planner backend: toppra, trapezoidal, or curobo " + f"(default: {default_planner}). NeuralPlanner is not exposed " + "by this tutorial selector." + ), + ) if "debug_state" in features: parser.add_argument( "--debug_state", @@ -312,32 +352,93 @@ def add_tutorial_robot( ) -def create_toppra_motion_generator(robot: Robot) -> MotionGenerator: - """Create the standard TOPPRA motion generator for a tutorial robot. +def create_tutorial_motion_generator( + robot: Robot, + planner: TutorialPlanner = "toppra", +) -> MotionGenerator: + """Create a selected non-neural motion generator for a tutorial robot. + + The selector intentionally mirrors the planner types that are usable from + the atomic-action tutorials. ``NeuralPlanner`` is omitted because it needs + a model-specific ONNX configuration and is not a drop-in backend for these + examples. + + Args: + robot: Robot whose trajectories will be planned. + planner: Planner backend to construct. + + Returns: + The configured motion generator for ``planner``. + + Raises: + ValueError: If ``planner`` is not one of the supported tutorial + backends. + """ + planner_cfg_types = { + "toppra": ToppraPlannerCfg, + "trapezoidal": TrapezoidalPlannerCfg, + "curobo": CuroboPlannerCfg, + } + if planner not in TUTORIAL_PLANNERS: + raise ValueError( + f"Unsupported tutorial planner {planner!r}; " + f"choose one of {TUTORIAL_PLANNERS}." + ) + planner_cfg = planner_cfg_types[planner](robot_uid=robot.uid) + return MotionGenerator(cfg=MotionGenCfg(planner_cfg=planner_cfg)) + + +def create_toppra_motion_generator( + robot: Robot, + planner: TutorialPlanner = "toppra", +) -> MotionGenerator: + """Create a tutorial motion generator, defaulting to TOPPRA. + + ``planner`` keeps this historical helper compatible while allowing a + tutorial to opt into the shared command-line selector. Args: robot: Robot whose trajectories will be planned. + planner: Planner backend to construct. Returns: The configured motion generator. """ - return MotionGenerator( - cfg=MotionGenCfg(planner_cfg=ToppraPlannerCfg(robot_uid=robot.uid)) - ) + return create_tutorial_motion_generator(robot, planner) + +def create_trapezoidal_motion_generator( + robot: Robot, + planner: TutorialPlanner = "trapezoidal", +) -> MotionGenerator: + """Create a tutorial motion generator, defaulting to trapezoidal timing. -def create_curobo_motion_generator(robot: Robot) -> MotionGenerator: - """Create a cuRobo-backed motion generator for a tutorial robot. + Args: + robot: Robot whose trajectories will be planned. + planner: Planner backend to construct. + + Returns: + The configured motion generator. + """ + return create_tutorial_motion_generator(robot, planner) + + +def create_curobo_motion_generator( + robot: Robot, + planner: TutorialPlanner = "curobo", +) -> MotionGenerator: + """Create a tutorial motion generator, defaulting to cuRobo. Args: robot: Robot whose trajectories will be planned. + planner: Planner backend to construct. The default preserves the + historical cuRobo helper behavior. Returns: - The configured motion generator with an empty external collision world. + The configured motion generator. For cuRobo, its external collision + world is empty until a tutorial supplies one explicitly. """ - return MotionGenerator( - cfg=MotionGenCfg(planner_cfg=CuroboPlannerCfg(robot_uid=robot.uid)) - ) + return create_tutorial_motion_generator(robot, planner) def get_hand_open_close_qpos( @@ -1130,7 +1231,9 @@ def create_tutorial_robot_cfg( "ROBOTIQ_HAND_JOINT_PATTERN", "TOP_DOWN_EEF_ROTATION", "TutorialCliFeature", + "TutorialPlanner", "TutorialRobot", + "TUTORIAL_PLANNERS", "TUTORIAL_ROBOTS", "add_tutorial_robot", "add_ur5_gripper_robot", @@ -1141,8 +1244,10 @@ def create_tutorial_robot_cfg( "create_parallel_jaw_grasp_pose_generator", "create_curobo_motion_generator", "create_franka_panda_robot_cfg", + "create_trapezoidal_motion_generator", "create_toppra_motion_generator", "create_tutorial_argument_parser", + "create_tutorial_motion_generator", "create_tutorial_robot_cfg", "create_tutorial_simulation", "create_ur10_robotiq_robot_cfg", diff --git a/scripts/tutorials/atomic_action/twist.py b/scripts/tutorials/atomic_action/twist.py index bc366bdb5..18a3b3865 100644 --- a/scripts/tutorials/atomic_action/twist.py +++ b/scripts/tutorials/atomic_action/twist.py @@ -172,7 +172,10 @@ def main() -> None: ) target = create_rigid_knob(sim) if args.rigid_object else create_microwave(sim) hand_open, hand_close = get_hand_open_close_qpos(robot) - motion_gen = create_toppra_motion_generator(robot) + motion_gen = create_toppra_motion_generator( + robot, + planner=getattr(args, "planner", "toppra"), + ) semantics, target_pose = create_knob_semantics(target) engine = AtomicActionEngine( @@ -199,7 +202,10 @@ def main() -> None: target_pose, ), control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, - motion_policy=MotionPolicy(sample_count=TWIST_SAMPLE_INTERVAL), + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=TWIST_SAMPLE_INTERVAL, + ), skill_options=TwistOptions( hand_interp_steps=HAND_INTERP_STEPS, pre_grasp_distance=0.12, diff --git a/tests/compute/test_trajectory.py b/tests/compute/test_trajectory.py index e10be61d5..b5c08a0e3 100644 --- a/tests/compute/test_trajectory.py +++ b/tests/compute/test_trajectory.py @@ -105,6 +105,22 @@ def test_resample_with_distance_allows_path_downsampling() -> None: assert torch.equal(result, expected) +def test_resample_with_distance_falls_back_when_warp_is_unavailable( + monkeypatch: pytest.MonkeyPatch, +) -> None: + path = torch.tensor([[[0.0, 0.0], [1.0, 0.0], [1.0, 1.0]]]) + + def fail_from_torch(_trajectory: torch.Tensor) -> None: + raise RuntimeError("Warp runtime unavailable") + + monkeypatch.setattr(wp, "from_torch", fail_from_torch) + + result = resample_with_distance(path, interp_num=3, device="cpu") + + expected = torch.tensor([[[0.0, 0.0], [1.0, 0.0], [1.0, 1.0]]]) + assert torch.equal(result, expected) + + @pytest.mark.parametrize( "device", ["cpu", pytest.param("cuda:0", marks=pytest.mark.gpu)] ) diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 9253a5e66..5efdd7cc5 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -114,7 +114,13 @@ ParallelJawGripperModelCfg, ) from embodichain.lab.sim.motion.motion_generator import MotionGenerator -from embodichain.lab.sim.motion.planners import MoveType, PlanOptions, PlanResult +from embodichain.lab.sim.motion.planners import ( + MoveType, + PlanOptions, + PlanResult, + TrapezoidalPlanOptions, + TrapezoidalPlanner, +) from embodichain.utils.math import axis_angle_to_rotation_matrix, pose_inv NUM_ENVS = 2 @@ -1151,7 +1157,9 @@ def test_pick_combined_motion_gen_preserves_backend_samples_before_split() -> No ) generator.generate.assert_called_once() - assert generator.generate.call_args.kwargs["options"].sample_count is None + motion_options = generator.generate.call_args.kwargs["options"] + assert motion_options.sample_count is None + assert motion_options.plan_opts is None assert plan.commands.frame_count == sample_count @@ -4984,6 +4992,7 @@ def test_position_only_motion_policy_emits_explicit_zero_velocity() -> None: def test_move_joints_supports_sparse_trapezoidal_planning() -> None: + """MoveJoints passes a sparse goal through the native time-profile backend.""" generator = _motion_generator() planner = object.__new__(TrapezoidalPlanner) planner.cfg = SimpleNamespace(planner_type="trapezoidal") @@ -5013,6 +5022,7 @@ def test_move_joints_supports_sparse_trapezoidal_planning() -> None: def test_move_joints_holds_stationary_trapezoidal_goal() -> None: + """MoveJoints emits a zero-derivative hold for a stationary goal.""" generator = _motion_generator() planner = object.__new__(TrapezoidalPlanner) planner.cfg = SimpleNamespace(planner_type="trapezoidal") diff --git a/tests/sim/atomic_actions/test_primitives_helpers.py b/tests/sim/atomic_actions/test_primitives_helpers.py index 2ff754fe0..6474db31f 100644 --- a/tests/sim/atomic_actions/test_primitives_helpers.py +++ b/tests/sim/atomic_actions/test_primitives_helpers.py @@ -26,6 +26,7 @@ repeat_qpos, resolve_batched_pose, resolve_object_target, + resample_planned_trajectory, ) BATCH_SIZE = 2 @@ -75,6 +76,19 @@ def test_assemble_full_robot_trajectory_rejects_empty_parts() -> None: ) +def test_resample_planned_trajectory_fits_native_path_to_phase_length() -> None: + trajectory = torch.tensor( + [[[0.0, 0.0], [0.25, 0.5], [1.0, 1.0], [1.5, 1.0]]], + dtype=torch.float32, + ) + + result = resample_planned_trajectory(trajectory, sample_count=3) + + assert result.shape == (1, 3, 2) + torch.testing.assert_close(result[:, 0], trajectory[:, 0]) + torch.testing.assert_close(result[:, -1], trajectory[:, -1]) + + def test_resolve_object_target_uses_custom_name_in_shape_error() -> None: with pytest.raises(ValueError, match="placing_object_target_pose"): resolve_object_target( diff --git a/tests/sim/atomic_actions/test_tutorial_utils.py b/tests/sim/atomic_actions/test_tutorial_utils.py index 7093722d4..8f965ba2f 100644 --- a/tests/sim/atomic_actions/test_tutorial_utils.py +++ b/tests/sim/atomic_actions/test_tutorial_utils.py @@ -50,18 +50,21 @@ from scripts.tutorials.atomic_action.tutorial_utils import ( ROBOTIQ_2F_140_TCP, ROBOTIQ_HAND_JOINT_PATTERN, + TUTORIAL_PLANNERS, TUTORIAL_ROBOTS, + TutorialPlanner, broadcast_pose_batch, broadcast_waypoint_pose_batch, clone_local_pose_from_first_env, create_antipodal_semantics, create_curobo_motion_generator, create_franka_panda_robot_cfg, + create_parallel_jaw_grasp_pose_generator, create_tutorial_argument_parser, + create_tutorial_motion_generator, create_tutorial_robot_cfg, create_ur10_robotiq_robot_cfg, create_ur5_gripper_robot_cfg, - create_parallel_jaw_grasp_pose_generator, get_hand_open_close_qpos, replay_trajectory, should_open_tutorial_window, @@ -702,16 +705,51 @@ def test_curobo_motion_generator_factory_selects_curobo_backend() -> None: assert cfg.planner_cfg.robot_uid == "tutorial_robot" +@pytest.mark.parametrize("planner", TUTORIAL_PLANNERS) +def test_tutorial_motion_generator_factory_selects_requested_backend( + planner: TutorialPlanner, +) -> None: + robot = MagicMock(uid="tutorial_robot") + + with patch( + "scripts.tutorials.atomic_action.tutorial_utils.MotionGenerator" + ) as motion_generator_cls: + result = create_tutorial_motion_generator(robot, planner) + + cfg = motion_generator_cls.call_args.kwargs["cfg"] + assert result is motion_generator_cls.return_value + assert cfg.planner_cfg.planner_type == planner + assert cfg.planner_cfg.robot_uid == "tutorial_robot" + + +def test_tutorial_motion_generator_factory_rejects_neural_backend() -> None: + with pytest.raises(ValueError, match="Unsupported tutorial planner"): + create_tutorial_motion_generator( + MagicMock(uid="tutorial_robot"), "neural" + ) # type: ignore[arg-type] + + 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([]) franka_args = parser.parse_args(["--robot", "franka"]) ur10_args = parser.parse_args(["--robot", "ur10"]) + trapezoidal_args = parser.parse_args(["--planner", "trapezoidal"]) assert TUTORIAL_ROBOTS == ("ur5", "franka", "ur10") + assert TUTORIAL_PLANNERS == ("toppra", "trapezoidal", "curobo") assert default_args.robot == "ur5" assert franka_args.robot == "franka" assert ur10_args.robot == "ur10" + assert default_args.planner == "toppra" + assert trapezoidal_args.planner == "trapezoidal" + + +def test_shared_tutorial_planner_selection_excludes_neural_backend() -> None: + parser = create_tutorial_argument_parser("test parser") + + with pytest.raises(SystemExit): + parser.parse_args(["--planner", "neural"]) def test_arm_direction_uses_selected_robot_solver_roots() -> None: @@ -744,9 +782,15 @@ def test_all_atomic_action_tutorials_accept_both_robot_choices( default_args = module.parse_arguments() with patch("sys.argv", [f"{module_name}.py", "--robot", "franka"]): franka_args = module.parse_arguments() + with patch( + "sys.argv", + [f"{module_name}.py", "--planner", "trapezoidal"], + ): + trapezoidal_args = module.parse_arguments() assert default_args.robot == "ur5" assert franka_args.robot == "franka" + assert trapezoidal_args.planner == "trapezoidal" def test_place_tutorial_registers_pick_object_with_simulation_engine_factory() -> None: diff --git a/tests/sim/motion/test_motion_generator_batched.py b/tests/sim/motion/test_motion_generator_batched.py index 88e04a1e8..1eec25b1f 100644 --- a/tests/sim/motion/test_motion_generator_batched.py +++ b/tests/sim/motion/test_motion_generator_batched.py @@ -117,11 +117,15 @@ def _trapezoidal_generator() -> MotionGenerator: return generator -def test_trapezoidal_generator_prepends_start_for_single_joint_target() -> None: +def test_trapezoidal_generator_prepends_observed_start_for_sparse_joint_goal() -> None: + """A joint-only backend must start from the observed robot configuration.""" generator = _trapezoidal_generator() + generator.interpolate_trajectory = Mock( + side_effect=AssertionError("sparse joint targets must bypass interpolation") + ) + start = torch.tensor([[0.0, 0.0]], dtype=torch.float64) goal = torch.tensor([[0.4, -0.2]], dtype=torch.float64) - result = generator.generate( [PlanState.from_qpos(goal)], MotionGenOptions( @@ -130,14 +134,16 @@ def test_trapezoidal_generator_prepends_start_for_single_joint_target() -> None: ), ) + assert result.positions is not None torch.testing.assert_close(result.positions[:, 0], start) torch.testing.assert_close(result.positions[:, -1], goal) -def test_trapezoidal_generator_holds_stationary_single_joint_target() -> None: +def test_trapezoidal_generator_holds_stationary_sparse_joint_goal() -> None: + """A stationary single goal still forms a valid two-waypoint hold.""" generator = _trapezoidal_generator() - start = torch.tensor([[0.4, -0.2]], dtype=torch.float64) + start = torch.tensor([[0.4, -0.2]], dtype=torch.float64) result = generator.generate( [PlanState.from_qpos(start)], MotionGenOptions( @@ -147,6 +153,7 @@ def test_trapezoidal_generator_holds_stationary_single_joint_target() -> None: ) assert bool(result.success.all()) + assert result.positions is not None assert result.positions.shape[1] == 2 torch.testing.assert_close( result.positions, start.unsqueeze(1).expand_as(result.positions) @@ -159,7 +166,33 @@ def test_trapezoidal_generator_holds_stationary_single_joint_target() -> None: ) +def test_trapezoidal_generator_covers_dense_ik_waypoints() -> None: + """Quantity sampling grows to retain all Cartesian-to-joint waypoints.""" + generator = _trapezoidal_generator() + generator.robot = SimpleNamespace( + compute_fk=lambda *, qpos, name, to_matrix: torch.eye( + 4, dtype=qpos.dtype, device=qpos.device + ).expand(qpos.shape[0], -1, -1) + ) + dense_qpos = torch.linspace(0.0, 0.5, 6, dtype=torch.float64).reshape(1, 6, 1) + dense_qpos = dense_qpos.expand(-1, -1, 2).clone() + generator.interpolate_trajectory = Mock(return_value=(dense_qpos, None)) + + result = generator.generate( + [PlanState.from_xpos(torch.eye(4, dtype=torch.float64).unsqueeze(0))], + MotionGenOptions( + start_qpos=torch.zeros(1, 2, dtype=torch.float64), + sample_count=3, + is_interpolate=True, + ), + ) + + assert result.positions is not None + assert result.positions.shape[1] == dense_qpos.shape[1] + + def test_trapezoidal_generator_preserves_native_derivatives() -> None: + """Native Trapezoidal samples and derivatives survive normalization.""" generator = _trapezoidal_generator() start = torch.tensor([[0.0, 0.0]], dtype=torch.float64) goal = torch.tensor([[0.4, -0.2]], dtype=torch.float64) @@ -176,6 +209,8 @@ def test_trapezoidal_generator_preserves_native_derivatives() -> None: ), ) + assert result.positions is not None + assert expected.positions is not None torch.testing.assert_close(result.positions, expected.positions) torch.testing.assert_close(result.dt, expected.dt) assert result.velocities is not None From 4cb393021237b70be2c8b5b4038f2e40c861b661 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 12 Sep 2026 05:11:19 +0000 Subject: [PATCH 2/3] feat(atomic-actions): default tutorials to trapezoidal planner Use trapezoidal timing when atomic-action tutorials omit --planner, while retaining cuRobo for the live collision-world recovery demo. Update planner guidance and regression coverage accordingly. --- .../sim/atomic_actions/builtin_actions.md | 6 ++++-- docs/source/tutorial/atomic_actions.rst | 15 +++++++++------ scripts/tutorials/atomic_action/assemble.py | 3 +-- scripts/tutorials/atomic_action/axis_align.py | 2 +- scripts/tutorials/atomic_action/control_dt.py | 2 +- .../atomic_action/coordinated_pickment.py | 2 +- .../atomic_action/coordinated_placement.py | 2 +- scripts/tutorials/atomic_action/hand_over.py | 2 +- .../atomic_action/move_end_effector.py | 3 +-- .../atomic_action/move_held_object.py | 3 +-- .../tutorials/atomic_action/move_joints.py | 3 +-- .../atomic_action/moving_target_recovery.py | 3 +-- scripts/tutorials/atomic_action/open_door.py | 2 +- scripts/tutorials/atomic_action/pickup.py | 3 +-- scripts/tutorials/atomic_action/place.py | 3 +-- scripts/tutorials/atomic_action/pour.py | 6 ++++-- scripts/tutorials/atomic_action/press.py | 2 +- scripts/tutorials/atomic_action/slide.py | 2 +- .../tutorials/atomic_action/tutorial_utils.py | 7 ++++--- scripts/tutorials/atomic_action/twist.py | 2 +- .../sim/atomic_actions/test_tutorial_utils.py | 19 ++++++++++++++++++- 21 files changed, 55 insertions(+), 37 deletions(-) diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index c06664132..d772b34c1 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -1064,8 +1064,10 @@ python scripts/tutorials/atomic_action/hand_over.py --headless --auto_play --dev ``` The scripts share a `--planner` selector for the non-neural backends -(`toppra`, `trapezoidal`, or `curobo`). See {doc}`/tutorial/atomic_actions` for -backend-specific caveats and examples. +(`toppra`, `trapezoidal`, or `curobo`); `trapezoidal` is the default. See +{doc}`/tutorial/atomic_actions` for backend-specific caveats and examples. +The dynamic-obstacle recovery example remains cuRobo-only because it updates a +live collision world. See {doc}`/tutorial/atomic_actions` for engine setup, static compilation, closed-loop execution, effect verification, and custom-action guidance. diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index ce7300f97..9cc44394f 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -175,11 +175,12 @@ It selects the non-neural backend used to construct its - Behavior and availability * - ``toppra`` - :class:`~embodichain.lab.sim.motion.planners.ToppraPlanner` - - Time-optimal joint-space timing. This remains the default for tutorials - that previously used TOPPRA. + - Time-optimal joint-space timing. Select this explicitly when comparing + against the trapezoidal default. * - ``trapezoidal`` - :class:`~embodichain.lab.sim.motion.planners.TrapezoidalPlanner` - - Deterministic trapezoidal joint-space timing. Cartesian targets are + - Deterministic trapezoidal joint-space timing and the default for atomic + action tutorials. Cartesian targets are converted through the shared IK/interpolation path before planning; quantity sampling retains every converted waypoint. When options are resolved from the backend-neutral request, ``MotionGenerator`` treats @@ -190,18 +191,20 @@ It selects the non-neural backend used to construct its * - ``curobo`` - :class:`~embodichain.lab.sim.motion.planners.CuroboPlanner` - CUDA-backed Cartesian/joint planning with collision-world support when - the tutorial supplies one. This remains the default for the tutorials - that previously used cuRobo. + the tutorial supplies one. Select this explicitly when CUDA-backed + collision-aware planning is needed. ``NeuralPlanner`` is intentionally not a choice here: it requires a tutorial-specific ONNX model and frame configuration rather than being a drop-in backend for these examples. For example, the same pose tutorial can be run with each supported backend as follows: +Unless noted below, omitting ``--planner`` selects ``trapezoidal``. + .. code-block:: bash + python scripts/tutorials/atomic_action/move_end_effector.py --device cpu python scripts/tutorials/atomic_action/move_end_effector.py --device cpu --planner toppra - python scripts/tutorials/atomic_action/move_end_effector.py --device cpu --planner trapezoidal python scripts/tutorials/atomic_action/move_end_effector.py --device cuda --planner curobo The selector does not override a skill's explicit motion contract. In diff --git a/scripts/tutorials/atomic_action/assemble.py b/scripts/tutorials/atomic_action/assemble.py index 8c10d7cab..5d2ac3ce8 100644 --- a/scripts/tutorials/atomic_action/assemble.py +++ b/scripts/tutorials/atomic_action/assemble.py @@ -134,7 +134,6 @@ def parse_arguments() -> argparse.Namespace: "visualize_axes", ), default_renderer="hybrid", - default_planner="curobo", ) return parser.parse_args() @@ -260,7 +259,7 @@ def run_assemble_demo( ) motion_gen = create_curobo_motion_generator( robot, - planner=getattr(args, "planner", "curobo"), + planner=getattr(args, "planner", "trapezoidal"), ) left_open, left_close = get_hand_open_close_qpos( robot, hand_control_part="left_hand", close_qpos=HAND_CLOSE_QPOS diff --git a/scripts/tutorials/atomic_action/axis_align.py b/scripts/tutorials/atomic_action/axis_align.py index a41466329..c73369b6f 100644 --- a/scripts/tutorials/atomic_action/axis_align.py +++ b/scripts/tutorials/atomic_action/axis_align.py @@ -152,7 +152,7 @@ def main() -> None: initialize_pre_pick_robot_pose(robot, obj, hand_open) motion_gen = create_toppra_motion_generator( robot, - planner=getattr(args, "planner", "toppra"), + planner=getattr(args, "planner", "trapezoidal"), ) engine = create_simulation_atomic_action_engine( diff --git a/scripts/tutorials/atomic_action/control_dt.py b/scripts/tutorials/atomic_action/control_dt.py index 3e062a32d..bdac7f5b0 100644 --- a/scripts/tutorials/atomic_action/control_dt.py +++ b/scripts/tutorials/atomic_action/control_dt.py @@ -67,7 +67,7 @@ def main() -> None: engine = AtomicActionEngine( motion_generator=create_toppra_motion_generator( robot, - planner=getattr(args, "planner", "toppra"), + planner=getattr(args, "planner", "trapezoidal"), ) ) diff --git a/scripts/tutorials/atomic_action/coordinated_pickment.py b/scripts/tutorials/atomic_action/coordinated_pickment.py index 98fa7bdbf..205a57793 100644 --- a/scripts/tutorials/atomic_action/coordinated_pickment.py +++ b/scripts/tutorials/atomic_action/coordinated_pickment.py @@ -386,7 +386,7 @@ def run_coordinated_pickment_demo( left_to_right_arm_direction = compute_left_to_right_arm_direction(robot, sim.device) motion_gen = create_toppra_motion_generator( robot, - planner=getattr(args, "planner", "toppra"), + planner=getattr(args, "planner", "trapezoidal"), ) hand_close_qpos = ( diff --git a/scripts/tutorials/atomic_action/coordinated_placement.py b/scripts/tutorials/atomic_action/coordinated_placement.py index 36db35f0b..59ff454b9 100644 --- a/scripts/tutorials/atomic_action/coordinated_placement.py +++ b/scripts/tutorials/atomic_action/coordinated_placement.py @@ -552,7 +552,7 @@ def run_coordinated_placement_demo( pan_semantics = create_manual_object_semantics(pan, PAN_LABEL) motion_gen = create_toppra_motion_generator( robot, - planner=getattr(args, "planner", "toppra"), + planner=getattr(args, "planner", "trapezoidal"), ) right_open, right_close = get_hand_open_close_qpos( diff --git a/scripts/tutorials/atomic_action/hand_over.py b/scripts/tutorials/atomic_action/hand_over.py index 7d6d5b1ec..5f00cf879 100644 --- a/scripts/tutorials/atomic_action/hand_over.py +++ b/scripts/tutorials/atomic_action/hand_over.py @@ -192,7 +192,7 @@ def run_handover_demo( object_semantics = create_antipodal_semantics(obj, label="handover") motion_gen = create_toppra_motion_generator( robot, - planner=getattr(args, "planner", "toppra"), + planner=getattr(args, "planner", "trapezoidal"), ) left_open, left_close = get_hand_open_close_qpos( diff --git a/scripts/tutorials/atomic_action/move_end_effector.py b/scripts/tutorials/atomic_action/move_end_effector.py index 321923746..f5180f177 100644 --- a/scripts/tutorials/atomic_action/move_end_effector.py +++ b/scripts/tutorials/atomic_action/move_end_effector.py @@ -56,7 +56,6 @@ def parse_arguments() -> argparse.Namespace: parser = create_tutorial_argument_parser( "Demonstrate MoveEndEffector with a multi-waypoint pose trajectory.", features=("visualize_axes",), - default_planner="curobo", ) return parser.parse_args() @@ -68,7 +67,7 @@ def main() -> None: robot = add_tutorial_robot(sim, args.robot) motion_gen = create_curobo_motion_generator( robot, - planner=getattr(args, "planner", "curobo"), + planner=getattr(args, "planner", "trapezoidal"), ) engine = AtomicActionEngine(motion_generator=motion_gen) diff --git a/scripts/tutorials/atomic_action/move_held_object.py b/scripts/tutorials/atomic_action/move_held_object.py index ba266630b..27800a77e 100644 --- a/scripts/tutorials/atomic_action/move_held_object.py +++ b/scripts/tutorials/atomic_action/move_held_object.py @@ -74,7 +74,6 @@ def parse_arguments() -> argparse.Namespace: parser = create_tutorial_argument_parser( "Pick up a paper cup and move it by object pose.", features=("grasp_sampling", "visualize_axes"), - default_planner="curobo", ) return parser.parse_args() @@ -121,7 +120,7 @@ def main() -> None: obj = create_pick_object(sim) motion_gen = create_curobo_motion_generator( robot, - planner=getattr(args, "planner", "curobo"), + planner=getattr(args, "planner", "trapezoidal"), ) hand_open, hand_close = get_hand_open_close_qpos(robot) diff --git a/scripts/tutorials/atomic_action/move_joints.py b/scripts/tutorials/atomic_action/move_joints.py index 60ed84f71..e0d7f4030 100644 --- a/scripts/tutorials/atomic_action/move_joints.py +++ b/scripts/tutorials/atomic_action/move_joints.py @@ -55,7 +55,6 @@ def parse_arguments() -> argparse.Namespace: parser = create_tutorial_argument_parser( "Demonstrate MoveJoints with named and explicit qpos targets.", features=("visualize_axes",), - default_planner="curobo", ) return parser.parse_args() @@ -67,7 +66,7 @@ def main() -> None: robot = add_tutorial_robot(sim, args.robot) motion_gen = create_curobo_motion_generator( robot, - planner=getattr(args, "planner", "curobo"), + planner=getattr(args, "planner", "trapezoidal"), ) home = robot.get_qpos(name="arm")[0].clone() diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index 5b53ea20c..9682f0b6c 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -230,7 +230,6 @@ def parse_arguments() -> argparse.Namespace: """Parse command-line arguments for the moving-target tutorial.""" parser = create_tutorial_argument_parser( "Demonstrate ExecutionRunner replanning after a visible target move.", - default_planner="curobo", ) parser.add_argument( "--no_target_motion", @@ -256,7 +255,7 @@ def main() -> None: ) motion_gen = create_curobo_motion_generator( robot, - planner=getattr(args, "planner", "curobo"), + planner=getattr(args, "planner", "trapezoidal"), ) hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, target, hand_open) diff --git a/scripts/tutorials/atomic_action/open_door.py b/scripts/tutorials/atomic_action/open_door.py index af2c971ed..da5c8c982 100644 --- a/scripts/tutorials/atomic_action/open_door.py +++ b/scripts/tutorials/atomic_action/open_door.py @@ -163,7 +163,7 @@ def main() -> None: engine = AtomicActionEngine( motion_generator=create_toppra_motion_generator( robot, - planner=getattr(args, "planner", "toppra"), + planner=getattr(args, "planner", "trapezoidal"), ), control_profiles={ "hand": ControlPartCommandProfile.joint_positions( diff --git a/scripts/tutorials/atomic_action/pickup.py b/scripts/tutorials/atomic_action/pickup.py index 88df9092f..c1e8b8b14 100644 --- a/scripts/tutorials/atomic_action/pickup.py +++ b/scripts/tutorials/atomic_action/pickup.py @@ -73,7 +73,6 @@ def parse_arguments() -> argparse.Namespace: parser = create_tutorial_argument_parser( "Demonstrate PickUp on a cube.", features=("grasp_sampling", "visualize_axes"), - default_planner="curobo", ) parser.add_argument( "--approach", choices=[*APPROACH_DIRECTIONS, "custom"], default="top" @@ -132,7 +131,7 @@ def main() -> None: initialize_pre_pick_robot_pose(robot, obj, hand_open) motion_gen = create_curobo_motion_generator( robot, - planner=getattr(args, "planner", "curobo"), + planner=getattr(args, "planner", "trapezoidal"), ) engine = create_simulation_atomic_action_engine( motion_generator=motion_gen, diff --git a/scripts/tutorials/atomic_action/place.py b/scripts/tutorials/atomic_action/place.py index 96d34ff01..a7bf82863 100644 --- a/scripts/tutorials/atomic_action/place.py +++ b/scripts/tutorials/atomic_action/place.py @@ -74,7 +74,6 @@ def parse_arguments() -> argparse.Namespace: parser = create_tutorial_argument_parser( "Pick up a cube and place it at a target pose.", features=("grasp_sampling", "visualize_axes"), - default_planner="curobo", ) return parser.parse_args() @@ -128,7 +127,7 @@ def main() -> None: obj = create_pick_object(sim) motion_gen = create_curobo_motion_generator( robot, - planner=getattr(args, "planner", "curobo"), + planner=getattr(args, "planner", "trapezoidal"), ) hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, obj, hand_open) diff --git a/scripts/tutorials/atomic_action/pour.py b/scripts/tutorials/atomic_action/pour.py index 5b46f791e..34c2ef0e4 100644 --- a/scripts/tutorials/atomic_action/pour.py +++ b/scripts/tutorials/atomic_action/pour.py @@ -94,7 +94,9 @@ def parse_arguments() -> argparse.Namespace: return parser.parse_args() -def _create_pick_motion_policy(planner: TutorialPlanner = "toppra") -> MotionPolicy: +def _create_pick_motion_policy( + planner: TutorialPlanner = "trapezoidal", +) -> MotionPolicy: """Create a planner-specific PickUp policy with a fixed motion sample count.""" if planner == "toppra": plan_opts = ToppraPlanOptions( @@ -130,7 +132,7 @@ def main() -> None: ) hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, obj, hand_open) - planner = getattr(args, "planner", "toppra") + planner = getattr(args, "planner", "trapezoidal") motion_gen = create_toppra_motion_generator(robot, planner=planner) engine = create_simulation_atomic_action_engine( diff --git a/scripts/tutorials/atomic_action/press.py b/scripts/tutorials/atomic_action/press.py index b920a21f8..930712c7f 100644 --- a/scripts/tutorials/atomic_action/press.py +++ b/scripts/tutorials/atomic_action/press.py @@ -187,7 +187,7 @@ def main() -> None: hand_open, hand_close = get_hand_open_close_qpos(robot, close_qpos=0.040) motion_gen = create_toppra_motion_generator( robot, - planner=getattr(args, "planner", "toppra"), + planner=getattr(args, "planner", "trapezoidal"), ) semantics, target_pose = create_button_semantics(target) affordance = semantics.affordance diff --git a/scripts/tutorials/atomic_action/slide.py b/scripts/tutorials/atomic_action/slide.py index 09b753df5..4ce378345 100644 --- a/scripts/tutorials/atomic_action/slide.py +++ b/scripts/tutorials/atomic_action/slide.py @@ -193,7 +193,7 @@ def main() -> None: hand_open, hand_close = get_hand_open_close_qpos(robot) motion_gen = create_toppra_motion_generator( robot, - planner=getattr(args, "planner", "toppra"), + planner=getattr(args, "planner", "trapezoidal"), ) semantics = create_drawer_semantics(drawer) affordance = semantics.affordance diff --git a/scripts/tutorials/atomic_action/tutorial_utils.py b/scripts/tutorials/atomic_action/tutorial_utils.py index b7c9d4666..bdd33897c 100644 --- a/scripts/tutorials/atomic_action/tutorial_utils.py +++ b/scripts/tutorials/atomic_action/tutorial_utils.py @@ -136,7 +136,7 @@ def create_tutorial_argument_parser( features: Collection[TutorialCliFeature] = (), default_device: str | None = None, default_renderer: str | None = None, - default_planner: TutorialPlanner = "toppra", + default_planner: TutorialPlanner = "trapezoidal", ) -> argparse.ArgumentParser: """Create a launcher parser with the shared atomic-tutorial switches. @@ -145,7 +145,8 @@ def create_tutorial_argument_parser( features: Optional groups of tutorial-specific shared arguments. default_device: Optional device override for the launcher arguments. default_renderer: Optional renderer override for the launcher arguments. - default_planner: Planner selected when ``--planner`` is omitted. + default_planner: Planner selected when ``--planner`` is omitted. The + shared default is deterministic ``trapezoidal`` timing. Returns: The configured argument parser. @@ -354,7 +355,7 @@ def add_tutorial_robot( def create_tutorial_motion_generator( robot: Robot, - planner: TutorialPlanner = "toppra", + planner: TutorialPlanner = "trapezoidal", ) -> MotionGenerator: """Create a selected non-neural motion generator for a tutorial robot. diff --git a/scripts/tutorials/atomic_action/twist.py b/scripts/tutorials/atomic_action/twist.py index 18a3b3865..e79c034c8 100644 --- a/scripts/tutorials/atomic_action/twist.py +++ b/scripts/tutorials/atomic_action/twist.py @@ -174,7 +174,7 @@ def main() -> None: hand_open, hand_close = get_hand_open_close_qpos(robot) motion_gen = create_toppra_motion_generator( robot, - planner=getattr(args, "planner", "toppra"), + planner=getattr(args, "planner", "trapezoidal"), ) semantics, target_pose = create_knob_semantics(target) diff --git a/tests/sim/atomic_actions/test_tutorial_utils.py b/tests/sim/atomic_actions/test_tutorial_utils.py index 8f965ba2f..098905fb2 100644 --- a/tests/sim/atomic_actions/test_tutorial_utils.py +++ b/tests/sim/atomic_actions/test_tutorial_utils.py @@ -729,6 +729,18 @@ def test_tutorial_motion_generator_factory_rejects_neural_backend() -> None: ) # type: ignore[arg-type] +def test_tutorial_motion_generator_factory_defaults_to_trapezoidal() -> None: + robot = MagicMock(uid="tutorial_robot") + + with patch( + "scripts.tutorials.atomic_action.tutorial_utils.MotionGenerator" + ) as motion_generator_cls: + create_tutorial_motion_generator(robot) + + cfg = motion_generator_cls.call_args.kwargs["cfg"] + assert cfg.planner_cfg.planner_type == "trapezoidal" + + 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([]) @@ -741,7 +753,7 @@ def test_shared_robot_selection_keeps_ur5_default_and_accepts_all_variants() -> assert default_args.robot == "ur5" assert franka_args.robot == "franka" assert ur10_args.robot == "ur10" - assert default_args.planner == "toppra" + assert default_args.planner == "trapezoidal" assert trapezoidal_args.planner == "trapezoidal" @@ -790,6 +802,10 @@ def test_all_atomic_action_tutorials_accept_both_robot_choices( assert default_args.robot == "ur5" assert franka_args.robot == "franka" + expected_planner = ( + "curobo" if module_name == "dynamic_obstacle_recovery" else "trapezoidal" + ) + assert default_args.planner == expected_planner assert trapezoidal_args.planner == "trapezoidal" @@ -914,6 +930,7 @@ def test_pour_tutorial_uses_configured_pickup_and_local_rotation_axis() -> None: assert module.POUR_INTERNAL_AXIS == (1.0, 0.0, 0.0) pick_policy = module._create_pick_motion_policy() assert pick_policy.sample_count == module.PICK_SAMPLE_INTERVAL + assert isinstance(pick_policy.plan_opts, module.TrapezoidalPlanOptions) assert pick_policy.plan_opts.sample_method is module.TrajectorySampleMethod.QUANTITY assert pick_policy.plan_opts.sample_interval == module.PICK_MOTION_SAMPLE_COUNT From fdac5aa76fdbf854c245487374a974b6e537f9ae Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 12 Sep 2026 05:34:24 +0000 Subject: [PATCH 3/3] feat(atomic-actions): use one global sun light in tutorials --- .../sim/atomic_actions/builtin_actions.md | 3 ++ docs/source/tutorial/atomic_actions.rst | 5 ++++ scripts/tutorials/atomic_action/assemble.py | 1 - .../atomic_action/coordinated_pickment.py | 1 - .../atomic_action/coordinated_placement.py | 1 - scripts/tutorials/atomic_action/hand_over.py | 1 - .../tutorials/atomic_action/tutorial_utils.py | 12 ++++---- .../sim/atomic_actions/test_tutorial_utils.py | 29 +++++++++++++++++++ 8 files changed, 44 insertions(+), 9 deletions(-) diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index d772b34c1..7334653e1 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -1068,6 +1068,9 @@ The scripts share a `--planner` selector for the non-neural backends {doc}`/tutorial/atomic_actions` for backend-specific caveats and examples. The dynamic-obstacle recovery example remains cuRobo-only because it updates a live collision world. +Every script also receives the same single global `sun` light from the shared +tutorial scene setup; vectorized environments do not create per-arena point +lights. See {doc}`/tutorial/atomic_actions` for engine setup, static compilation, closed-loop execution, effect verification, and custom-action guidance. diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index 9cc44394f..8b41bae3b 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -159,6 +159,11 @@ The ``motion_generator`` variable in the snippets below is a configured :class:`~embodichain.lab.sim.motion.motion_generator.MotionGenerator`; its robot, planner, device, cache, and collision world become the resources owned by the engine. +All atomic-action tutorials use the shared scene setup, which creates exactly +one global ``sun`` light (``main_light``) for the whole simulation. The sun +uses a common downward direction and is not duplicated for each vectorized +arena; tutorial-specific light positions are no longer needed. + Selecting a motion planner ~~~~~~~~~~~~~~~~~~~~~~~~~~ diff --git a/scripts/tutorials/atomic_action/assemble.py b/scripts/tutorials/atomic_action/assemble.py index 5d2ac3ce8..af32f79a6 100644 --- a/scripts/tutorials/atomic_action/assemble.py +++ b/scripts/tutorials/atomic_action/assemble.py @@ -385,7 +385,6 @@ def main() -> None: sim = create_tutorial_simulation( args, arena_space=3.0, - light_pos=(0.0, -0.4, 3.0), ) robot = create_dual_robot(sim, args.robot) run_assemble_demo(args, sim, robot) diff --git a/scripts/tutorials/atomic_action/coordinated_pickment.py b/scripts/tutorials/atomic_action/coordinated_pickment.py index 205a57793..2b3ab54f6 100644 --- a/scripts/tutorials/atomic_action/coordinated_pickment.py +++ b/scripts/tutorials/atomic_action/coordinated_pickment.py @@ -524,7 +524,6 @@ def main() -> None: sim = create_tutorial_simulation( args, arena_space=3.0, - light_pos=(0.0, -0.4, 3.0), ) robot = create_dual_robot(sim, args.robot) run_coordinated_pickment_demo(args, sim, robot) diff --git a/scripts/tutorials/atomic_action/coordinated_placement.py b/scripts/tutorials/atomic_action/coordinated_placement.py index 59ff454b9..9ffb55e14 100644 --- a/scripts/tutorials/atomic_action/coordinated_placement.py +++ b/scripts/tutorials/atomic_action/coordinated_placement.py @@ -865,7 +865,6 @@ def main() -> None: sim = create_tutorial_simulation( args, arena_space=3.0, - light_pos=(0.0, -0.4, 3.0), ) robot = create_dual_robot(sim, args.robot) run_coordinated_placement_demo(args, sim, robot) diff --git a/scripts/tutorials/atomic_action/hand_over.py b/scripts/tutorials/atomic_action/hand_over.py index 5f00cf879..164f529e5 100644 --- a/scripts/tutorials/atomic_action/hand_over.py +++ b/scripts/tutorials/atomic_action/hand_over.py @@ -296,7 +296,6 @@ def main() -> None: sim = create_tutorial_simulation( args, arena_space=3.0, - light_pos=(0.0, -0.4, 3.0), ) robot = create_dual_robot(sim, args.robot) run_handover_demo(args, sim, robot) diff --git a/scripts/tutorials/atomic_action/tutorial_utils.py b/scripts/tutorials/atomic_action/tutorial_utils.py index bdd33897c..2791acc42 100644 --- a/scripts/tutorials/atomic_action/tutorial_utils.py +++ b/scripts/tutorials/atomic_action/tutorial_utils.py @@ -94,7 +94,7 @@ palm_depth=0.096, ) DEFAULT_GRIPPER_CLOSE_QPOS = 0.036 -DEFAULT_TUTORIAL_LIGHT_POS = (1.0, 0.0, 3.0) +DEFAULT_TUTORIAL_SUN_DIRECTION = (0.0, 0.0, -1.0) _FRANKA_TUTORIAL_BASE_ROTATION = (0.0, 0.0, 180.0) _DEFAULT_GRIPPER_TCP_Z = 0.17 _GRIPPER_TCP = ( @@ -224,7 +224,7 @@ def create_tutorial_simulation( args: argparse.Namespace, *, arena_space: float = 2.5, - light_pos: Sequence[float] = DEFAULT_TUTORIAL_LIGHT_POS, + sun_direction: Sequence[float] = DEFAULT_TUTORIAL_SUN_DIRECTION, ) -> SimulationManager: """Create the shared simulation setup used by atomic-action tutorials. @@ -232,7 +232,8 @@ def create_tutorial_simulation( args: Parsed launcher arguments containing environment count, device, and renderer selections. arena_space: Spacing between parallel simulation arenas in meters. - light_pos: Position of the scene's key light. + sun_direction: Direction of the single global sun light. The vector + points from the light toward the scene. Returns: A simulation manager with the tutorial key light configured. @@ -253,9 +254,10 @@ def create_tutorial_simulation( sim.add_light( cfg=LightCfg( uid="main_light", + light_type="sun", color=(0.6, 0.6, 0.6), intensity=30.0, - init_pos=list(light_pos), + direction=tuple(sun_direction), ) ) return sim @@ -1224,7 +1226,7 @@ def create_tutorial_robot_cfg( "DEFAULT_AXIS_LEN", "DEFAULT_AXIS_SIZE", "DEFAULT_GRIPPER_CLOSE_QPOS", - "DEFAULT_TUTORIAL_LIGHT_POS", + "DEFAULT_TUTORIAL_SUN_DIRECTION", "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 098905fb2..88bd4a33c 100644 --- a/tests/sim/atomic_actions/test_tutorial_utils.py +++ b/tests/sim/atomic_actions/test_tutorial_utils.py @@ -48,6 +48,7 @@ create_dual_tutorial_robot_cfg, ) from scripts.tutorials.atomic_action.tutorial_utils import ( + DEFAULT_TUTORIAL_SUN_DIRECTION, ROBOTIQ_2F_140_TCP, ROBOTIQ_HAND_JOINT_PATTERN, TUTORIAL_PLANNERS, @@ -62,6 +63,7 @@ create_parallel_jaw_grasp_pose_generator, create_tutorial_argument_parser, create_tutorial_motion_generator, + create_tutorial_simulation, create_tutorial_robot_cfg, create_ur10_robotiq_robot_cfg, create_ur5_gripper_robot_cfg, @@ -741,6 +743,33 @@ def test_tutorial_motion_generator_factory_defaults_to_trapezoidal() -> None: assert cfg.planner_cfg.planner_type == "trapezoidal" +def test_tutorial_simulation_uses_one_global_sun_light() -> None: + args = Namespace(num_envs=4, device="cpu", renderer="hybrid") + simulation = MagicMock() + + with ( + patch( + "scripts.tutorials.atomic_action.tutorial_utils.SimulationManager", + return_value=simulation, + ), + patch("scripts.tutorials.atomic_action.tutorial_utils.SimulationManagerCfg"), + patch("scripts.tutorials.atomic_action.tutorial_utils.RenderCfg"), + patch("scripts.tutorials.atomic_action.tutorial_utils.LightCfg") as light_cfg, + patch( + "scripts.tutorials.atomic_action.tutorial_utils.visualization_cfg_from_args" + ), + ): + result = create_tutorial_simulation(args) + + assert result is simulation + simulation.add_light.assert_called_once_with(cfg=light_cfg.return_value) + light_kwargs = light_cfg.call_args.kwargs + assert light_kwargs["uid"] == "main_light" + assert light_kwargs["light_type"] == "sun" + assert light_kwargs["direction"] == DEFAULT_TUTORIAL_SUN_DIRECTION + assert "init_pos" not in light_kwargs + + 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([])