diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 358e32fd3..62dc9d03d 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -169,10 +169,10 @@ from the source after adaptation, including near-identical zero-time joins. Original action joins and intervals are restored before replay so callbacks retain their source indices. MoveJoint compiles each required joint target as a separate invocation, preserving its ordered waypoints as action boundaries -through control-grid retiming and spatial expansion. Pour's single -rotation/return segment is protected from spatial perturbation because its -required interior target has no exported -waypoint boundary; its preceding PickUp free phases remain editable. Slide +through control-grid retiming and spatial expansion. Pour exports rotation, +apex dwell and return boundaries; its constrained held-object phases are +protected from spatial perturbation while the preceding PickUp free phases +remain editable. Slide replans its push from each row's robot/handle observations after pull. Every Slide reference remains row-local through expansion and replay, including its start, locked contact path, timing and sampling provenance; shared affordance diff --git a/agent_context/topics/atomic-actions/execution.md b/agent_context/topics/atomic-actions/execution.md index 54f0c4b12..a5e0e4dbd 100644 --- a/agent_context/topics/atomic-actions/execution.md +++ b/agent_context/topics/atomic-actions/execution.md @@ -111,19 +111,15 @@ resample fractional durations. `MotionPolicy` forwards the explicit strategy unchanged. `ik_interp` rejects backend `plan_opts`; `motion_gen` always invokes the backend. Segments requesting -`cartesian_linear=True` preserve caller-supplied Cartesian samples and currently -require `ik_interp`, as used by Slide. A planner policy for such a segment -raises instead of silently bypassing backend timing. OpenDoor and PushObject -request straight contact segments only under `ik_interp`; under `motion_gen` -they send each contact segment's endpoint to the backend. -Press supports both strategies: `ik_interp` preserves the contact/press/retract -Cartesian samples; `motion_gen` sends approach/contact/press/retract targets to -the selected backend in one ordered request, then aligns the three internal -boundaries chronologically on native samples. +`cartesian_linear=True` preserve caller-supplied Cartesian samples and require +`ik_interp`. PushObject, Slide, Press, OpenDoor and Twist fail closed under +`motion_gen`: endpoint-only backend planning cannot certify their straight-line +or articulated contact paths, and the action must not silently fall back to IK. Each phase retains its native time profile while retiming to the control grid; requested phase counts are minima, so long phases expand rather than speeding -up. Press validates its final assembled arm samples through a capable backend -and rejects collision-aware backend paths without that validation capability. +up. Composite phases use terminal holds to align lengths; they do not +distance-resample a longer native plan back into the requested count. Press +validates its final assembled arm samples through a capable backend. Exact validation needs poses for every configured dynamic obstacle. Missing poses fail the plan with diagnostics; they never permit unchecked success. `dynamic_collision_mode=off` forbids filling missing poses from the scene, so @@ -135,11 +131,28 @@ endpoints, or a fallback to IK. 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. 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. +and now-invalid native acceleration samples are discarded. Default `ik_interp` +policies use rest-to-rest minimum-jerk joint segments. Omitted velocity follows +the Robot's ordered per-joint caps, while an explicit scalar request is +intersected with those caps; acceleration remains a planner-owned scalar budget. +`sample_count` remains a lower bound. Exact Cartesian +paths use one global minimum-jerk progress law and batch IK for every expanded +sample, so enforcing limits does not replace contact geometry with joint-space +interpolation. Articulated paths whose geometry cannot be reconstructed from +Cartesian chords provide an exact normalized-progress sampler; OpenDoor and +Twist regenerate every expanded pose around their configured external axis. +CoordinatedPickment +uses sparse IK only as a seed, then batch-projects every shared-object waypoint +for both arms so joint interpolation cannot break the closed chain. + +HandOver plans the two arm assignments from independent phase-length maps. A +mixed batch terminal-pads corresponding phases to their shared maximum before +selecting rows, which preserves handover boundaries when one assignment needs +more retimed samples. It holds both closed hands for `hold_steps` before source +release. Pour +splits rotation and return at the poured pose and inserts a physical-time dwell. +Actions preserving an existing grasp command the observed hand state until an +explicit release; free-hand approaches interpolate from the observed state. Tracking recovery is separate from task-level semantic recovery: @@ -154,14 +167,18 @@ Tracking recovery is separate from task-level semantic recovery: default) or `zero` (explicit position-only control). Configured execution policies decode the same field under `motion`. `AtomicAction.build_plan` derives missing velocities after composite skills -have assembled their final full-robot positions and timing. Stationary joint -intervals and terminal settling commands use zero velocity; failed rows remain -held. MoveHeldObject and Pour preserve native arm derivatives when embedding -unchanged timed planner output. Differentiation does not retime composite -paths to enforce robot velocity/acceleration limits. +have assembled their final full-robot positions and timing. The first command, +both ends of every stationary joint interval, and terminal settling commands +use zero velocity; failed rows remain held. `SimulationExecutionAdapter` always clears absent velocity references to zero; it never substitutes observed velocities or depends on peer active masks. It writes position and velocity targets through the existing `set_qpos` and `set_qvel` APIs. Inactive rows hold observed positions with zero target velocity. A nonzero damping gain is needed for the velocity error to contribute to tracking. +Active rows in one runtime frame must share one positive hold duration; zero is +reserved for terminal padding. The runner rejects mismatched durations instead +of choosing the maximum. Simulation control periods and sleeps must be integer +multiples of `physics_dt`; the adapter never rounds a motion-command request +upward. Passive runner polling, including physical-effect waits, rounds the +configured minimum cycle up to an advertised clock quantum before sleeping. diff --git a/agent_context/topics/ik-solvers/ik-solvers.md b/agent_context/topics/ik-solvers/ik-solvers.md index 5329d1a35..8257c1aab 100644 --- a/agent_context/topics/ik-solvers/ik-solvers.md +++ b/agent_context/topics/ik-solvers/ik-solvers.md @@ -55,10 +55,11 @@ transforms over those axes without materializing per-target copies. Concrete `get_ik()` APIs retain their own candidate and failure shapes. Pink returns `(N, 1, dof)`. Pytorch's normal nearest path retains a singleton -candidate axis and its all-solutions path returns `(N, samples, dof)`, but its -early all-targets-failed path returns `(N, dof)`. Do not squeeze blindly or -assume all-solutions masks share one universal shape. Preserve batch size one; -prefer the batch adapter when only the nearest solution is needed. +candidate axis when successful and returns `(N, dof)` when every target fails. +Its all-solutions path always returns validity `(N, samples)` and positions +`(N, samples, dof)`, including the early all-targets-failed path. Do not squeeze +blindly or assume all-solutions masks share one universal shape. Preserve batch +size one; prefer the batch adapter when only the nearest solution is needed. ## Continuous batch IK @@ -71,6 +72,10 @@ A supporting solver supplies all-candidate IK and `_select_continuous_ik_path`. The hook consumes candidates `(B, N, K, dof)`, validity `(B, N, K)` and initial seeds `(B, dof)`, returning validity `(B, N)` and a path `(B, N, dof)`. Candidate count `K` belongs to the solver; OPW implements this capability. +Pytorch also implements it by choosing each valid multi-start candidate nearest +to the preceding selected posture under `ik_nearest_weight`; this continuous +path selection is independent of its ordinary per-target +`ik_solution_selection` policy. ## Computation and state ownership diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index fb6c867dc..8162f4e5d 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -48,8 +48,10 @@ Whenever positions exist, `dt` contains per-sample arrival intervals `(B, N)`; Timing does not alter simulator `physics_dt`. `MotionGenOptions.strategy` selects the timing owner. `motion_gen` invokes the -backend; `ik_interp` requires explicit start/count/dt, rejects backend options -and derivative limits, and supports joint/EEF moves. Required Cartesian IK +backend; `ik_interp` requires explicit start/count/dt, rejects backend-specific +options, consumes optional scalar velocity/acceleration limits, and supports +joint/EEF moves. Limited interpolation uses minimum-jerk rest-to-rest segments +and treats the requested count as a lower bound. Required Cartesian IK failures fail the row; waypoints are never discarded to salvage success. Unsupported routes raise instead of silently selecting another strategy. diff --git a/agent_context/topics/motion-planning/planner-details.md b/agent_context/topics/motion-planning/planner-details.md index 51bc129a4..d668f3b68 100644 --- a/agent_context/topics/motion-planning/planner-details.md +++ b/agent_context/topics/motion-planning/planner-details.md @@ -21,7 +21,12 @@ Unconfirmed starts and later poses still require successful IK and FK consistenc `interpolate_trajectory()` raises on any failed row; `generate()` carries per-row success in a normalized `PlanResult`. Fixed Cartesian sample preservation requires explicit `ik_interp`; -it is not an implicit fallback from backend planning. +it is not an implicit fallback from backend planning. When velocity or +acceleration limits expand such a path, `MotionGenOptions.cartesian_path_sampler` +may regenerate poses from normalized minimum-jerk progress. The callback must +end at the requested target, begin within the active IK solver's accepted pose +tolerance, and preserve the batch, dtype, and device. Without it, the facade +resamples the caller-supplied Cartesian polyline. Preserve these coupled result rules in `motion_generator.py`: @@ -115,6 +120,14 @@ row's duration up to the destination clock, samples uniform source-path phase, and recomputes qvel with zero first/last-valid and padded-hold velocities. Source `PlanResult.dt` remains planner timing. +`compute/trajectory/interpolation.py::interpolate_minimum_jerk()` expands joint +keyframes onto a fixed control interval while preserving every keyframe. The +requested sample count is a minimum; per-segment counts grow to satisfy scalar +or ordered per-joint velocity and acceleration limits, and spare samples are +distributed by joint displacement rather than equally across unequal moves. +The emitted dtype is checked on the destination grid so float32 cancellation +cannot silently violate those derivative limits. + `motion/execution.py::play_joint_trajectory()` requires `control_dt` to be an integer multiple of `physics_dt` and advances that many unchanged substeps. Position-plus-velocity command mode is opt-in. Gym execution instead owns its diff --git a/agent_context/topics/task-programs/configuration.md b/agent_context/topics/task-programs/configuration.md index 9bad7e9e9..b04147a4d 100644 --- a/agent_context/topics/task-programs/configuration.md +++ b/agent_context/topics/task-programs/configuration.md @@ -99,6 +99,13 @@ Ownership is explicit rather than a generic deep merge: effect-assurance policy. Planner collision objects are attached from the selected live scene and are not serialized in the policy. +Configured `motion` policies accept optional finite, strictly positive +`velocity_limit` and `acceleration_limit` values. Configured Pour action options +accept a finite, non-negative `dwell_duration`; unknown timing keys still fail +the closed-field decoder. Omitting `velocity_limit` preserves the Robot's +ordered per-joint speed caps; an explicit scalar request is intersected with +those caps rather than replacing them. + Grasp generators select toolkit-owned geometry with a string `model` ID; an inline mapping is a custom calibration and its `model_id` does not trigger catalog lookup. The catalog is owned by diff --git a/embodichain/compute/trajectory/interpolation.py b/embodichain/compute/trajectory/interpolation.py index 1645f5ca4..19b33bfe8 100644 --- a/embodichain/compute/trajectory/interpolation.py +++ b/embodichain/compute/trajectory/interpolation.py @@ -18,11 +18,249 @@ from __future__ import annotations +import math from collections.abc import Sequence import torch -__all__ = ["interpolate_with_distance", "interpolate_with_nums"] +__all__ = [ + "interpolate_with_distance", + "interpolate_with_nums", +] + + +def interpolate_minimum_jerk( + trajectory: torch.Tensor, + minimum_sample_count: int, + step_dt: float, + velocity_limit: float | torch.Tensor, + acceleration_limit: float | torch.Tensor, +) -> torch.Tensor: + """Interpolate joint keyframes with rest-to-rest minimum-jerk segments. + + ``minimum_sample_count`` is a lower bound. Each keyframe interval is + expanded as needed so the analytic peak velocity and acceleration of the + quintic time law stay within the scalar per-joint limits. Every input + keyframe remains an exact output sample. + + Args: + trajectory: Joint keyframes with shape ``(B, N, D)``. + minimum_sample_count: Minimum total number of output samples. + step_dt: Fixed output interval in seconds. + velocity_limit: Positive scalar or ordered per-joint velocity limits. + acceleration_limit: Positive scalar or ordered per-joint acceleration + limits. + + Returns: + Interpolated joint positions with a shared sample count for the batch. + + Raises: + ValueError: If shapes, counts, timing, limits, or values are invalid. + """ + if trajectory.ndim != 3 or not trajectory.is_floating_point(): + raise ValueError("trajectory must be floating point with shape (B, N, D).") + if trajectory.shape[1] == 0 or trajectory.shape[2] == 0: + raise ValueError("trajectory must have non-zero N and D dimensions.") + if not torch.isfinite(trajectory).all(): + raise ValueError("trajectory must contain only finite values.") + if ( + isinstance(minimum_sample_count, bool) + or not isinstance(minimum_sample_count, int) + or minimum_sample_count < trajectory.shape[1] + ): + raise ValueError( + "minimum_sample_count must be an integer at least as large as the " + "keyframe count." + ) + if ( + isinstance(step_dt, bool) + or not isinstance(step_dt, (int, float)) + or not math.isfinite(step_dt) + or step_dt <= 0.0 + ): + raise ValueError("step_dt must be a positive finite number.") + + def limit_tensor( + value: float | torch.Tensor, + name: str, + *, + allow_tensor_zero: bool = False, + ) -> torch.Tensor: + if isinstance(value, bool): + raise ValueError(f"{name} must contain positive finite values.") + limit = torch.as_tensor( + value, + dtype=trajectory.dtype, + device=trajectory.device, + ) + scalar = limit.ndim == 0 + if scalar: + limit = limit.expand(trajectory.shape[2]) + if limit.shape != (trajectory.shape[2],): + raise ValueError( + f"{name} must be scalar or have shape ({trajectory.shape[2]},)." + ) + invalid = ( + (limit < 0.0).any() + if allow_tensor_zero and not scalar + else (limit <= 0.0).any() + ) + if not torch.isfinite(limit).all() or invalid: + raise ValueError(f"{name} must contain positive finite values.") + return limit + + velocity_limits = limit_tensor( + velocity_limit, + "velocity_limit", + allow_tensor_zero=True, + ) + acceleration_limits = limit_tensor(acceleration_limit, "acceleration_limit") + if trajectory.shape[0] == 0: + return trajectory.new_empty((0, minimum_sample_count, trajectory.shape[2])) + if trajectory.shape[1] == 1: + return trajectory.expand(-1, minimum_sample_count, -1).clone() + + delta = torch.abs(trajectory[:, 1:] - trajectory[:, :-1]) + zero_velocity_joints = velocity_limits == 0.0 + if zero_velocity_joints.any() and (delta[..., zero_velocity_joints] > 0.0).any(): + raise ValueError("A zero-velocity joint cannot move between keyframes.") + safe_velocity_limits = torch.where( + zero_velocity_joints, + torch.ones_like(velocity_limits), + velocity_limits, + ) + # For p(u)=10u^3-15u^4+6u^5, max(p')=1.875 and + # max(abs(p''))=10/sqrt(3) ~= 5.7735. + duration_from_velocity = (1.875 * delta / safe_velocity_limits.view(1, 1, -1)).amax( + dim=-1 + ) + duration_from_acceleration = torch.sqrt( + (10.0 / math.sqrt(3.0)) * delta / acceleration_limits.view(1, 1, -1) + ).amax(dim=-1) + required = torch.ceil( + torch.maximum(duration_from_velocity, duration_from_acceleration) + / float(step_dt) + ).to(torch.int64) + interval_counts = required.amax(dim=0).clamp_min(1) + + minimum_intervals = minimum_sample_count - 1 + missing = minimum_intervals - int(interval_counts.sum().item()) + if missing > 0: + # Allocate spare samples by the worst-row joint displacement rather + # than splitting them equally across unequal moves. This keeps peak + # speeds comparable when the caller's minimum count, not a limit, is + # what determines the final duration. + weights = delta.amax(dim=(0, 2)) + if float(weights.sum().item()) == 0.0: + weights = torch.ones_like(weights) + quotas = weights / weights.sum() * missing + additions = torch.floor(quotas).to(torch.int64) + remainder = missing - int(additions.sum().item()) + if remainder: + ranked = torch.argsort(quotas - additions, descending=True, stable=True) + additions[ranked[:remainder]] += 1 + interval_counts += additions + + def build_positions() -> torch.Tensor: + segments = [trajectory[:, :1]] + for index, count in enumerate(interval_counts.tolist()): + blend = _minimum_jerk_progress( + count + 1, + device=trajectory.device, + dtype=torch.float64, + ) + start = trajectory[:, index : index + 1].to(torch.float64) + end = trajectory[:, index + 1 : index + 2].to(torch.float64) + segment = torch.lerp(start, end, blend[1:].view(1, -1, 1)).to( + trajectory.dtype + ) + segment[:, -1] = trajectory[:, index + 1] + segments.append(segment) + return torch.cat(segments, dim=1) + + # Analytic duration is only an initial estimate. Quantization in the + # emitted dtype and finite differencing on the destination clock are the + # actual consumer contract, so validate those derivatives before returning. + for _ in range(8): + positions = build_positions() + interval_velocity = positions.diff(dim=1) / float(step_dt) + boundary_acceleration = torch.cat( + [ + interval_velocity[:, :1], + interval_velocity.diff(dim=1), + -interval_velocity[:, -1:], + ], + dim=1, + ) / float(step_dt) + velocity_ratio = float( + (interval_velocity.abs() / safe_velocity_limits.view(1, 1, -1)) + .amax() + .item() + ) + acceleration_ratio = float( + (boundary_acceleration.abs() / acceleration_limits.view(1, 1, -1)) + .amax() + .item() + ) + scale = max(velocity_ratio, math.sqrt(acceleration_ratio)) + if scale <= 1.0 + 1.0e-5: + return positions + growth = max(1.01, scale * 1.02) + expanded = torch.ceil(interval_counts.to(torch.float64) * growth).to( + torch.int64 + ) + interval_counts = torch.maximum(expanded, interval_counts + 1) + + raise ValueError( + "Unable to satisfy discrete velocity and acceleration limits in the " + "trajectory output dtype." + ) + + +def _minimum_jerk_progress( + sample_count: int, + *, + device: torch.device | str, + dtype: torch.dtype, +) -> torch.Tensor: + """Return stable rest-to-rest quintic progress on a fixed sample grid. + + The polynomial is evaluated symmetrically in float64 before one final cast. + This avoids cancellation near one making dense float32 progress decrease. + + Args: + sample_count: Number of progress samples, including both endpoints. + device: Device for the returned tensor. + dtype: Floating-point output dtype. + + Returns: + Monotonic progress from exactly zero through exactly one. + + Raises: + ValueError: If fewer than two samples or a non-floating dtype is used. + """ + if ( + isinstance(sample_count, bool) + or not isinstance(sample_count, int) + or sample_count < 2 + ): + raise ValueError("sample_count must be an integer at least 2.") + if not dtype.is_floating_point: + raise ValueError("dtype must be floating point.") + tau = torch.linspace( + 0.0, + 1.0, + sample_count, + device=device, + dtype=torch.float64, + ) + reverse_tau = 1.0 - tau + lower = tau**3 * (10.0 + tau * (-15.0 + 6.0 * tau)) + upper = 1.0 - reverse_tau**3 * (10.0 + reverse_tau * (-15.0 + 6.0 * reverse_tau)) + progress = torch.where(tau <= 0.5, lower, upper).to(dtype=dtype) + progress[0] = 0.0 + progress[-1] = 1.0 + return progress def _allocate_segment_intervals( diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index c2c8fa00c..beb82e3ad 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -573,6 +573,8 @@ def build_plan( # and the final settling window must never retain a moving target. stationary = timed.positions[:, 1:] == timed.positions[:, :-1] velocities[:, :-1] = velocities[:, :-1].masked_fill(stationary, 0.0) + velocities[:, 1:] = velocities[:, 1:].masked_fill(stationary, 0.0) + velocities[:, 0] = 0.0 velocities[:, -1] = 0.0 timed = replace(timed, velocities=velocities) diff --git a/embodichain/lab/sim/atomic_actions/policies.py b/embodichain/lab/sim/atomic_actions/policies.py index 6d7338d18..248e447d9 100644 --- a/embodichain/lab/sim/atomic_actions/policies.py +++ b/embodichain/lab/sim/atomic_actions/policies.py @@ -18,6 +18,8 @@ from __future__ import annotations +import math +from collections.abc import Callable from copy import deepcopy from dataclasses import dataclass from enum import Enum @@ -61,7 +63,7 @@ class MotionPolicy: """Motion strategy: ``motion_gen`` or ``ik_interp``.""" sample_count: int = 50 - """Requested trajectory sample count when the backend does not preserve samples.""" + """Minimum trajectory sample count when the backend does not preserve samples.""" velocity_targets: Literal["auto", "zero"] = "auto" """Use trajectory velocities or explicit zero velocity targets.""" @@ -72,6 +74,12 @@ class MotionPolicy: plan_opts: PlanOptions | None = None """Optional typed planner-specific options.""" + velocity_limit: float | None = None + """Optional scalar speed request, capped by effective Robot joint limits.""" + + acceleration_limit: float = 0.5 + """Scalar planner-owned acceleration budget in joint units per second squared.""" + def __post_init__(self) -> None: valid_strategies = {"motion_gen", "ik_interp"} if self.strategy not in valid_strategies: @@ -87,6 +95,12 @@ def __post_init__(self) -> None: raise ValueError("velocity_targets must be auto or zero.") if self.sample_count < 2: raise ValueError("sample_count must be at least 2.") + for name in ("velocity_limit", "acceleration_limit"): + value = getattr(self, name) + if value is None and name == "velocity_limit": + continue + if not math.isfinite(value) or value <= 0.0: + raise ValueError(f"{name} must be finite and greater than zero.") mode = self.dynamic_collision_mode if isinstance(mode, str): try: @@ -111,6 +125,9 @@ def to_motion_gen_options( sample_count: int | None = None, interpolation_dt: float | None = None, cartesian_linear: bool = False, + cartesian_path_sampler: ( + Callable[["torch.Tensor"], "torch.Tensor"] | None + ) = None, ) -> "MotionGenOptions": """Translate this atomic policy into motion-generator options. @@ -122,6 +139,9 @@ def to_motion_gen_options( deterministic interpolation. cartesian_linear: Whether every supplied Cartesian keyframe is a required linear-path sample rather than a sparse endpoint. + cartesian_path_sampler: Optional exact-path sampler receiving + normalized progress values. Use this for constrained curves + such as rotations about an external hinge. Returns: Independently owned options for :class:`MotionGenerator`. @@ -142,6 +162,8 @@ def to_motion_gen_options( return MotionGenOptions( strategy=self.strategy, sample_count=self.sample_count if sample_count is None else sample_count, + velocity_limit=self.velocity_limit, + acceleration_limit=self.acceleration_limit, start_qpos=start_qpos, control_part=control_part, plan_opts=self.plan_opts, @@ -149,6 +171,7 @@ def to_motion_gen_options( interpolation_dt=interpolation_dt, is_linear=cartesian_linear, preserve_cartesian_samples=cartesian_linear, + cartesian_path_sampler=cartesian_path_sampler, ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/_helpers.py b/embodichain/lab/sim/atomic_actions/primitives/_helpers.py index 73768172f..a913a706f 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/_helpers.py +++ b/embodichain/lab/sim/atomic_actions/primitives/_helpers.py @@ -23,7 +23,10 @@ import torch -from embodichain.compute.trajectory import resample_with_distance +from embodichain.compute.trajectory import ( + resample_with_distance, + retime_to_control_grid, +) from ..bindings import EndpointBinding from ..state import PlanningContext @@ -95,25 +98,64 @@ def repeat_qpos(qpos: torch.Tensor, n_waypoints: int) -> torch.Tensor: return qpos.unsqueeze(1).repeat(1, n_waypoints, 1) +def hold_inactive_cartesian_rows( + path: torch.Tensor, + *, + active_mask: torch.Tensor, + hold_pose: torch.Tensor, +) -> torch.Tensor: + """Replace inactive Cartesian path rows with a consistent pose hold. + + Exact-path interpolation validates its Cartesian start against FK of the + supplied joint seed. A row that failed an earlier segment therefore must + not retain the successful row's requested contact geometry. + """ + if path.ndim != 4 or path.shape[2:] != (4, 4): + raise ValueError("path must have shape (B, T, 4, 4).") + if active_mask.dtype != torch.bool or active_mask.shape != (path.shape[0],): + raise ValueError("active_mask must have bool shape (B,).") + if hold_pose.shape != (path.shape[0], 4, 4): + raise ValueError("hold_pose must have shape (B, 4, 4).") + if ( + active_mask.device != path.device + or hold_pose.device != path.device + or hold_pose.dtype != path.dtype + ): + raise ValueError( + "path, active_mask, and hold_pose must share a device, and path and " + "hold_pose must share a dtype." + ) + return torch.where( + active_mask[:, None, None, None], + path, + hold_pose[:, None].expand_as(path), + ) + + def resample_planned_trajectory( trajectory: torch.Tensor, sample_count: int, + *, + dt: torch.Tensor | None = None, + control_dt: float | None = None, ) -> torch.Tensor: - """Fit a planner path to a composite action's fixed phase length. + """Put a planner path on a composite action's control-time grid. 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. + backend-neutral requested count. When timing is supplied, the requested + count is only a lower bound: native duration is quantized without speeding + it up, and any remaining samples are terminal holds. The legacy + position-only behavior remains available to callers that have no timing. Args: trajectory: Batched joint path with shape ``(B, N, D)``. sample_count: Required number of output samples. + dt: Optional native arrival intervals with shape ``(B, N)``. + control_dt: Destination control interval, required together with ``dt``. Returns: - The original path when its length already matches, otherwise a - cumulative-distance resampling with exactly ``sample_count`` samples. + A control-grid path with at least ``sample_count`` samples when timing + is supplied, otherwise exactly ``sample_count`` distance samples. Raises: TypeError: If ``trajectory`` is not a floating-point tensor. @@ -133,6 +175,15 @@ def resample_planned_trajectory( raise ValueError("sample_count must be positive.") if trajectory.shape[0] == 0: return trajectory.new_empty((0, sample_count, trajectory.shape[2])) + if (dt is None) != (control_dt is None): + raise ValueError("dt and control_dt must be supplied together.") + if dt is not None: + assert control_dt is not None + retimed, _, _, _ = retime_to_control_grid(trajectory, dt, control_dt) + if retimed.shape[1] >= sample_count: + return retimed + padding = retimed[:, -1:].expand(-1, sample_count - retimed.shape[1], -1) + return torch.cat([retimed, padding], dim=1) if trajectory.shape[1] == sample_count: return trajectory if trajectory.shape[1] == 1: @@ -194,7 +245,34 @@ 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, resample_planned_trajectory(result.positions, n_waypoints) + if result.dt is None: + raise ValueError("Motion planning result must contain arrival intervals.") + return result.success, resample_planned_trajectory( + result.positions, + n_waypoints, + dt=result.dt, + control_dt=interpolation_dt, + ) + + +def pad_trajectory_to_length( + trajectory: torch.Tensor, + sample_count: int, +) -> torch.Tensor: + """Pad a joint trajectory with its terminal pose to a shared length.""" + if trajectory.ndim != 3 or trajectory.shape[1] == 0: + raise ValueError("trajectory must have shape (B, N, D) with N > 0.") + if sample_count < trajectory.shape[1]: + raise ValueError("sample_count cannot shorten a trajectory.") + if sample_count == trajectory.shape[1]: + return trajectory + return torch.cat( + [ + trajectory, + trajectory[:, -1:].expand(-1, sample_count - trajectory.shape[1], -1), + ], + dim=1, + ) def arm_qpos_from_state( @@ -213,6 +291,8 @@ def split_joint_trajectory_at_pose( control_part: str, first_sample_count: int, second_sample_count: int, + dt: torch.Tensor | None = None, + control_dt: float | None = None, ) -> tuple[torch.Tensor, torch.Tensor]: """Split and resample a joint path at its closest target-pose sample. @@ -286,15 +366,53 @@ def split_joint_trajectory_at_pose( 1, second_indices.unsqueeze(-1).expand(*gather_shape), ) + if dt is None and control_dt is None: + return ( + resample_with_distance(first, first_sample_count, device=trajectory.device), + resample_with_distance( + second, second_sample_count, device=trajectory.device + ), + ) + if dt is None or control_dt is None: + raise ValueError("dt and control_dt must be supplied together.") + if dt.shape != trajectory.shape[:2]: + raise ValueError("dt must have shape (B, N) matching trajectory.") + first_dt = torch.gather(dt, 1, first_indices).clone() + first_dt[:, 0] = 0.0 + first_dt = torch.where( + sample_indices[None] <= split_indices[:, None], first_dt, 0.0 + ) + second_dt = torch.gather(dt, 1, second_indices).clone() + second_dt[:, 0] = 0.0 + second_dt = torch.where( + second_indices < trajectory.shape[1] - 1, + second_dt, + torch.where( + sample_indices[None] <= trajectory.shape[1] - 1 - split_indices[:, None], + second_dt, + 0.0, + ), + ) return ( - resample_with_distance(first, first_sample_count, device=trajectory.device), - resample_with_distance(second, second_sample_count, device=trajectory.device), + resample_planned_trajectory( + first, + first_sample_count, + dt=first_dt, + control_dt=control_dt, + ), + resample_planned_trajectory( + second, + second_sample_count, + dt=second_dt, + control_dt=control_dt, + ), ) __all__ = [ "arm_qpos_from_state", "assemble_full_robot_trajectory", + "pad_trajectory_to_length", "plan_named_arm_trajectory", "resample_planned_trajectory", "require_shared_task_state_key", diff --git a/embodichain/lab/sim/atomic_actions/primitives/axis_align.py b/embodichain/lab/sim/atomic_actions/primitives/axis_align.py index 123f6749c..79026a30d 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/axis_align.py +++ b/embodichain/lab/sim/atomic_actions/primitives/axis_align.py @@ -381,8 +381,13 @@ def _plan( ) full[:] = context.last_qpos.unsqueeze(1) offset = pre_close_arm.shape[1] + approach_hand = interpolate_hand_qpos( + context.last_qpos[:, hand_joint_ids], + hand_open_qpos, + n_waypoints=offset, + ) full[:, :offset, arm_joint_ids] = pre_close_arm - full[:, :offset, hand_joint_ids] = hand_open_qpos.unsqueeze(1) + full[:, :offset, hand_joint_ids] = approach_hand stop = offset + hand_close.shape[1] full[:, offset:stop, arm_joint_ids] = pre_close_arm[:, -1].unsqueeze(1) full[:, offset:stop, hand_joint_ids] = hand_close @@ -533,8 +538,12 @@ def _plan_pose_phase( ) assert isinstance(result.success, torch.Tensor) assert result.positions is not None + assert result.dt is not None return result.success, resample_planned_trajectory( - result.positions, sample_count + result.positions, + sample_count, + dt=result.dt, + control_dt=interpolation_dt, ) def _axis_alignment_eef_keyframes( diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index 4ba1c433f..34394bb90 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -1261,6 +1261,19 @@ def _log_ik_failures( f"environment(s) {failed_env_ids}." ) + @staticmethod + def _log_joint_step_failures( + control_part: str, + failed_mask: torch.Tensor, + ) -> None: + """Report dense paths rejected for one-control-interval joint jumps.""" + failed_env_ids = torch.nonzero(failed_mask, as_tuple=False).flatten().tolist() + if failed_env_ids: + logger.log_warning( + f"Rejected {control_part} dense object path with joint steps " + f"above its velocity limit in environment(s) {failed_env_ids}." + ) + def _plan_masked_arm_trajectory( self, control_part: str, @@ -1318,6 +1331,9 @@ def _plan_synchronized_object_motion( active_mask: torch.Tensor, resources: _CoordinatedPickResources, options: CoordinatedPickmentOptions, + *, + velocity_limit: float | None, + step_dt: float, ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: n_waypoints = object_pose_traj.shape[1] keyframe_indices = self._select_motion_keyframe_indices(n_waypoints, options) @@ -1393,11 +1409,126 @@ def _plan_synchronized_object_motion( left_traj[:, keyframe_col] = left_qpos_seed right_traj[:, keyframe_col] = right_qpos_seed - return ( - success_mask, - self._interpolate_qpos_keyframes(left_traj, keyframe_indices, n_waypoints), - self._interpolate_qpos_keyframes(right_traj, keyframe_indices, n_waypoints), + left_seed = self._interpolate_qpos_keyframes( + left_traj, keyframe_indices, n_waypoints + ) + right_seed = self._interpolate_qpos_keyframes( + right_traj, keyframe_indices, n_waypoints + ) + left_targets = torch.matmul(object_pose_traj, left_object_to_eef[:, None]) + right_targets = torch.matmul(object_pose_traj, right_object_to_eef[:, None]) + left_solver = self.robot.get_solver(resources.left_arm.control_part) + right_solver = self.robot.get_solver(resources.right_arm.control_part) + left_continuous = bool( + left_solver is not None and left_solver.supports_continuous_batch_ik + ) + right_continuous = bool( + right_solver is not None and right_solver.supports_continuous_batch_ik + ) + left_valid, left_projected = self.robot.compute_batch_ik( + pose=left_targets, + name=resources.left_arm.control_part, + joint_seed=left_start_qpos if left_continuous else left_seed, + continuous=left_continuous, + ) + right_valid, right_projected = self.robot.compute_batch_ik( + pose=right_targets, + name=resources.right_arm.control_part, + joint_seed=right_start_qpos if right_continuous else right_seed, + continuous=right_continuous, + ) + left_valid = torch.as_tensor(left_valid, device=self.device).to(torch.bool) + right_valid = torch.as_tensor(right_valid, device=self.device).to(torch.bool) + expected_valid_shape = (self.num_envs, n_waypoints) + if ( + left_valid.shape != expected_valid_shape + or right_valid.shape != expected_valid_shape + ): + raise ValueError( + "Coordinated object-motion batch IK success must have shape " + f"{expected_valid_shape}." + ) + left_projected = torch.as_tensor( + left_projected, dtype=torch.float32, device=self.device + ) + right_projected = torch.as_tensor( + right_projected, dtype=torch.float32, device=self.device ) + if ( + left_projected.shape != left_seed.shape + or right_projected.shape != right_seed.shape + ): + raise ValueError( + "Coordinated object-motion batch IK qpos must match its joint seeds." + ) + left_valid &= torch.isfinite(left_projected).all(dim=-1) + right_valid &= torch.isfinite(right_projected).all(dim=-1) + left_step_valid = self._joint_steps_within_velocity_limits( + left_start_qpos, + left_projected, + resources.left_arm.control_part, + velocity_limit=velocity_limit, + step_dt=step_dt, + ) + right_step_valid = self._joint_steps_within_velocity_limits( + right_start_qpos, + right_projected, + resources.right_arm.control_part, + velocity_limit=velocity_limit, + step_dt=step_dt, + ) + left_path_valid = left_valid.all(dim=1) & left_step_valid + right_path_valid = right_valid.all(dim=1) & right_step_valid + projected_success = left_path_valid & right_path_valid + self._log_ik_failures( + resources.left_arm.control_part, + "dense object path", + success_mask & ~left_valid.all(dim=1), + ) + self._log_ik_failures( + resources.right_arm.control_part, + "dense object path", + success_mask & ~right_valid.all(dim=1), + ) + self._log_joint_step_failures( + resources.left_arm.control_part, + success_mask & left_valid.all(dim=1) & ~left_step_valid, + ) + self._log_joint_step_failures( + resources.right_arm.control_part, + success_mask & right_valid.all(dim=1) & ~right_step_valid, + ) + success_mask &= projected_success + left_projected = torch.where( + left_path_valid[:, None, None], left_projected, left_seed + ) + right_projected = torch.where( + right_path_valid[:, None, None], right_projected, right_seed + ) + return success_mask, left_projected, right_projected + + def _joint_steps_within_velocity_limits( + self, + start_qpos: torch.Tensor, + trajectory: torch.Tensor, + control_part: str, + *, + velocity_limit: float | None, + step_dt: float, + ) -> torch.Tensor: + """Reject dense IK paths that jump farther than one control interval.""" + limits = self.motion_generator._resolve_velocity_limits( + control_part=control_part, + batch_size=self.num_envs, + dof=start_qpos.shape[-1], + requested=velocity_limit, + reference=start_qpos, + ) + positions = torch.cat((start_qpos[:, None], trajectory), dim=1) + max_step = limits.view(1, 1, -1) * step_dt + return torch.isfinite(positions).all(dim=(1, 2)) & ( + positions.diff(dim=1).abs() <= max_step + 1.0e-6 + ).all(dim=(1, 2)) def _plan( self, @@ -1477,8 +1608,16 @@ def _plan( state, left_approach_traj, right_approach_traj, - self._repeat_qpos(resources.left_hand_open_qpos, segments["approach"]), - self._repeat_qpos(resources.right_hand_open_qpos, segments["approach"]), + self._interpolate_qpos( + state.last_qpos[:, list(resources.left_hand.joint_ids)], + resources.left_hand_open_qpos, + segments["approach"], + ), + self._interpolate_qpos( + state.last_qpos[:, list(resources.right_hand.joint_ids)], + resources.right_hand_open_qpos, + segments["approach"], + ), resources=resources, ) @@ -1519,6 +1658,8 @@ def _plan( success_mask, resources, options, + velocity_limit=request.motion_policy.velocity_limit, + step_dt=context.require_control_dt(), ) ) @@ -1549,6 +1690,8 @@ def _plan( success_mask, resources, options, + velocity_limit=request.motion_policy.velocity_limit, + step_dt=context.require_control_dt(), ) ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py index 6ddafa2cd..d6ea99d5a 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py @@ -59,6 +59,7 @@ ) from embodichain.lab.sim.atomic_actions.primitives._helpers import ( assemble_full_robot_trajectory, + pad_trajectory_to_length, plan_named_arm_trajectory, require_shared_task_state_key, repeat_qpos, @@ -343,14 +344,27 @@ def _plan( request, context, message="Support approach failed." ) + approach_count = max( + placing_approach_traj.shape[1], support_approach_traj.shape[1] + ) + placing_approach_traj = pad_trajectory_to_length( + placing_approach_traj, approach_count + ) + support_approach_traj = pad_trajectory_to_length( + support_approach_traj, approach_count + ) + segments["approach"] = approach_count + placing_place_qpos = placing_approach_traj[:, -1] support_place_qpos = support_approach_traj[:, -1] + placing_hand_qpos = state.last_qpos[:, list(resources.placing_hand.joint_ids)] + support_hand_qpos = state.last_qpos[:, list(resources.support_hand.joint_ids)] approach_trajectory = self._assemble_segment( state.last_qpos, placing_approach_traj, support_approach_traj, - self._repeat_qpos(resources.placing_hand_close_qpos, segments["approach"]), - self._repeat_qpos(resources.support_hand_close_qpos, segments["approach"]), + self._repeat_qpos(placing_hand_qpos, segments["approach"]), + self._repeat_qpos(support_hand_qpos, segments["approach"]), resources=resources, ) @@ -360,8 +374,8 @@ def _plan( state.last_qpos, self._repeat_qpos(placing_place_qpos, segments["hold"]), self._repeat_qpos(support_place_qpos, segments["hold"]), - self._repeat_qpos(resources.placing_hand_close_qpos, segments["hold"]), - self._repeat_qpos(resources.support_hand_close_qpos, segments["hold"]), + self._repeat_qpos(placing_hand_qpos, segments["hold"]), + self._repeat_qpos(support_hand_qpos, segments["hold"]), resources=resources, ) @@ -372,13 +386,11 @@ def _plan( self._repeat_qpos(placing_place_qpos, segments["release"]), self._repeat_qpos(support_place_qpos, segments["release"]), interpolate_hand_qpos( - resources.placing_hand_close_qpos, + placing_hand_qpos, resources.placing_hand_open_qpos, n_waypoints=segments["release"], ), - self._repeat_qpos( - resources.support_hand_close_qpos, segments["release"] - ), + self._repeat_qpos(support_hand_qpos, segments["release"]), resources=resources, ) @@ -400,18 +412,17 @@ def _plan( if not success_mask.any(): logger.log_warning("CoordinatedPlacement failed to plan placing retreat.") return self.failed_plan(request, context, message="Placing retreat failed.") + segments["retreat"] = placing_retreat_traj.shape[1] placing_hand_retreat_qpos = ( - resources.placing_hand_open_qpos - if release - else resources.placing_hand_close_qpos + resources.placing_hand_open_qpos if release else placing_hand_qpos ) retreat_trajectory = self._assemble_segment( state.last_qpos, placing_retreat_traj, self._repeat_qpos(support_place_qpos, segments["retreat"]), self._repeat_qpos(placing_hand_retreat_qpos, segments["retreat"]), - self._repeat_qpos(resources.support_hand_close_qpos, segments["retreat"]), + self._repeat_qpos(support_hand_qpos, segments["retreat"]), resources=resources, ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 80866c5c2..76bdfa927 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -66,6 +66,7 @@ ) from embodichain.lab.sim.atomic_actions.primitives._helpers import ( assemble_full_robot_trajectory, + pad_trajectory_to_length, plan_named_arm_trajectory, repeat_qpos, require_shared_task_state_key, @@ -447,9 +448,10 @@ def _plan( second_root_pose, resources.first, resources.second, - segment_lengths, + dict(segment_lengths), eligible, ) + segment_lengths = selected.segment_lengths success = selected.success & eligible full = selected.trajectory first_object_to_eef = selected.handover_object_to_eef @@ -469,9 +471,10 @@ def _plan( first_root_pose, resources.second, resources.first, - segment_lengths, + dict(segment_lengths), eligible, ) + segment_lengths = selected.segment_lengths success = selected.success & eligible full = selected.trajectory first_object_to_eef = selected.receive_object_to_eef @@ -491,7 +494,7 @@ def _plan( second_root_pose, resources.first, resources.second, - segment_lengths, + dict(segment_lengths), first_is_handover & eligible, ) second_to_first = self._plan_direction( @@ -505,13 +508,30 @@ def _plan( first_root_pose, resources.second, resources.first, - segment_lengths, + dict(segment_lengths), ~first_is_handover & eligible, ) - if first_to_second.trajectory.shape != second_to_first.trajectory.shape: + if tuple(first_to_second.segment_lengths) != tuple( + second_to_first.segment_lengths + ): raise ValueError( - "Both HandOver arm assignments must produce matching trajectory shapes." + "Both HandOver arm assignments must produce matching segment names." + ) + segment_lengths = { + name: max( + first_to_second.segment_lengths[name], + second_to_first.segment_lengths[name], ) + for name in first_to_second.segment_lengths + } + first_trajectory = self._pad_directional_trajectory( + first_to_second, + segment_lengths, + ) + second_trajectory = self._pad_directional_trajectory( + second_to_first, + segment_lengths, + ) success = ( torch.where( first_is_handover, @@ -522,8 +542,8 @@ def _plan( ) full = torch.where( first_is_handover[:, None, None], - first_to_second.trajectory, - second_to_first.trajectory, + first_trajectory, + second_trajectory, ) pose_selector = first_is_handover[:, None, None] first_object_to_eef = torch.where( @@ -846,6 +866,7 @@ def _plan_existing_hold( device=self.device, name="HandOver existing-hold source transfer success", ) + lengths["transfer"] = source_transfer.shape[1] source_hold_qpos = source_transfer[:, -1] phase_success, destination_approach = plan_named_arm_trajectory( self.motion_generator, @@ -862,6 +883,7 @@ def _plan_existing_hold( device=self.device, name="HandOver existing-hold destination approach success", ) + lengths["approach"] = destination_approach.shape[1] destination_hold_qpos = destination_approach[:, -1] phase_success, source_retreat = plan_named_arm_trajectory( self.motion_generator, @@ -878,12 +900,16 @@ def _plan_existing_hold( device=self.device, name="HandOver existing-hold source retreat success", ) + lengths["retreat"] = source_retreat.shape[1] # An existing hold is already physically established. Preserve the # observed source-hand closure until the release phase instead of # commanding a configured grasp setpoint that may be tighter than the # contact-supported pose. source_hand_qpos = context.last_qpos[:, list(resources.first.hand.joint_ids)] + destination_hand_qpos = context.last_qpos[ + :, list(resources.second.hand.joint_ids) + ] segment_values: list[tuple[str, torch.Tensor]] = [ ( @@ -893,7 +919,11 @@ def _plan_existing_hold( source_transfer, repeat_qpos(destination_start_qpos, lengths["transfer"]), repeat_qpos(source_hand_qpos, lengths["transfer"]), - repeat_qpos(resources.second.hand_open_qpos, lengths["transfer"]), + interpolate_hand_qpos( + destination_hand_qpos, + resources.second.hand_open_qpos, + n_waypoints=lengths["transfer"], + ), resources.first, resources.second, ), @@ -1109,7 +1139,7 @@ def _compute_existing_hold_segment_lengths( release = max(2, options.hand_interp_steps) hold = options.hold_steps retreat = max(2, options.retreat_steps) - reserved = close + release + hold + retreat + reserved = close + release + retreat remaining = sample_count - reserved if remaining < 4: raise ValueError( @@ -1393,6 +1423,7 @@ def _plan_direction( failed_mask=active_mask & ~pickup_approach_success, ) success &= pickup_approach_success + segment_lengths["pickup_approach"] = pickup_approach.shape[1] handover_grasp_qpos = pickup_approach[:, -1] pickup_transport_targets = torch.stack( @@ -1423,6 +1454,7 @@ def _plan_direction( failed_mask=active_mask & ~pickup_transport_success, ) success &= pickup_transport_success + segment_lengths["pickup_transport"] = pickup_transport.shape[1] handover_middle_qpos = pickup_transport[:, -1] receive_approach_targets = torch.stack( @@ -1453,6 +1485,7 @@ def _plan_direction( failed_mask=active_mask & ~receive_approach_success, ) success &= receive_approach_success + segment_lengths["receive_approach"] = receive_approach.shape[1] receive_grasp_qpos = receive_approach[:, -1] source_retreat: torch.Tensor | None = None @@ -1496,6 +1529,7 @@ def _plan_direction( failed_mask=active_mask & ~source_retreat_success, ) success &= source_retreat_success + segment_lengths["source_retreat"] = source_retreat.shape[1] receive_place: torch.Tensor | None = None receive_final_qpos: torch.Tensor | None = None @@ -1529,17 +1563,26 @@ def _plan_direction( failed_mask=active_mask & ~placement_success, ) success &= placement_success + segment_lengths["place"] = receive_place.shape[1] receive_final_qpos = receive_place[:, -1] + handover_start_hand = state.last_qpos[:, list(handover.hand.joint_ids)] + receive_start_hand = state.last_qpos[:, list(receive.hand.joint_ids)] segments = [ self._assemble_segment( state, pickup_approach, repeat_qpos(receive_start_qpos, segment_lengths["pickup_approach"]), - repeat_qpos( - handover.hand_open_qpos, segment_lengths["pickup_approach"] + interpolate_hand_qpos( + handover_start_hand, + handover.hand_open_qpos, + n_waypoints=segment_lengths["pickup_approach"], + ), + interpolate_hand_qpos( + receive_start_hand, + receive.hand_open_qpos, + n_waypoints=segment_lengths["pickup_approach"], ), - repeat_qpos(receive.hand_open_qpos, segment_lengths["pickup_approach"]), handover, receive, ), @@ -1597,6 +1640,15 @@ def _plan_direction( handover, receive, ), + self._assemble_segment( + state, + repeat_qpos(handover_middle_qpos, segment_lengths["receive_hold"]), + repeat_qpos(receive_grasp_qpos, segment_lengths["receive_hold"]), + repeat_qpos(handover.hand_grasp_qpos, segment_lengths["receive_hold"]), + repeat_qpos(receive.hand_grasp_qpos, segment_lengths["receive_hold"]), + handover, + receive, + ), self._assemble_segment( state, repeat_qpos(handover_middle_qpos, segment_lengths["handover_release"]), @@ -1966,12 +2018,42 @@ def _middle_object_pose( middle_position.scatter_(1, dominant_axis[:, None], selected_midpoint) return middle + @staticmethod + def _pad_directional_trajectory( + plan: _DirectionalPlan, + segment_lengths: Mapping[str, int], + ) -> torch.Tensor: + """Pad corresponding phases without shifting handover boundaries.""" + if tuple(plan.segment_lengths) != tuple(segment_lengths): + raise ValueError( + "Both HandOver arm assignments must produce matching segment names." + ) + offset = 0 + segments: list[torch.Tensor] = [] + for name, source_length in plan.segment_lengths.items(): + stop = offset + source_length + segment = plan.trajectory[:, offset:stop] + if source_length == 0: + if segment_lengths[name] != 0: + raise ValueError( + "HandOver cannot expand an empty directional phase." + ) + offset = stop + continue + segments.append(pad_trajectory_to_length(segment, segment_lengths[name])) + offset = stop + if offset != plan.trajectory.shape[1]: + raise ValueError( + "HandOver directional segment lengths do not cover the trajectory." + ) + return torch.cat(segments, dim=1) + @staticmethod def _compute_segment_lengths( sample_count: int, options: HandOverOptions, ) -> dict[str, int]: - """Split the sample budget across enabled arm and hand phases.""" + """Split the sample budget, then add the required receive-side hold.""" hand_count = options.hand_interp_steps hand_phase_count = 4 if options.release_at_target else 3 motion_phase_count = 4 @@ -1990,6 +2072,7 @@ def _compute_segment_lengths( "pickup_transport": motion_counts[1], "receive_approach": motion_counts[2], "receive_close": hand_count, + "receive_hold": options.hold_steps, "handover_release": hand_count, } if options.release_at_target: diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py index a1bfbf26b..79ee95586 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -143,12 +143,6 @@ def _plan( control_part = motion_target.control_part arm_joint_ids = list(motion_target.joint_ids) hand_joint_ids = list(grasp_target.joint_ids) - hand_grasp_qpos = grasp.joint_positions( - GRASP_COMMAND, - num_envs=context.batch_size, - device=self.device, - dtype=context.robot.qpos.dtype, - ) state = context held_object = state.get_held_object(task_state_key) if held_object is None: @@ -222,7 +216,6 @@ def plan_target(poses: torch.Tensor) -> PlanResult: success = result.success & eligible base_qpos = state.last_qpos.clone() - base_qpos[:, hand_joint_ids] = hand_grasp_qpos _, timed = to_full_robot_trajectory( result, base_qpos=base_qpos, diff --git a/embodichain/lab/sim/atomic_actions/primitives/open_door.py b/embodichain/lab/sim/atomic_actions/primitives/open_door.py index e05c970a3..778acfc31 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/open_door.py +++ b/embodichain/lab/sim/atomic_actions/primitives/open_door.py @@ -28,6 +28,7 @@ from __future__ import annotations import math +from collections.abc import Callable from dataclasses import dataclass from numbers import Real from typing import ClassVar @@ -64,6 +65,7 @@ ) from embodichain.lab.sim.atomic_actions.primitives._helpers import ( arm_qpos_from_state, + hold_inactive_cartesian_rows, resample_planned_trajectory, ) from embodichain.lab.sim.atomic_actions.requirements import ( @@ -183,6 +185,11 @@ def _plan( context: PlanningContext, ) -> ActionPlan: """Plan the complete six-stage door-opening motion without side effects.""" + if request.motion_policy.strategy != "ik_interp": + raise ValueError( + "OpenDoor requires strategy='ik_interp' because reach, hinge, " + "and retract phases must preserve every Cartesian path sample." + ) target = self.require_goal(request) affordance = self._require_open_door_affordance(target.semantics) options = request.skill_options @@ -309,8 +316,9 @@ def _plan( affordance.rotation_axis, affordance.axis_origin, hinge_rotation, - options.door_waypoint_count, + n_open - 1, ) + rotated_approach_direction = torch.matmul( opened_link_poses[:, -1, :3, :3], approach_direction_local, @@ -328,10 +336,8 @@ def _plan( n_approach, interpolation_dt=interpolation_dt, ) - # ik_interp follows exact straight-line samples into and out of the - # handle. motion_gen backends do not guarantee Cartesian samples, so - # they receive the segment endpoint instead. - linear_contact = request.motion_policy.strategy == "ik_interp" + # Reach and retract remain explicit straight-line Cartesian samples. + linear_contact = True reach_keyframes = ( axis_translation_keyframes( approach_xpos, @@ -342,6 +348,17 @@ def _plan( if linear_contact else grasp_xpos ) + reach_active = actionable_grasp & approach_success + reach_hold_pose = self.robot.compute_fk( + qpos=approach_arm[:, -1], + name=control_part, + to_matrix=True, + ).to(reach_keyframes) + reach_keyframes = hold_inactive_cartesian_rows( + reach_keyframes, + active_mask=reach_active, + hold_pose=reach_hold_pose, + ) reach_success, reach_arm = self._plan_pose_segment( reach_keyframes, approach_arm[:, -1], @@ -351,13 +368,42 @@ def _plan( interpolation_dt=interpolation_dt, cartesian_linear=linear_contact, ) - open_success, open_arm = self._plan_pose_segment( + open_active = reach_active & reach_success + open_hold_pose = self.robot.compute_fk( + qpos=reach_arm[:, -1], + name=control_part, + to_matrix=True, + ).to(opened_grasp_xpos) + open_keyframes = hold_inactive_cartesian_rows( opened_grasp_xpos, + active_mask=open_active, + hold_pose=open_hold_pose, + ) + + def sample_open_path(progress: torch.Tensor) -> torch.Tensor: + sampled = self._opened_link_and_eef_poses_at_fractions( + link_pose, + grasp_xpos, + affordance.rotation_axis, + affordance.axis_origin, + hinge_rotation, + progress, + )[1] + return hold_inactive_cartesian_rows( + sampled, + active_mask=open_active, + hold_pose=open_hold_pose, + ) + + open_success, open_arm = self._plan_pose_segment( + open_keyframes, reach_arm[:, -1], control_part, request, n_open, interpolation_dt=interpolation_dt, + cartesian_linear=True, + cartesian_path_sampler=sample_open_path, ) retract_keyframes = ( axis_translation_keyframes( @@ -369,6 +415,17 @@ def _plan( if linear_contact else retract_xpos ) + retract_active = open_active & open_success + retract_hold_pose = self.robot.compute_fk( + qpos=open_arm[:, -1], + name=control_part, + to_matrix=True, + ).to(retract_keyframes) + retract_keyframes = hold_inactive_cartesian_rows( + retract_keyframes, + active_mask=retract_active, + hold_pose=retract_hold_pose, + ) retract_success, retract_arm = self._plan_pose_segment( retract_keyframes, open_arm[:, -1], @@ -397,6 +454,14 @@ def _plan( hand_open_qpos, n_waypoints=options.hand_interp_steps, ) + segment_lengths = { + "approach": approach_arm.shape[1], + "reach": reach_arm.shape[1], + "close": hand_close.shape[1], + "open": open_arm.shape[1], + "release": hand_open.shape[1], + "retract": retract_arm.shape[1], + } full = torch.empty( (self.num_envs, sum(segment_lengths.values()), self.robot_dof), dtype=context.robot.qpos.dtype, @@ -404,10 +469,18 @@ def _plan( ) full[:] = context.last_qpos.unsqueeze(1) offset = 0 - for arm in (approach_arm, reach_arm): + for index, arm in enumerate((approach_arm, reach_arm)): stop = offset + arm.shape[1] full[:, offset:stop, arm_joint_ids] = arm - full[:, offset:stop, hand_joint_ids] = hand_open_qpos.unsqueeze(1) + full[:, offset:stop, hand_joint_ids] = ( + interpolate_hand_qpos( + context.last_qpos[:, hand_joint_ids], + hand_open_qpos, + n_waypoints=arm.shape[1], + ) + if index == 0 + else hand_open_qpos.unsqueeze(1) + ) offset = stop stop = offset + hand_close.shape[1] full[:, offset:stop, arm_joint_ids] = reach_arm[:, -1].unsqueeze(1) @@ -698,6 +771,7 @@ def _plan_pose_segment( *, interpolation_dt: float, cartesian_linear: bool = False, + cartesian_path_sampler: Callable[[torch.Tensor], torch.Tensor] | None = None, ) -> tuple[torch.Tensor, torch.Tensor]: result = self.motion_generator.generate( build_pose_plan_states(target_pose), @@ -707,12 +781,17 @@ def _plan_pose_segment( sample_count=sample_count, interpolation_dt=interpolation_dt, cartesian_linear=cartesian_linear, + cartesian_path_sampler=cartesian_path_sampler, ), ) assert isinstance(result.success, torch.Tensor) assert result.positions is not None + assert result.dt is not None return result.success, resample_planned_trajectory( - result.positions, sample_count + result.positions, + sample_count, + dt=result.dt, + control_dt=interpolation_dt, ) def _opened_link_and_eef_poses( @@ -725,8 +804,6 @@ def _opened_link_and_eef_poses( waypoint_count: int, ) -> tuple[torch.Tensor, torch.Tensor]: """Rotate link poses about the hinge and recover corresponding EEF poses.""" - axis = rotation_axis.to(device=self.device, dtype=torch.float32) - axis = axis / torch.linalg.vector_norm(axis) fractions = torch.linspace( 1.0 / waypoint_count, 1.0, @@ -734,12 +811,34 @@ def _opened_link_and_eef_poses( dtype=torch.float32, device=self.device, ) + return self._opened_link_and_eef_poses_at_fractions( + link_pose, + grasp_xpos, + rotation_axis, + axis_origin, + hinge_rotation, + fractions, + ) + + def _opened_link_and_eef_poses_at_fractions( + self, + link_pose: torch.Tensor, + grasp_xpos: torch.Tensor, + rotation_axis: torch.Tensor, + axis_origin: tuple[float, float, float], + hinge_rotation: torch.Tensor, + fractions: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor]: + """Sample the rigid handle path at normalized hinge-angle fractions.""" + axis = rotation_axis.to(device=self.device, dtype=torch.float32) + axis = axis / torch.linalg.vector_norm(axis) + fractions = fractions.to(device=self.device, dtype=torch.float32) angles = hinge_rotation.to(device=self.device, dtype=torch.float32)[:, None] angles = angles * fractions[None] rotations = ( torch.eye(4, dtype=torch.float32, device=self.device) .reshape(1, 1, 4, 4) - .repeat(link_pose.shape[0], waypoint_count, 1, 1) + .repeat(link_pose.shape[0], fractions.numel(), 1, 1) ) rotations[:, :, :3, :3] = axis_angle_to_rotation_matrix( angles[:, :, None] * axis diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index eb02b729d..2a5ed61ed 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -380,6 +380,7 @@ def _get_full_pickup_trajectory( ) assert isinstance(motion_result.success, torch.Tensor) assert motion_result.positions is not None + assert motion_result.dt is not None approach_arm, lift_arm = split_joint_trajectory_at_pose( motion_result.positions, grasp_xpos, @@ -387,7 +388,11 @@ def _get_full_pickup_trajectory( control_part=manipulator.control_part, first_sample_count=n_approach, second_sample_count=n_lift, + dt=motion_result.dt, + control_dt=interpolation_dt, ) + n_approach = approach_arm.shape[1] + n_lift = lift_arm.shape[1] grasp_arm_qpos = approach_arm[:, -1, :] is_success = motion_result.success @@ -410,8 +415,13 @@ def _get_full_pickup_trajectory( full[:, :, :] = last_qpos.unsqueeze(1) arm_joint_ids = list(manipulator.joint_ids) hand_joint_ids = list(end_effector.joint_ids) + approach_hand = interpolate_hand_qpos( + last_qpos[:, hand_joint_ids], + hand_open_qpos, + n_waypoints=n_approach, + ) full[:, :n_approach, arm_joint_ids] = approach_arm - full[:, :n_approach, hand_joint_ids] = hand_open_qpos.unsqueeze(1) + full[:, :n_approach, hand_joint_ids] = approach_hand full[:, close_start:settle_start, arm_joint_ids] = grasp_arm_qpos.unsqueeze(1) full[:, close_start:settle_start, hand_joint_ids] = hand_close_path if n_settle: diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index 3c8aa4ef7..549880801 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -241,12 +241,6 @@ def _plan( device=self.device, dtype=context.robot.qpos.dtype, ) - hand_grasp_qpos = grasp.joint_positions( - GRASP_COMMAND, - num_envs=context.batch_size, - device=self.device, - dtype=context.robot.qpos.dtype, - ) state = context held_mask = context.task.held_object_mask(task_state_key) exclusive_mask = context.task.exclusive_held_object_mask(task_state_key) @@ -270,6 +264,7 @@ def _plan( place_xpos = place_xpos.unsqueeze(1) start_arm_qpos = arm_qpos_from_state(state, arm_joint_ids) + start_hand_qpos = state.last_qpos[:, hand_joint_ids] if isinstance(target, PlaceGoal) and target.tcp_symmetry == "z_roll_180": place_xpos = self._select_tcp_symmetric_place_variant( place_xpos, start_arm_qpos, control_part @@ -312,6 +307,7 @@ def _plan( ) assert isinstance(motion_result.success, torch.Tensor) assert motion_result.positions is not None + assert motion_result.dt is not None down_arm, back_arm = split_joint_trajectory_at_pose( motion_result.positions, place_xpos[:, -1], @@ -319,12 +315,16 @@ def _plan( control_part=control_part, first_sample_count=n_down, second_sample_count=n_back, + dt=motion_result.dt, + control_dt=context.control_dt, ) + n_down = down_arm.shape[1] + n_back = back_arm.shape[1] reach_arm_qpos = down_arm[:, -1, :] success = motion_result.success & eligible hand_open_path = interpolate_hand_qpos( - hand_grasp_qpos, hand_open_qpos, n_waypoints=n_open + start_hand_qpos, hand_open_qpos, n_waypoints=n_open ) n_settle = options.release_settle_steps @@ -338,7 +338,7 @@ def _plan( ) full[:, :, :] = state.last_qpos.unsqueeze(1) full[:, :n_down, arm_joint_ids] = down_arm - full[:, :n_down, hand_joint_ids] = hand_grasp_qpos.unsqueeze(1) + full[:, :n_down, hand_joint_ids] = start_hand_qpos.unsqueeze(1) full[:, open_start:settle_start, arm_joint_ids] = reach_arm_qpos.unsqueeze(1) full[:, open_start:settle_start, hand_joint_ids] = hand_open_path if n_settle: diff --git a/embodichain/lab/sim/atomic_actions/primitives/pour.py b/embodichain/lab/sim/atomic_actions/primitives/pour.py index 8339e01a2..b814d2732 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pour.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pour.py @@ -37,13 +37,14 @@ ActionOptions, ResolvedActionRequest, ) -from embodichain.lab.sim.atomic_actions.plans import ActionPlan +from embodichain.lab.sim.atomic_actions.plans import ActionPlan, TimedTrajectory 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, require_shared_task_state_key, + split_joint_trajectory_at_pose, ) from embodichain.lab.sim.atomic_actions.requirements import ( CARTESIAN_POSE_CAPABILITY, @@ -53,7 +54,6 @@ from embodichain.lab.sim.atomic_actions.state import PlanningContext from embodichain.lab.sim.atomic_actions.trajectory_ops import ( build_pose_plan_states, - to_full_robot_trajectory, ) @@ -69,9 +69,14 @@ class PourOptions(ActionOptions): rotate_angle: float = math.pi / 4.0 """Signed rotation about the held object's local internal axis, in radians.""" + dwell_duration: float = 0.5 + """Minimum time to remain at the poured orientation, in seconds.""" + def __post_init__(self) -> None: if not math.isfinite(self.rotate_angle): raise ValueError("rotate_angle must be finite.") + if not math.isfinite(self.dwell_duration) or self.dwell_duration < 0.0: + raise ValueError("dwell_duration must be finite and non-negative.") class Pour(AtomicAction[PourGoal, PourOptions]): @@ -186,20 +191,36 @@ def _plan( assert result.dt is not None success = result.success & eligible - hand_grasp_qpos = grasp_endpoint.joint_positions( - GRASP_COMMAND, - num_envs=context.batch_size, - device=self.device, - dtype=context.robot.qpos.dtype, + control_dt = context.require_control_dt() + minimum_pour = max(2, request.motion_policy.sample_count // 2) + minimum_return = max(2, request.motion_policy.sample_count - minimum_pour) + pour_arm, return_arm = split_joint_trajectory_at_pose( + result.positions, + poured_eef_pose, + robot=self.robot, + control_part=control_part, + first_sample_count=minimum_pour, + second_sample_count=minimum_return, + dt=result.dt, + control_dt=control_dt, + ) + dwell_steps = math.ceil(options.dwell_duration / control_dt) + dwell_arm = pour_arm[:, -1:].expand(-1, dwell_steps, -1) + return_arm = return_arm[:, 1:] + arm_trajectory = torch.cat((pour_arm, dwell_arm, return_arm), dim=1) + full = ( + context.last_qpos.unsqueeze(1) + .expand(-1, arm_trajectory.shape[1], -1) + .clone() ) - base_qpos = context.last_qpos.clone() - base_qpos[:, hand_joint_ids] = hand_grasp_qpos - _, timed = to_full_robot_trajectory( - result, - base_qpos=base_qpos, - joint_ids=arm_joint_ids, + full[:, :, arm_joint_ids] = arm_trajectory + # Preserve the measured contact-supported grasp instead of tightening + # the hand to a configured nominal value on the first frame. + full[:, :, hand_joint_ids] = context.last_qpos[:, hand_joint_ids].unsqueeze(1) + timed = TimedTrajectory.from_uniform_step( + full, env_ids=context.env_ids, - control_dt=context.require_control_dt(), + step_dt=control_dt, ) return self.build_plan( @@ -207,7 +228,11 @@ def _plan( context, success=success, trajectory=timed, - segment_lengths={"pour": timed.waypoint_count}, + segment_lengths={ + "pour": pour_arm.shape[1], + "dwell": dwell_steps, + "return": return_arm.shape[1], + }, ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/press.py b/embodichain/lab/sim/atomic_actions/primitives/press.py index 0429e03d4..8f92dbaa2 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/press.py +++ b/embodichain/lab/sim/atomic_actions/primitives/press.py @@ -169,6 +169,11 @@ def _plan( context: PlanningContext, ) -> ActionPlan: """Plan close, approach, press, and retract without stepping simulation.""" + if request.motion_policy.strategy != "ik_interp": + raise ValueError( + "Press requires strategy='ik_interp' because its contact and " + "press phases must preserve every Cartesian path sample." + ) target = self.require_goal(request) affordance = self._require_press_affordance(target.semantics) options = request.skill_options @@ -227,95 +232,70 @@ def _plan( hand_grasp_qpos, n_waypoints=options.hand_interp_steps, ) - if request.motion_policy.strategy == "motion_gen": - targets = torch.stack( - (approach_xpos, contact_xpos, pressed_xpos, approach_xpos), dim=1 - ) - motion_options = request.motion_policy.to_motion_gen_options( - start_qpos=start_arm_qpos, - control_part=control_part, - interpolation_dt=interpolation_dt, - ) - motion_options.sample_count = None - result = self.motion_generator.generate( - build_pose_plan_states(targets), options=motion_options - ) - assert isinstance(result.success, torch.Tensor) - assert result.positions is not None - motion_phases = self._split_motion_path( - result.positions, - result.dt, - targets[:, :3], - control_part, - (n_approach, n_contact, n_press, n_retract), - interpolation_dt, - ) - success = contact_sample.success & result.success - else: - approach_success, approach_phase = self._plan_pose_segment( - approach_xpos, - start_arm_qpos, - control_part, - request, - n_approach, - interpolation_dt=interpolation_dt, - ) - approach_arm = approach_phase[0] - contact_keyframes = axis_translation_keyframes( - approach_xpos, - contact_xpos, - contact_xpos[:, :3, 2], - n_waypoints=n_contact - 1, - ) - contact_success, contact_phase = self._plan_pose_segment( - contact_keyframes, - approach_arm[:, -1], - control_part, - request, - n_contact, - interpolation_dt=interpolation_dt, - cartesian_linear=True, - ) - contact_arm = contact_phase[0] - press_keyframes = axis_translation_keyframes( - contact_xpos, - pressed_xpos, - contact_xpos[:, :3, 2], - n_waypoints=n_press - 1, - ) - press_success, press_phase = self._plan_pose_segment( - press_keyframes, - contact_arm[:, -1], - control_part, - request, - n_press, - interpolation_dt=interpolation_dt, - cartesian_linear=True, - ) - press_arm = press_phase[0] - retract_keyframes = axis_translation_keyframes( - pressed_xpos, - approach_xpos, - contact_xpos[:, :3, 2], - n_waypoints=n_retract - 1, - ) - retract_success, retract_phase = self._plan_pose_segment( - retract_keyframes, - press_arm[:, -1], - control_part, - request, - n_retract, - interpolation_dt=interpolation_dt, - cartesian_linear=True, - ) - success = ( - contact_sample.success - & approach_success - & contact_success - & press_success - & retract_success - ) - motion_phases = (approach_phase, contact_phase, press_phase, retract_phase) + approach_success, approach_phase = self._plan_pose_segment( + approach_xpos, + start_arm_qpos, + control_part, + request, + n_approach, + interpolation_dt=interpolation_dt, + ) + approach_arm = approach_phase[0] + contact_keyframes = axis_translation_keyframes( + approach_xpos, + contact_xpos, + contact_xpos[:, :3, 2], + n_waypoints=n_contact - 1, + ) + contact_success, contact_phase = self._plan_pose_segment( + contact_keyframes, + approach_arm[:, -1], + control_part, + request, + n_contact, + interpolation_dt=interpolation_dt, + cartesian_linear=True, + ) + contact_arm = contact_phase[0] + press_keyframes = axis_translation_keyframes( + contact_xpos, + pressed_xpos, + contact_xpos[:, :3, 2], + n_waypoints=n_press - 1, + ) + press_success, press_phase = self._plan_pose_segment( + press_keyframes, + contact_arm[:, -1], + control_part, + request, + n_press, + interpolation_dt=interpolation_dt, + cartesian_linear=True, + ) + press_arm = press_phase[0] + retract_keyframes = axis_translation_keyframes( + pressed_xpos, + approach_xpos, + contact_xpos[:, :3, 2], + n_waypoints=n_retract - 1, + ) + retract_success, retract_phase = self._plan_pose_segment( + retract_keyframes, + press_arm[:, -1], + control_part, + request, + n_retract, + interpolation_dt=interpolation_dt, + cartesian_linear=True, + ) + success = ( + contact_sample.success + & approach_success + & contact_success + & press_success + & retract_success + ) + motion_phases = (approach_phase, contact_phase, press_phase, retract_phase) parts = (hand_close, *(phase[0] for phase in motion_phases)) lengths = tuple(part.shape[1] for part in parts) @@ -397,15 +377,6 @@ def _plan( ), ) success = success & validity.all(dim=1) - elif ( - request.motion_policy.strategy == "motion_gen" - and self.motion_generator.collision_world_info is not None - ): - raise ValueError( - "Press requires joint-trajectory validation when retiming a " - "collision-aware backend path." - ) - return self.build_plan( request, context, diff --git a/embodichain/lab/sim/atomic_actions/primitives/push_object.py b/embodichain/lab/sim/atomic_actions/primitives/push_object.py index b72715e75..d92d8e8ab 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/push_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/push_object.py @@ -314,6 +314,11 @@ def _plan( context: PlanningContext, ) -> ActionPlan: """Plan close, approach, contact, planar push, and retract segments.""" + if request.motion_policy.strategy != "ik_interp": + raise ValueError( + "PushObject requires strategy='ik_interp' because its contact " + "and push phases must preserve every Cartesian path sample." + ) goal = self.require_goal(request) options = request.skill_options interpolation_dt = context.require_control_dt() @@ -491,10 +496,8 @@ def _plan( n_approach, interpolation_dt=interpolation_dt, ) - # ik_interp follows exact straight-line contact, push and retract - # samples. motion_gen backends do not guarantee Cartesian samples, so - # they receive each segment endpoint instead. - linear_contact = request.motion_policy.strategy == "ik_interp" + # Contact, push, and retract remain explicit Cartesian samples. + linear_contact = True contact_keyframes = ( axis_translation_keyframes( approach_xpos, @@ -645,8 +648,12 @@ def _plan_pose_segment( ) assert isinstance(result.success, torch.Tensor) assert result.positions is not None + assert result.dt is not None return result.success, resample_planned_trajectory( - result.positions, sample_count + result.positions, + sample_count, + dt=result.dt, + control_dt=interpolation_dt, ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/slide.py b/embodichain/lab/sim/atomic_actions/primitives/slide.py index 85fa880af..0e77c3372 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/slide.py +++ b/embodichain/lab/sim/atomic_actions/primitives/slide.py @@ -55,6 +55,7 @@ ) from embodichain.lab.sim.atomic_actions.primitives._helpers import ( arm_qpos_from_state, + pad_trajectory_to_length, resample_planned_trajectory, ) from embodichain.lab.sim.atomic_actions.requirements import ( @@ -224,6 +225,11 @@ def _plan( context: PlanningContext, ) -> ActionPlan: """Plan the complete pull/push sequence without stepping simulation.""" + if request.motion_policy.strategy != "ik_interp": + raise ValueError( + "Slide requires strategy='ik_interp' because its contact path " + "must preserve every Cartesian sample." + ) target = self.require_goal(request) affordance = self._require_slide_affordance(target.semantics) options = request.skill_options @@ -483,6 +489,16 @@ def _plan( hand_open_qpos, n_waypoints=options.hand_interp_steps, ) + release_length = max( + hand_open.shape[1], + 0 if release_arm is None else release_arm.shape[1], + ) + hand_open = pad_trajectory_to_length(hand_open, release_length) + release_arm_phase = ( + translate_arm[:, -1:].expand(-1, release_length, -1) + if release_arm is None + else pad_trajectory_to_length(release_arm, release_length) + ) named_parts: list[tuple[str, torch.Tensor]] = [ ("approach", approach_arm), ("reach", reach_arm), @@ -508,13 +524,11 @@ def _plan( full[:, offset:stop, hand_joint_ids] = hand_preshape_qpos.unsqueeze(1) offset = stop - if options.preshape_fraction > 0.0: - count = min(approach_arm.shape[1], options.hand_interp_steps) - full[:, :count, hand_joint_ids] = interpolate_hand_qpos( - context.robot.qpos[:, hand_joint_ids], - hand_preshape_qpos, - n_waypoints=count, - ) + full[:, : approach_arm.shape[1], hand_joint_ids] = interpolate_hand_qpos( + context.last_qpos[:, hand_joint_ids], + hand_preshape_qpos, + n_waypoints=approach_arm.shape[1], + ) stop = offset + hand_close.shape[1] full[:, offset:stop, arm_joint_ids] = reach_arm[:, -1].unsqueeze(1) @@ -527,9 +541,7 @@ def _plan( offset = stop stop = offset + hand_open.shape[1] - full[:, offset:stop, arm_joint_ids] = ( - translate_arm[:, -1].unsqueeze(1) if release_arm is None else release_arm - ) + full[:, offset:stop, arm_joint_ids] = release_arm_phase full[:, offset:stop, hand_joint_ids] = hand_open offset = stop @@ -670,8 +682,12 @@ def _plan_pose_segment( ) assert isinstance(result.success, torch.Tensor) assert result.positions is not None + assert result.dt is not None return result.success, resample_planned_trajectory( - result.positions, sample_count + result.positions, + sample_count, + dt=result.dt, + control_dt=interpolation_dt, ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/twist.py b/embodichain/lab/sim/atomic_actions/primitives/twist.py index afabdf99b..d641a0511 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/twist.py +++ b/embodichain/lab/sim/atomic_actions/primitives/twist.py @@ -19,6 +19,7 @@ from __future__ import annotations import math +from collections.abc import Callable from dataclasses import dataclass from typing import ClassVar @@ -59,6 +60,7 @@ ) from embodichain.lab.sim.atomic_actions.primitives._helpers import ( arm_qpos_from_state, + hold_inactive_cartesian_rows, resample_planned_trajectory, ) from embodichain.lab.sim.atomic_actions.requirements import ( @@ -68,6 +70,7 @@ ) from embodichain.lab.sim.atomic_actions.state import PlanningContext from embodichain.lab.sim.atomic_actions.trajectory_ops import ( + axis_translation_keyframes, build_pose_plan_states, interpolate_hand_qpos, resolve_pose_target, @@ -169,6 +172,11 @@ def _plan( context: PlanningContext, ) -> ActionPlan: """Plan all six twisting segments without stepping simulation.""" + if request.motion_policy.strategy != "ik_interp": + raise ValueError( + "Twist requires strategy='ik_interp' because the constrained " + "twist path must preserve every Cartesian sample." + ) target = self.require_goal(request) affordance = self._require_twist_affordance(target.semantics) options = request.skill_options @@ -218,18 +226,18 @@ def _plan( grasp_xpos, -grasp_xpos[:, :3, 2] * options.pre_grasp_distance, ) + n_approach, n_reach, n_twist, n_retract = self._motion_segment_lengths( + request.motion_policy.sample_count, + options.hand_interp_steps, + options.twist_waypoint_count, + ) twist_xpos = self._twisted_grasp_poses( link_pose, grasp_xpos, affordance.twist_axis, affordance.require_axis_origin(), options.twist_angle, - options.twist_waypoint_count, - ) - - n_approach, n_reach, n_twist, n_retract = self._motion_segment_lengths( - request.motion_policy.sample_count, - options.hand_interp_steps, + n_twist - 1, ) approach_success, approach_arm = self._plan_pose_segment( @@ -240,21 +248,68 @@ def _plan( n_approach, interpolation_dt=interpolation_dt, ) - reach_success, reach_arm = self._plan_pose_segment( + reach_keyframes = axis_translation_keyframes( + pre_grasp_xpos, grasp_xpos, + grasp_xpos[:, :3, 2], + n_waypoints=n_reach - 1, + ) + reach_active = grasp_sample.success & approach_success + reach_hold_pose = self.robot.compute_fk( + qpos=approach_arm[:, -1], + name=control_part, + to_matrix=True, + ).to(reach_keyframes) + reach_keyframes = hold_inactive_cartesian_rows( + reach_keyframes, + active_mask=reach_active, + hold_pose=reach_hold_pose, + ) + reach_success, reach_arm = self._plan_pose_segment( + reach_keyframes, approach_arm[:, -1], control_part, request, n_reach, interpolation_dt=interpolation_dt, + cartesian_samples=True, ) - twist_success, twist_arm = self._plan_pose_segment( + twist_active = reach_active & reach_success + twist_hold_pose = self.robot.compute_fk( + qpos=reach_arm[:, -1], + name=control_part, + to_matrix=True, + ).to(twist_xpos) + twist_keyframes = hold_inactive_cartesian_rows( twist_xpos, + active_mask=twist_active, + hold_pose=twist_hold_pose, + ) + + def sample_twist_path(progress: torch.Tensor) -> torch.Tensor: + sampled = self._twisted_grasp_poses_at_fractions( + link_pose, + grasp_xpos, + affordance.twist_axis, + affordance.require_axis_origin(), + options.twist_angle, + progress, + ) + return hold_inactive_cartesian_rows( + sampled, + active_mask=twist_active, + hold_pose=twist_hold_pose, + ) + + twist_success, twist_arm = self._plan_pose_segment( + twist_keyframes, reach_arm[:, -1], control_part, request, n_twist, interpolation_dt=interpolation_dt, + cartesian_samples=True, + cartesian_path_sampler=sample_twist_path, ) retract_success, retract_arm = self._plan_pose_segment( pre_grasp_xpos, @@ -305,10 +360,18 @@ def _plan( hand_grasp_qpos, hand_open_qpos, ) - for arm, hand in zip(arm_parts[:2], arm_hands[:2]): + for index, (arm, hand) in enumerate(zip(arm_parts[:2], arm_hands[:2])): stop = offset + arm.shape[1] full[:, offset:stop, arm_joint_ids] = arm - full[:, offset:stop, hand_joint_ids] = hand.unsqueeze(1) + full[:, offset:stop, hand_joint_ids] = ( + interpolate_hand_qpos( + context.last_qpos[:, hand_joint_ids], + hand, + n_waypoints=arm.shape[1], + ) + if index == 0 + else hand.unsqueeze(1) + ) offset = stop stop = offset + hand_close.shape[1] full[:, offset:stop, arm_joint_ids] = reach_arm[:, -1].unsqueeze(1) @@ -362,16 +425,23 @@ def _require_twist_affordance( def _motion_segment_lengths( sample_count: int, hand_interp_steps: int, + twist_waypoint_count: int, ) -> tuple[int, int, int, int]: motion_count = sample_count - 2 * hand_interp_steps - if motion_count < 8: + minimum_twist_count = twist_waypoint_count + 1 + if motion_count < minimum_twist_count + 6: raise ValueError( "Not enough waypoints for Twist. Increase sample_count or " - "decrease hand_interp_steps." + "decrease hand_interp_steps or twist_waypoint_count." ) base, remainder = divmod(motion_count, 4) values = [base + (index < remainder) for index in range(4)] - return values[0], values[1], values[2], values[3] + if values[2] >= minimum_twist_count: + return values[0], values[1], values[2], values[3] + remaining_count = motion_count - minimum_twist_count + other_base, other_remainder = divmod(remaining_count, 3) + other = [other_base + (index < other_remainder) for index in range(3)] + return other[0], other[1], minimum_twist_count, other[2] def _plan_pose_segment( self, @@ -382,6 +452,8 @@ def _plan_pose_segment( sample_count: int, *, interpolation_dt: float, + cartesian_samples: bool = False, + cartesian_path_sampler: Callable[[torch.Tensor], torch.Tensor] | None = None, ) -> tuple[torch.Tensor, torch.Tensor]: result = self.motion_generator.generate( build_pose_plan_states(target_pose), @@ -390,12 +462,18 @@ def _plan_pose_segment( control_part=control_part, sample_count=sample_count, interpolation_dt=interpolation_dt, + cartesian_linear=cartesian_samples, + cartesian_path_sampler=cartesian_path_sampler, ), ) assert isinstance(result.success, torch.Tensor) assert result.positions is not None + assert result.dt is not None return result.success, resample_planned_trajectory( - result.positions, sample_count + result.positions, + sample_count, + dt=result.dt, + control_dt=interpolation_dt, ) def _twisted_grasp_poses( @@ -408,19 +486,40 @@ def _twisted_grasp_poses( waypoint_count: int, ) -> torch.Tensor: """Build Cartesian EEF keyframes that follow the target's twist arc.""" - axis = twist_axis.to(device=self.device, dtype=torch.float32) - axis = axis / torch.linalg.vector_norm(axis) - angles = torch.linspace( - twist_angle / waypoint_count, - twist_angle, + fractions = torch.linspace( + 1.0 / waypoint_count, + 1.0, waypoint_count, dtype=torch.float32, device=self.device, ) + return self._twisted_grasp_poses_at_fractions( + link_pose, + grasp_xpos, + twist_axis, + axis_origin, + twist_angle, + fractions, + ) + + def _twisted_grasp_poses_at_fractions( + self, + link_pose: torch.Tensor, + grasp_xpos: torch.Tensor, + twist_axis: torch.Tensor, + axis_origin: tuple[float, float, float], + twist_angle: float, + fractions: torch.Tensor, + ) -> torch.Tensor: + """Sample the rigid grasp path at normalized twist-angle fractions.""" + axis = twist_axis.to(device=self.device, dtype=torch.float32) + axis = axis / torch.linalg.vector_norm(axis) + fractions = fractions.to(device=self.device, dtype=torch.float32) + angles = twist_angle * fractions rotations = ( torch.eye(4, dtype=torch.float32, device=self.device) .reshape(1, 4, 4) - .repeat(waypoint_count, 1, 1) + .repeat(fractions.numel(), 1, 1) ) rotations[:, :3, :3] = axis_angle_to_rotation_matrix(angles[:, None] * axis) link_to_eef = torch.bmm(pose_inv(link_pose), grasp_xpos) diff --git a/embodichain/lab/sim/atomic_actions/runner.py b/embodichain/lab/sim/atomic_actions/runner.py index 20918655b..9f6e4da62 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -417,6 +417,7 @@ def __init__( self._command_sink = command_sink self._clock = clock or MonotonicExecutionClock() self.cfg = cfg or ExecutionRunnerCfg() + self._passive_cycle_time = self._resolve_passive_cycle_time() self._physical_evidence_provider = physical_evidence_provider self._observation_revision = 0 self._last_evidence: PhysicalEvidenceFrame | None = None @@ -821,12 +822,21 @@ def step( dispatches: list[CommandDispatch] = [] if tick.command is not None: - self._remember_targets(tick.command.targets) operation = ( CommandOperation.SEND if bool(tick.command.active_mask.any().item()) else CommandOperation.HOLD ) + try: + interval = self._command_interval(tick.command) + except Exception as exc: + return self._fail( + "Command interval validation failed: " + f"{type(exc).__name__}: {exc}", + context=context, + tick=tick, + ) + self._remember_targets(tick.command.targets) dispatch = self._dispatch( operation, command=(tick.command if operation is CommandOperation.SEND else None), @@ -850,7 +860,6 @@ def step( ) if operation is CommandOperation.SEND: self._command_count += 1 - interval = self._command_interval(tick.command) self._next_step_at = self._clock_now() + interval elif tick.hold_targets and ( tick.pending_effect is None or self.cfg.hold_during_effect_verification @@ -876,10 +885,10 @@ def step( tick=tick, dispatches=dispatches, ) - self._next_step_at = self._clock_now() + self.cfg.minimum_cycle_time + self._next_step_at = self._clock_now() + self._passive_cycle_time elif tick.pending_effect is not None: self._remember_targets(tick.hold_targets) - self._next_step_at = self._clock_now() + self.cfg.minimum_cycle_time + self._next_step_at = self._clock_now() + self._passive_cycle_time else: self._next_step_at = self._clock_now() @@ -1094,18 +1103,63 @@ def _clock_now(self) -> float: def _command_interval(self, command: RuntimeCommandFrame) -> float: """Resolve a synchronized batch interval from per-environment durations.""" - durations = ( - command.hold_duration[command.active_mask] - if command.active_mask.any() - else command.hold_duration - ) - requested = float(durations.max().item()) if durations.numel() else 0.0 - return max(requested, self.cfg.minimum_cycle_time) + if not command.active_mask.any(): + return self._passive_cycle_time + durations = command.hold_duration[command.active_mask] + positive = durations[durations > 0] + if not positive.numel(): + return self._passive_cycle_time + requested = float(positive[0].item()) + tolerance = max(1.0e-9, abs(requested) * 1.0e-6) + if not torch.allclose( + positive, + positive[:1].expand_as(positive), + rtol=1.0e-6, + atol=tolerance, + ): + raise ValueError( + "Active command rows must share one positive hold duration; " + "zero-duration terminal padding is the only allowed difference." + ) + if requested + tolerance < self.cfg.minimum_cycle_time: + raise ValueError( + "Command hold duration is shorter than minimum_cycle_time; " + "the runtime may not silently slow a planned trajectory." + ) + return requested + + def _resolve_passive_cycle_time(self) -> float: + """Align passive polling to an optional clock sleep quantum.""" + requested = self.cfg.minimum_cycle_time + if requested == 0.0: + return 0.0 + quantum = getattr(self._clock, "sleep_quantum", None) + if quantum is None: + return requested + if isinstance(quantum, bool) or not isinstance(quantum, (int, float)): + raise TypeError("ExecutionClock.sleep_quantum must be a real number.") + quantum = float(quantum) + if not math.isfinite(quantum) or quantum <= 0.0: + raise ValueError( + "ExecutionClock.sleep_quantum must be finite and greater than zero." + ) + ratio = requested / quantum + nearest = round(ratio) + if nearest >= 1 and math.isclose( + ratio, + nearest, + rel_tol=1.0e-6, + abs_tol=1.0e-9, + ): + step_count = nearest + else: + step_count = max(1, math.ceil(ratio)) + return step_count * quantum def _remaining_wait(self, now: float) -> float: """Return scheduled wait while absorbing float32 timing roundoff.""" remaining = self._next_step_at - now - tolerance = max(1.0e-9, self.cfg.minimum_cycle_time * 1.0e-6) + tolerance = max(1.0e-9, self._passive_cycle_time * 1.0e-6) return remaining if remaining > tolerance else 0.0 def _dispatch( diff --git a/embodichain/lab/sim/atomic_actions/sim_adapter.py b/embodichain/lab/sim/atomic_actions/sim_adapter.py index 11171d07a..b6ae89217 100644 --- a/embodichain/lab/sim/atomic_actions/sim_adapter.py +++ b/embodichain/lab/sim/atomic_actions/sim_adapter.py @@ -449,6 +449,18 @@ def __init__( ) if not math.isfinite(resolved_control_dt) or resolved_control_dt <= 0.0: raise ValueError("control_dt must be finite and greater than zero.") + control_ratio = resolved_control_dt / resolved_physics_dt + control_step_count = round(control_ratio) + if ( + not math.isclose( + control_ratio, + control_step_count, + rel_tol=1.0e-6, + abs_tol=1.0e-9, + ) + or control_step_count < 1 + ): + raise ValueError("control_dt must be an integer multiple of physics_dt.") qpos = robot.get_qpos() if not isinstance(qpos, torch.Tensor) or qpos.dim() != 2: raise ValueError("robot.get_qpos() must return shape (B, robot_dof).") @@ -491,8 +503,13 @@ def now(self) -> float: """ return self._elapsed_time + @property + def sleep_quantum(self) -> float: + """Return the smallest representable simulated sleep duration.""" + return self.physics_dt + def sleep(self, duration: float) -> None: - """Advance physics by at least the requested duration. + """Advance physics by exactly the requested grid-aligned duration. Args: duration: Requested simulated duration in seconds. @@ -503,19 +520,14 @@ def sleep(self, duration: float) -> None: return step_ratio = duration / self.physics_dt nearest_step_count = round(step_ratio) - step_count = max( - 1, - ( - nearest_step_count - if math.isclose( - step_ratio, - nearest_step_count, - rel_tol=1.0e-6, - abs_tol=1.0e-9, - ) - else math.ceil(step_ratio) - ), - ) + if nearest_step_count < 1 or not math.isclose( + step_ratio, + nearest_step_count, + rel_tol=1.0e-6, + abs_tol=1.0e-9, + ): + raise ValueError("duration must be an integer multiple of physics_dt.") + step_count = nearest_step_count self.simulation.update(physics_dt=self.physics_dt, step=step_count) self._elapsed_time += step_count * self.physics_dt diff --git a/embodichain/lab/sim/atomic_actions/trajectory_ops.py b/embodichain/lab/sim/atomic_actions/trajectory_ops.py index 8a97513e9..ca2fe9f6e 100644 --- a/embodichain/lab/sim/atomic_actions/trajectory_ops.py +++ b/embodichain/lab/sim/atomic_actions/trajectory_ops.py @@ -293,6 +293,10 @@ def interpolate_hand_qpos( n_waypoints: int, ) -> torch.Tensor: """Interpolate hand joint positions between two semantic commands.""" + if isinstance(n_waypoints, bool) or not isinstance(n_waypoints, int): + raise TypeError("n_waypoints must be an integer.") + if n_waypoints < 1: + raise ValueError("n_waypoints must be at least one.") is_unbatched = start_hand_qpos.dim() == 1 and end_hand_qpos.dim() == 1 end_hand_qpos = end_hand_qpos.to( device=start_hand_qpos.device, @@ -302,12 +306,20 @@ def interpolate_hand_qpos( start_hand_qpos = start_hand_qpos.unsqueeze(0) if end_hand_qpos.dim() == 1: end_hand_qpos = end_hand_qpos.unsqueeze(0) - weights = torch.linspace( - 0, - 1, - steps=n_waypoints, - device=start_hand_qpos.device, - dtype=start_hand_qpos.dtype, + weights = ( + torch.ones( + 1, + device=start_hand_qpos.device, + dtype=start_hand_qpos.dtype, + ) + if n_waypoints == 1 + else torch.linspace( + 0, + 1, + steps=n_waypoints, + device=start_hand_qpos.device, + dtype=start_hand_qpos.dtype, + ) ) result = torch.lerp( start_hand_qpos.unsqueeze(1), diff --git a/embodichain/lab/sim/motion/motion_generator.py b/embodichain/lab/sim/motion/motion_generator.py index 3c8f3c0d6..b3d470ec5 100644 --- a/embodichain/lab/sim/motion/motion_generator.py +++ b/embodichain/lab/sim/motion/motion_generator.py @@ -17,7 +17,7 @@ from __future__ import annotations import math -from collections.abc import Mapping +from collections.abc import Callable, Mapping from copy import deepcopy from dataclasses import MISSING from typing import Literal @@ -48,7 +48,17 @@ resample_in_time, differentiate_positions, ) +from embodichain.compute.trajectory.interpolation import ( + _minimum_jerk_progress, + interpolate_minimum_jerk, +) from embodichain.utils import logger, configclass +from embodichain.utils.math import ( + axis_angle_from_quat, + axis_angle_to_rotation_matrix, + quat_error_magnitude, + quat_from_matrix, +) from .planners.utils import ( MoveType, PlanResult, @@ -90,10 +100,10 @@ class MotionGenOptions: """Requested output sample count; ``None`` preserves backend defaults.""" velocity_limit: float | None = None - """Optional scalar joint velocity limit used by compatible backends.""" + """Optional scalar speed request, capped by effective Robot joint limits.""" acceleration_limit: float | None = None - """Optional scalar joint acceleration limit used by compatible backends.""" + """Optional scalar planner-owned joint acceleration budget.""" start_qpos: torch.Tensor | None = None """Observed controlled-joint start, shape ``(B, DOF)``. @@ -135,10 +145,22 @@ class MotionGenOptions: """ preserve_cartesian_samples: bool = False - """Treat Cartesian targets as exact output samples in ``ik_interp`` only. + """Treat Cartesian targets as an exact sampled path in ``ik_interp`` only. This constrained mode requires exactly ``sample_count - 1`` target states; - the observed start configuration supplies the first output sample. + the observed start configuration supplies the first sample. Without motion + limits, targets are emitted unchanged. With limits, their Cartesian path is + resampled by a minimum-jerk progress law and every emitted pose is solved by + IK. + """ + + cartesian_path_sampler: Callable[[torch.Tensor], torch.Tensor] | None = None + """Optional exact-path sampler for normalized progress values. + + The callback receives a one-dimensional tensor spanning zero through one + and returns poses with shape ``(B, T, 4, 4)``. Use it for paths whose + geometry cannot be recovered from Cartesian chords, such as rotation about + an external hinge. It is valid only with ``preserve_cartesian_samples``. """ interpolate_position_step: float = 0.002 @@ -154,13 +176,17 @@ def __post_init__(self) -> None: "preserve_cartesian_samples requires strategy='ik_interp'; " "planner backends do not guarantee exact Cartesian samples." ) - if self.strategy == "ik_interp" and any( - value is not None - for value in (self.plan_opts, self.velocity_limit, self.acceleration_limit) - ): + if self.cartesian_path_sampler is not None: + if not callable(self.cartesian_path_sampler): + raise TypeError("cartesian_path_sampler must be callable or None.") + if not self.preserve_cartesian_samples: + raise ValueError( + "cartesian_path_sampler requires preserve_cartesian_samples=True." + ) + if self.strategy == "ik_interp" and self.plan_opts is not None: raise ValueError( - "strategy='ik_interp' does not consume plan_opts, velocity_limit, " - "or acceleration_limit; use strategy='motion_gen' for backend limits." + "strategy='ik_interp' does not consume plan_opts; use " + "strategy='motion_gen' for backend-specific options." ) if ( self.strategy == "ik_interp" @@ -179,10 +205,14 @@ def __post_init__(self) -> None: ) if self.sample_count is not None and self.sample_count < 2: raise ValueError("sample_count must be at least 2 when set.") - if self.velocity_limit is not None and self.velocity_limit <= 0.0: - raise ValueError("velocity_limit must be greater than zero when set.") - if self.acceleration_limit is not None and self.acceleration_limit <= 0.0: - raise ValueError("acceleration_limit must be greater than zero when set.") + if self.velocity_limit is not None and ( + not math.isfinite(self.velocity_limit) or self.velocity_limit <= 0.0 + ): + raise ValueError("velocity_limit must be finite and greater than zero.") + if self.acceleration_limit is not None and ( + not math.isfinite(self.acceleration_limit) or self.acceleration_limit <= 0.0 + ): + raise ValueError("acceleration_limit must be finite and greater than zero.") if self.interpolation_dt is not None: if isinstance(self.interpolation_dt, bool) or not isinstance( self.interpolation_dt, (int, float) @@ -468,8 +498,8 @@ def resolve_plan_options( if planner_type == "toppra" else TrapezoidalPlanOptions ) - constraints: dict[str, float] = { - "velocity": 0.2 if velocity_limit is None else velocity_limit, + constraints: dict[str, float | None] = { + "velocity": velocity_limit, "acceleration": ( 0.5 if acceleration_limit is None else acceleration_limit ), @@ -485,6 +515,128 @@ def resolve_plan_options( return options_type(**options_kwargs) return self.planner.default_plan_options() + def _resolve_velocity_limits( + self, + *, + control_part: str | None, + batch_size: int, + dof: int, + requested: float | None, + reference: torch.Tensor, + ) -> torch.Tensor: + """Intersect one scalar request with effective ordered Robot speed caps. + + Runtime joint-speed metadata is reduced across the participating batch, + and mimic follower caps are projected back to their active parents. + Missing or unbounded metadata uses the historical finite fallback only + for the affected joint. + """ + joint_names = getattr(self.robot, "joint_names", None) + use_full_limits = isinstance(joint_names, (list, tuple)) and bool(joint_names) + if use_full_limits: + joint_count = len(joint_names) + limits = ( + self.robot.get_qvel_limits(joint_ids=list(range(joint_count))) + .detach() + .clone() + ) + if limits.ndim != 2 or limits.shape[1] != joint_count: + raise ValueError( + "Robot velocity limits must have shape (B, robot DOF)." + ) + limits = torch.where( + torch.isfinite(limits) & (limits >= 0.0) & (limits < 1.0e9), + limits, + torch.inf, + ) + mimic_ids = tuple(getattr(self.robot, "mimic_ids", ())) + mimic_parents = tuple(getattr(self.robot, "mimic_parents", ())) + mimic_multipliers = tuple(getattr(self.robot, "mimic_multipliers", ())) + if not (len(mimic_ids) == len(mimic_parents) == len(mimic_multipliers)): + raise ValueError("Robot mimic metadata lengths must match.") + mimics = dict( + zip( + mimic_ids, + zip(mimic_parents, mimic_multipliers, strict=True), + strict=True, + ) + ) + for original_child in mimics: + if not isinstance(original_child, int): + raise ValueError("Mimic child indices must be integers.") + child = original_child + bound = limits[:, child].clone() + visited = {child} + while child in mimics: + parent, multiplier = mimics[child] + if ( + not isinstance(parent, int) + or isinstance(multiplier, bool) + or not isinstance(multiplier, (int, float)) + or not math.isfinite(multiplier) + or parent in visited + ): + raise ValueError( + "Mimic relations require finite multipliers and an " + "acyclic integer parent mapping." + ) + if multiplier == 0.0: + break + bound = bound / abs(multiplier) + limits[:, parent] = torch.minimum(limits[:, parent], bound) + visited.add(parent) + child = parent + if control_part is None and dof != joint_count: + matches = [ + name + for name in (self.robot.control_parts or {}) + if len(self.robot.get_joint_ids(name)) == dof + ] + if len(matches) != 1: + raise ValueError( + "Set control_part to map interpolated joints to Robot " + "velocity limits." + ) + control_part = matches[0] + joint_ids = ( + list(range(dof)) + if control_part is None + else self.robot.get_joint_ids(control_part) + ) + if len(joint_ids) != dof: + raise ValueError( + "control_part joint count does not match interpolation DOF." + ) + limits = limits[:, joint_ids] + else: + limits = self.robot.get_qvel_limits(name=control_part).detach().clone() + if limits.ndim != 2 or limits.shape[1] != dof: + raise ValueError( + "Robot velocity limits must have shape (B, interpolation DOF)." + ) + if limits.shape[0] < batch_size: + raise ValueError( + "Robot velocity limits do not cover the interpolation batch." + ) + limits = limits[:batch_size].to( + device=reference.device, + dtype=reference.dtype, + ) + limits = torch.where( + torch.isfinite(limits) & (limits >= 0.0) & (limits < 1.0e9), + limits, + torch.inf, + ) + caps = limits.amin(dim=0) + if requested is None: + return torch.where(torch.isfinite(caps), caps, 0.2) + requested_limits = torch.full_like(caps, float(requested)) + return torch.where( + torch.isfinite(caps), + torch.minimum(caps, requested_limits), + requested_limits, + ) + @classmethod def register_planner_type(cls, name: str, planner_class, planner_cfg_class) -> None: """ @@ -697,6 +849,29 @@ def _generate_with_planner( # lower bound instead of rejecting an otherwise valid path. An # explicit TrapezoidalPlanOptions remains caller-authoritative. plan_opts.sample_interval = len(target_plan_states) + planner_context_method = getattr( + type(self.planner), "with_motion_context", None + ) + legacy_backend_limits = planner_context_method in { + None, + BasePlanner.with_motion_context, + } + if ( + not explicit_plan_options + and legacy_backend_limits + and isinstance(plan_opts, (ToppraPlanOptions, TrapezoidalPlanOptions)) + ): + reference_qpos = target_plan_states[0].qpos + if reference_qpos is not None: + if reference_qpos.dim() == 1: + reference_qpos = reference_qpos.unsqueeze(0) + plan_opts.constraints["velocity"] = self._resolve_velocity_limits( + control_part=options.control_part, + batch_size=reference_qpos.shape[0], + dof=reference_qpos.shape[1], + requested=options.velocity_limit, + reference=reference_qpos, + ) plan_opts = self.planner.with_motion_context( plan_opts, start_qpos=options.start_qpos, @@ -888,11 +1063,7 @@ def _generate_ik_interpolation( [start_qpos.unsqueeze(1), torch.stack(qpos_targets, dim=1)], dim=1, ) - positions = interpolate_with_distance( - trajectory=keyframes, - interp_num=options.sample_count, - device=device, - ) + positions = self._interpolate_ik_keyframes(keyframes, options) dt = self._uniform_dt( batch_size=batch_size, waypoint_count=positions.shape[1], @@ -933,13 +1104,34 @@ def _generate_ik_interpolation( f"{len(target_states)} targets for sample_count " f"{options.sample_count}." ) - positions = keyframes + preserve_fixed_grid = False + if options.velocity_limit is None and options.acceleration_limit is None: + velocity_limits = self._resolve_velocity_limits( + control_part=options.control_part, + batch_size=batch_size, + dof=controlled_dof, + requested=None, + reference=keyframes, + ) + preserve_fixed_grid = bool( + ( + keyframes.diff(dim=1).abs() / options.interpolation_dt + <= velocity_limits.view(1, 1, -1) + 1.0e-6 + ) + .all() + .item() + ) + if preserve_fixed_grid: + positions = keyframes + else: + limited_success, positions = self._interpolate_cartesian_with_limits( + torch.stack(poses, dim=1), + keyframes, + options, + ) + success &= limited_success else: - positions = interpolate_with_distance( - trajectory=keyframes, - interp_num=options.sample_count, - device=device, - ) + positions = self._interpolate_ik_keyframes(keyframes, options) dt = self._uniform_dt( batch_size=batch_size, waypoint_count=positions.shape[1], @@ -952,6 +1144,293 @@ def _generate_ik_interpolation( dt=dt, ) + def _interpolate_cartesian_with_limits( + self, + target_poses: torch.Tensor, + solved_keyframes: torch.Tensor, + options: MotionGenOptions, + ) -> tuple[torch.Tensor, torch.Tensor]: + """Retain a sampled Cartesian path while enforcing joint motion limits.""" + assert options.control_part is not None + assert options.sample_count is not None + assert options.interpolation_dt is not None + velocity_limits = self._resolve_velocity_limits( + control_part=options.control_part, + batch_size=solved_keyframes.shape[0], + dof=solved_keyframes.shape[2], + requested=options.velocity_limit, + reference=solved_keyframes, + ) + acceleration_limit = ( + 0.5 if options.acceleration_limit is None else options.acceleration_limit + ) + acceleration_limits = torch.full_like(velocity_limits, acceleration_limit) + variation = solved_keyframes.diff(dim=1).abs().sum(dim=1) + zero_velocity_motion = (velocity_limits == 0.0) & (variation > 0.0).any(dim=0) + if zero_velocity_motion.any(): + raise ValueError("A zero-velocity Robot joint cannot follow this path.") + safe_velocity_limits = torch.where( + velocity_limits == 0.0, + torch.ones_like(velocity_limits), + velocity_limits, + ) + duration = torch.maximum( + (1.875 * variation / safe_velocity_limits).amax(dim=1), + torch.sqrt((10.0 / math.sqrt(3.0)) * variation / acceleration_limits).amax( + dim=1 + ), + ) + interval_count = max( + options.sample_count - 1, + int(torch.ceil(duration.max() / options.interpolation_dt).item()), + ) + success = torch.ones( + solved_keyframes.shape[0], + dtype=torch.bool, + device=solved_keyframes.device, + ) + positions = solved_keyframes + for _ in range(6): + if interval_count > 10_000: + raise ValueError( + "Cartesian path requires more than 10000 control intervals " + "to satisfy the requested motion limits." + ) + sampled_poses = self._sample_cartesian_path( + target_poses, + solved_keyframes, + interval_count + 1, + options.control_part, + path_sampler=options.cartesian_path_sampler, + ) + iteration_success, solved = self._solve_cartesian_waypoints( + sampled_poses[:, 1:], + solved_keyframes[:, 0], + options.control_part, + ) + success &= iteration_success + positions = torch.cat([solved_keyframes[:, :1], solved], dim=1) + interval_velocity = positions.diff(dim=1) / options.interpolation_dt + boundary_acceleration = ( + torch.cat( + [ + interval_velocity[:, :1], + interval_velocity.diff(dim=1), + -interval_velocity[:, -1:], + ], + dim=1, + ) + / options.interpolation_dt + ) + velocity_ratio = float( + (interval_velocity.abs() / safe_velocity_limits.view(1, 1, -1)) + .amax() + .item() + ) + acceleration_ratio = float( + (boundary_acceleration.abs() / acceleration_limits.view(1, 1, -1)) + .amax() + .item() + ) + scale = max(velocity_ratio, math.sqrt(acceleration_ratio)) + if scale <= 1.0 + 1.0e-5: + return success, positions + interval_count = math.ceil(interval_count * scale * 1.02) + + interval_velocity = positions.diff(dim=1) / options.interpolation_dt + boundary_acceleration = ( + torch.cat( + [ + interval_velocity[:, :1], + interval_velocity.diff(dim=1), + -interval_velocity[:, -1:], + ], + dim=1, + ) + / options.interpolation_dt + ) + success &= ( + interval_velocity.abs() <= velocity_limits.view(1, 1, -1) + 1.0e-6 + ).all(dim=(1, 2)) + success &= ( + boundary_acceleration.abs() <= acceleration_limits.view(1, 1, -1) + 1.0e-5 + ).all(dim=(1, 2)) + return success, positions + + def _sample_cartesian_path( + self, + target_poses: torch.Tensor, + solved_keyframes: torch.Tensor, + sample_count: int, + control_part: str, + *, + path_sampler: Callable[[torch.Tensor], torch.Tensor] | None = None, + ) -> torch.Tensor: + """Sample an exact path or Cartesian polyline with minimum-jerk progress.""" + if self.robot is None: + raise RuntimeError("MotionGenerator requires a robot before interpolation.") + start_pose = self.robot.compute_fk( + qpos=solved_keyframes[:, 0], + name=control_part, + to_matrix=True, + ).to(device=target_poses.device, dtype=target_poses.dtype) + path = torch.cat([start_pose[:, None], target_poses], dim=1) + segment_lengths = solved_keyframes.diff(dim=1).abs().amax(dim=-1) + cumulative = torch.cumsum(segment_lengths, dim=1) + total = cumulative[:, -1:] + progress = _minimum_jerk_progress( + sample_count, + device=target_poses.device, + dtype=target_poses.dtype, + ) + if path_sampler is not None: + result = path_sampler(progress) + expected_shape = (target_poses.shape[0], sample_count, 4, 4) + if not isinstance(result, torch.Tensor) or result.shape != expected_shape: + raise ValueError( + "cartesian_path_sampler must return a tensor with shape " + f"{expected_shape}." + ) + if result.device != target_poses.device: + raise ValueError( + "cartesian_path_sampler output must share the target device." + ) + if result.dtype != target_poses.dtype: + raise ValueError( + "cartesian_path_sampler output must share the target dtype." + ) + if not torch.isfinite(result).all(): + raise ValueError("cartesian_path_sampler output must be finite.") + position_tolerance, rotation_tolerance = self._cartesian_start_tolerances( + control_part + ) + start_position_error = torch.linalg.vector_norm( + result[:, 0, :3, 3] - path[:, 0, :3, 3], + dim=-1, + ) + start_rotation_error = quat_error_magnitude( + quat_from_matrix(result[:, 0, :3, :3]), + quat_from_matrix(path[:, 0, :3, :3]), + ) + start_matches = bool( + (start_position_error <= position_tolerance).all() + and (start_rotation_error <= rotation_tolerance).all() + and torch.allclose( + result[:, 0, 3], + path[:, 0, 3], + rtol=0.0, + atol=1.0e-6, + ) + ) + if not start_matches or not torch.allclose( + result[:, -1], path[:, -1], rtol=1.0e-5, atol=1.0e-6 + ): + raise ValueError( + "cartesian_path_sampler must preserve the requested path endpoints." + ) + return result + query = total * progress[None] + segment_index = torch.searchsorted( + cumulative.contiguous(), query.contiguous(), right=False + ).clamp(max=segment_lengths.shape[1] - 1) + segment_start_distance = torch.cat( + [torch.zeros_like(cumulative[:, :1]), cumulative[:, :-1]], dim=1 + ).gather(1, segment_index) + selected_length = segment_lengths.gather(1, segment_index) + alpha = torch.where( + selected_length > 0, + (query - segment_start_distance) / selected_length.clamp_min(1.0e-12), + torch.zeros_like(query), + ).clamp(0.0, 1.0) + gather_index = segment_index[:, :, None, None].expand(-1, -1, 4, 4) + start = torch.gather(path[:, :-1], 1, gather_index) + end = torch.gather(path[:, 1:], 1, gather_index) + result = ( + torch.eye(4, dtype=target_poses.dtype, device=target_poses.device) + .expand(target_poses.shape[0], sample_count, -1, -1) + .clone() + ) + result[:, :, :3, 3] = torch.lerp( + start[:, :, :3, 3], end[:, :, :3, 3], alpha[:, :, None] + ) + relative_rotation = torch.matmul( + start[:, :, :3, :3].transpose(-1, -2), end[:, :, :3, :3] + ) + axis_angle = axis_angle_from_quat(quat_from_matrix(relative_rotation)) + incremental_rotation = axis_angle_to_rotation_matrix( + axis_angle * alpha[:, :, None] + ) + result[:, :, :3, :3] = torch.matmul(start[:, :, :3, :3], incremental_rotation) + result[:, 0] = path[:, 0] + result[:, -1] = path[:, -1] + return result + + def _cartesian_start_tolerances( + self, + control_part: str, + ) -> tuple[float, float]: + """Return the active IK solver's accepted position and rotation errors.""" + solver = self.robot.get_solver(control_part) + cfg = None if solver is None else getattr(solver, "cfg", None) + + def configured_tolerance(*names: str) -> float: + for name in names: + value = getattr(cfg, name, None) + if ( + not isinstance(value, bool) + and isinstance(value, (int, float)) + and math.isfinite(value) + and value > 0.0 + ): + return max(1.0e-6, float(value)) + return 1.0e-6 + + return configured_tolerance( + "pos_eps", "position_tolerance" + ), configured_tolerance("rot_eps", "rotation_tolerance") + + def _interpolate_ik_keyframes( + self, + keyframes: torch.Tensor, + options: MotionGenOptions, + ) -> torch.Tensor: + """Interpolate IK keyframes under Robot-capped per-joint speeds.""" + assert options.sample_count is not None + assert options.interpolation_dt is not None + velocity_limits = self._resolve_velocity_limits( + control_part=options.control_part, + batch_size=keyframes.shape[0], + dof=keyframes.shape[2], + requested=options.velocity_limit, + reference=keyframes, + ) + if options.velocity_limit is None and options.acceleration_limit is None: + fixed_grid = interpolate_with_distance( + trajectory=keyframes, + interp_num=options.sample_count, + device=keyframes.device, + ) + if bool( + ( + fixed_grid.diff(dim=1).abs() / options.interpolation_dt + <= velocity_limits.view(1, 1, -1) + 1.0e-6 + ) + .all() + .item() + ): + return fixed_grid + return interpolate_minimum_jerk( + keyframes, + minimum_sample_count=options.sample_count, + step_dt=options.interpolation_dt, + velocity_limit=velocity_limits, + acceleration_limit=( + 0.5 + if options.acceleration_limit is None + else options.acceleration_limit + ), + ) + @staticmethod def _uniform_dt( *, @@ -1045,6 +1524,10 @@ def _normalize_plan_result( options.sample_count is not None and not preserve_samples and positions.shape[1] != options.sample_count + and not ( + options.strategy == "ik_interp" + and positions.shape[1] >= options.sample_count + ) ): positions, dt = resample_in_time(positions, dt, options.sample_count) resampled = True @@ -1114,6 +1597,14 @@ def normalize_derivative( ) if velocities is None: velocities = differentiate_positions(positions, dt) + if options.strategy == "ik_interp" and positions.shape[1]: + velocities = velocities.clone() + velocities[:, 0] = 0.0 + velocities[:, -1] = 0.0 + if positions.shape[1] > 1: + stationary = positions[:, 1:] == positions[:, :-1] + velocities[:, :-1] = velocities[:, :-1].masked_fill(stationary, 0.0) + velocities[:, 1:] = velocities[:, 1:].masked_fill(stationary, 0.0) return PlanResult( success=success, diff --git a/embodichain/lab/sim/motion/planners/toppra_planner.py b/embodichain/lab/sim/motion/planners/toppra_planner.py index fd7d5c717..61c9b2b6f 100644 --- a/embodichain/lab/sim/motion/planners/toppra_planner.py +++ b/embodichain/lab/sim/motion/planners/toppra_planner.py @@ -57,6 +57,13 @@ def _build_constraint_arrays(value, acc, dofs: int) -> tuple[np.ndarray, np.ndar return vlims, alims +def _constraint_for_numpy_worker(value: object) -> object: + """Materialize tensor-backed limits on CPU before NumPy worker dispatch.""" + if isinstance(value, torch.Tensor): + return value.detach().cpu().numpy() + return value + + def _toppra_solve_one_env( waypoints: np.ndarray, vel_constraint, @@ -438,8 +445,8 @@ def plan( [s.qpos.detach().cpu().numpy() for s in target_states], axis=1 ) # (B, N, DOF) - vc = options.constraints["velocity"] - ac = options.constraints["acceleration"] + vc = _constraint_for_numpy_worker(options.constraints["velocity"]) + ac = _constraint_for_numpy_worker(options.constraints["acceleration"]) args_per_env = [ (waypoints[i], vc, ac, options.sample_method, options.sample_interval) for i in range(b) diff --git a/embodichain/lab/sim/motion/solvers/pytorch_solver.py b/embodichain/lab/sim/motion/solvers/pytorch_solver.py index 86bf8070e..9627efd09 100644 --- a/embodichain/lab/sim/motion/solvers/pytorch_solver.py +++ b/embodichain/lab/sim/motion/solvers/pytorch_solver.py @@ -414,10 +414,11 @@ def get_ik( **kwargs: Additional arguments for future extensions. Returns: - tuple[list[bool], torch.Tensor]: A tuple containing: - - A tensor of booleans indicating whether valid solutions were found for each target pose. (Shape: (batch_size,)) - - A tensor of shape (batch_size, 1, dof) containing joint positions for - each target pose, or an empty tensor if no valid solutions were found. + A success tensor and selected joint positions. By default their + shapes are ``(batch_size,)`` and ``(batch_size, 1, dof)``. With + ``return_all_solutions=True``, per-candidate validity and positions + have shapes ``(batch_size, num_samples)`` and + ``(batch_size, num_samples, dof)``. """ # Convert target_pose to tensor and ensure correct device and dtype target_xpos = torch.as_tensor( @@ -488,11 +489,11 @@ def get_ik( ) if is_ik_success.any().item() is False: logger.log_warning("No IK solutions found for any of the target poses.") - failed_state = is_ik_success.reshape(batch_size, self._num_samples)[:, 0] - failed_qpos = ik_qpos.reshape(batch_size, self._num_samples, self.dof)[ - :, 0, : - ] - return failed_state, failed_qpos + failed_state = is_ik_success.reshape(batch_size, self._num_samples) + failed_qpos = ik_qpos.reshape(batch_size, self._num_samples, self.dof) + if return_all_solutions: + return failed_state, failed_qpos + return failed_state[:, 0], failed_qpos[:, 0] # map ik_qpos to within limits and check validity is_mask_valid, ik_qpos_mapped = self._qpos_map_to_limits(ik_qpos) is_success = torch.logical_and(is_ik_success, is_mask_valid) @@ -501,7 +502,7 @@ def get_ik( all_results = ik_qpos_mapped.reshape(batch_size, self._num_samples, self.dof) if return_all_solutions: - return all_is_success.any(dim=1), all_results + return all_is_success, all_results if self._solution_selection == "manipulability": # Re-rank successful candidates by Yoshikawa manipulability via @@ -523,6 +524,59 @@ def get_ik( closest_qpos = all_results[torch.arange(batch_size), closest_indices] return all_is_success.any(dim=1), closest_qpos[:, None, :] + @property + def supports_continuous_batch_ik(self) -> bool: + """Return whether batched candidates support continuous path selection.""" + return True + + def _select_continuous_ik_path( + self, + candidate_qpos: torch.Tensor, + candidate_valid: torch.Tensor, + qpos_seed: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor]: + """Select each IK sample nearest to the preceding selected posture.""" + if candidate_qpos.ndim != 4: + raise ValueError("candidate_qpos must have shape (B, N, K, DOF).") + batch_size, sample_count, candidate_count, dof = candidate_qpos.shape + if dof != self.dof: + raise ValueError(f"candidate_qpos must have {self.dof} joints, got {dof}.") + if candidate_valid.shape != (batch_size, sample_count, candidate_count): + raise ValueError( + "candidate_valid must match the first three candidate_qpos " + "dimensions." + ) + if qpos_seed.shape != (batch_size, dof): + raise ValueError(f"qpos_seed must have shape ({batch_size}, {dof}).") + + candidates = candidate_qpos.to(device=self.device, dtype=torch.float32) + valid_candidates = candidate_valid.to(device=self.device, dtype=torch.bool) + previous = qpos_seed.to(device=self.device, dtype=torch.float32) + weights = self.ik_nearest_weight.to( + device=self.device, dtype=torch.float32 + ).view(1, 1, dof) + batch_indices = torch.arange(batch_size, device=self.device) + path_valid: list[torch.Tensor] = [] + path_qpos: list[torch.Tensor] = [] + + for sample_index in range(sample_count): + sample_candidates = candidates[:, sample_index] + sample_valid = valid_candidates[:, sample_index] & torch.isfinite( + sample_candidates + ).all(dim=-1) + weighted_delta = (sample_candidates - previous[:, None]) * weights + distances = torch.linalg.vector_norm(weighted_delta, dim=-1) + distances = distances.masked_fill(~sample_valid, float("inf")) + best_indices = torch.argmin(distances, dim=1) + has_solution = sample_valid.any(dim=1) + selected = sample_candidates[batch_indices, best_indices] + selected = torch.where(has_solution[:, None], selected, previous) + path_valid.append(has_solution) + path_qpos.append(selected) + previous = selected + + return torch.stack(path_valid, dim=1), torch.stack(path_qpos, dim=1) + def get_all_fk(self, qpos: torch.tensor) -> torch.tensor: r"""Get the forward kinematics for all links from root to end link. diff --git a/embodichain/lab/sim/objects/robot.py b/embodichain/lab/sim/objects/robot.py index fb2542744..e7cc33c8b 100644 --- a/embodichain/lab/sim/objects/robot.py +++ b/embodichain/lab/sim/objects/robot.py @@ -1017,7 +1017,7 @@ def compute_batch_ik( ) -> tuple[torch.Tensor, torch.Tensor] | None: """Compute batched inverse kinematics for arena-frame poses. - When ``continuous`` is enabled, all analytic candidates are generated + When ``continuous`` is enabled, all solver candidates are generated in one solver call and the branch nearest to the previous path sample is selected sequentially. The selected solver must support continuous candidate selection. @@ -1124,9 +1124,14 @@ def compute_batch_ik( pose_batch.reshape(len(local_env_ids), n_batch, 4, 4), ) if continuous: + repeated_seed = ( + joint_seed_tensor[:, None] + .expand(batch_size, n_batch, n_dof) + .reshape(-1, n_dof) + ) candidate_valid, candidate_qpos = solver.get_ik( target_xpos=pose_batch.reshape(-1, 4, 4), - qpos_seed=None, + qpos_seed=repeated_seed, return_all_solutions=True, ) return solver._select_continuous_ik_path( diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index a161e067a..1c1534143 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -927,6 +927,7 @@ def _decode_action_options(value: object, *, path: str) -> ActionOptions: "approach_distance", "translation_distance", "rotate_angle", + "dwell_duration", "approach_height", "retract_height", "contact_distance", @@ -1107,7 +1108,7 @@ def _decode_action_options(value: object, *, path: str) -> ActionOptions: value, path=path, required=frozenset({"kind"}), - optional=frozenset({"rotate_angle"}), + optional=frozenset({"rotate_angle", "dwell_duration"}), ) kwargs = {} if "rotate_angle" in config: @@ -1115,6 +1116,12 @@ def _decode_action_options(value: object, *, path: str) -> ActionOptions: config["rotate_angle"], path=f"{path}.rotate_angle", ) + if "dwell_duration" in config: + kwargs["dwell_duration"] = _real( + config["dwell_duration"], + path=f"{path}.dwell_duration", + minimum=0.0, + ) return PourOptions(**kwargs) if kind == "push_object": config = _mapping( @@ -1310,28 +1317,45 @@ def _decode_motion_policy(value: object, *, path: str) -> MotionPolicy: value, path=path, required=frozenset({"sample_count"}), - optional=frozenset({"strategy", "dynamic_collision_mode", "velocity_targets"}), + optional=frozenset( + { + "strategy", + "velocity_limit", + "acceleration_limit", + "dynamic_collision_mode", + "velocity_targets", + } + ), ) - return MotionPolicy( - strategy=_identifier( + kwargs: dict[str, object] = { + "strategy": _identifier( config.get("strategy", "ik_interp"), path=f"{path}.strategy", ), - velocity_targets=_identifier( + "velocity_targets": _identifier( config.get("velocity_targets", "auto"), path=f"{path}.velocity_targets", ), - sample_count=_integer( + "sample_count": _integer( config["sample_count"], path=f"{path}.sample_count", minimum=2, ), - dynamic_collision_mode=_enum( + "dynamic_collision_mode": _enum( config.get("dynamic_collision_mode", DynamicCollisionMode.AUTO.value), DynamicCollisionMode, path=f"{path}.dynamic_collision_mode", ), - ) + } + for field_name in ("velocity_limit", "acceleration_limit"): + if field_name in config: + kwargs[field_name] = _real( + config[field_name], + path=f"{path}.{field_name}", + minimum=0.0, + strict_minimum=True, + ) + return MotionPolicy(**kwargs) def _decode_tracking_policy(value: object, *, path: str) -> TrackingPolicy: diff --git a/embodichain/lab/task_program/runtime/results.py b/embodichain/lab/task_program/runtime/results.py index 27196a757..aaac09396 100644 --- a/embodichain/lab/task_program/runtime/results.py +++ b/embodichain/lab/task_program/runtime/results.py @@ -420,6 +420,8 @@ def _motion_policy_to_metadata(policy: MotionPolicy) -> dict[str, object]: return { "strategy": policy.strategy, "sample_count": policy.sample_count, + "velocity_limit": _metadata_value(policy.velocity_limit), + "acceleration_limit": _metadata_value(policy.acceleration_limit), "dynamic_collision_mode": policy.dynamic_collision_mode.value, "plan_options": options_metadata, } diff --git a/scripts/benchmark/motion_generation/README.md b/scripts/benchmark/motion_generation/README.md index 1154f94ad..70b10cad3 100644 --- a/scripts/benchmark/motion_generation/README.md +++ b/scripts/benchmark/motion_generation/README.md @@ -45,7 +45,8 @@ python -m scripts.benchmark.motion_generation.run_benchmark \ Artifacts land under `outputs/benchmarks///` (`resolved_suite.yaml`, `case_manifest.json`, `trials.jsonl`, `aggregates.json`, `report.md` with exactly three tables). Atomic Task videos, when enabled, land -in that run's `videos/` directory. +in that run's `videos/` directory. `aggregates.json` and `report.md` record the +independent `report_schema_version` used to interpret their fields. ## Implemented diff --git a/scripts/benchmark/motion_generation/aggregation.py b/scripts/benchmark/motion_generation/aggregation.py index 881c5d15d..85875f21c 100644 --- a/scripts/benchmark/motion_generation/aggregation.py +++ b/scripts/benchmark/motion_generation/aggregation.py @@ -317,6 +317,19 @@ def _track_ids(records: list[TrialRecord], cases: list[BenchmarkCase]) -> list[s return sorted(tracks) +def _bypassed_case_ids( + records: Iterable[TrialRecord], algorithm_id: str, track: str +) -> frozenset[str]: + """Return cases whose scenario did not call the selected planner backend.""" + return frozenset( + record.case_id + for record in records + if record.algorithm_id == algorithm_id + and record.track == track + and not record.uses_selected_planner_backend + ) + + def _lifecycle_value( records: list[TrialRecord], track: str, @@ -337,6 +350,7 @@ def _lifecycle_value( and record.algorithm_id == algorithm_id and record.batch_size == batch_size and record.phase is phase + and record.uses_selected_planner_backend and (waypoint_count is None or record.waypoint_count == waypoint_count) ): return record.cost_time_ms @@ -348,13 +362,13 @@ def _performance_rows( metadata: list[PlannerMetadata], cases: list[BenchmarkCase], ) -> list[dict[str, object]]: - """Aggregate steady-state time and memory by track, algorithm, and input shape.""" + """Aggregate planner-attributed time and memory by track and input shape.""" measured_groups: dict[ tuple[str, str, str, str, str | None, str | None, int, int], list[TrialRecord], ] = defaultdict(list) for record in records: - if record.phase is TrialPhase.MEASURED: + if record.phase is TrialPhase.MEASURED and record.uses_selected_planner_backend: measured_groups[ ( record.track, @@ -518,7 +532,8 @@ def _metric_rows( Boolean success columns use the same case-level macro average as the leaderboard (missing cases contribute ``0.0``). Continuous quality metrics - remain conditioned on externally motion-valid outcomes. + remain conditioned on externally motion-valid outcomes. Cases whose + scenario bypasses the selected planner are excluded from planner metrics. """ measured_by_key: dict[ tuple[ @@ -538,7 +553,12 @@ def _metric_rows( list[TrialRecord], ] = defaultdict(list) unsupported_by_algorithm_track: dict[tuple[str, str], set[str]] = defaultdict(set) + bypassed_by_algorithm_track: dict[tuple[str, str], set[str]] = defaultdict(set) for record in records: + if not record.uses_selected_planner_backend: + bypassed_by_algorithm_track[(record.algorithm_id, record.track)].add( + record.case_id + ) if ( record.phase is TrialPhase.AVAILABILITY and record.status == "unsupported" @@ -548,7 +568,10 @@ def _metric_rows( unsupported_by_algorithm_track[(record.algorithm_id, record.track)].add( record.case_id ) - if record.phase is not TrialPhase.MEASURED: + if ( + record.phase is not TrialPhase.MEASURED + or not record.uses_selected_planner_backend + ): continue key = ( record.track, @@ -645,7 +668,14 @@ def _metric_rows( path_shape, start_state_bin, ) = group_key - group_cases = cases_by_group[group_key] + bypassed_case_ids = bypassed_by_algorithm_track[(info.algorithm_id, track)] + group_cases = [ + case + for case in cases_by_group[group_key] + if case.case_id not in bypassed_case_ids + ] + if not group_cases: + continue measured = measured_by_key.get( ( track, @@ -668,7 +698,7 @@ def _metric_rows( ) outcomes = [outcome for record in measured for outcome in record.outcomes] valid_outcomes = [outcome for outcome in outcomes if outcome.motion_valid] - expected = expected_by_group[group_key] + expected = sum(case.batch_size for case in group_cases) * measured_trials rows.append( { "track": track, @@ -844,22 +874,31 @@ def _leaderboard_rows( Success rates are macro-averaged over mandatory cases (equal case weight). ``latency_p95_ms`` uses the same case-equal weighting: per-case mean warm latency, then nearest-rank p95 across cases. ``coverage_rate`` remains an - outcome-count completeness check used for eligibility. + outcome-count completeness check used for eligibility. Cases explicitly + attributed to another motion algorithm are excluded from both ranking + numerators and denominators. """ entries: list[dict[str, object]] = [] for track in _track_ids(records, cases): - track_cases = [case for case in cases if case.track == track] - expected_outcomes = ( - sum(case.batch_size for case in track_cases) * measured_trials - ) + all_track_cases = [case for case in cases if case.track == track] track_entries: list[dict[str, object]] = [] for info in metadata: + bypassed_case_ids = _bypassed_case_ids(records, info.algorithm_id, track) + track_cases = [ + case + for case in all_track_cases + if case.case_id not in bypassed_case_ids + ] + expected_outcomes = ( + sum(case.batch_size for case in track_cases) * measured_trials + ) measured = [ record for record in records if record.algorithm_id == info.algorithm_id and record.track == track and record.phase is TrialPhase.MEASURED + and record.uses_selected_planner_backend ] outcomes = [outcome for record in measured for outcome in record.outcomes] excluded_case_ids = frozenset( @@ -916,7 +955,9 @@ def _leaderboard_rows( "algorithm_role": info.algorithm_role.value, "model_revision": info.model_revision, "planner_config_hash": info.config_hash[:12], - "eligible": coverage >= 1.0 - 1.0e-12, + "eligible": bool(track_cases) and coverage >= 1.0 - 1.0e-12, + "attributed_cases": len(track_cases), + "bypassed_cases": len(all_track_cases) - len(track_cases), "coverage_rate": coverage, "overall_success_rate": primary_rate, "planning_success_rate": planning_rate, diff --git a/scripts/benchmark/motion_generation/config.py b/scripts/benchmark/motion_generation/config.py index d2888ae81..b5a1a7528 100644 --- a/scripts/benchmark/motion_generation/config.py +++ b/scripts/benchmark/motion_generation/config.py @@ -130,7 +130,7 @@ class ProtocolCfg: rotation_threshold_rad: float = 0.1 joint_limit_tolerance_rad: float = 1.0e-5 confidence_level: float = 0.95 - report_schema_version: str = "1" + report_schema_version: str = "2" seeds: list[int] = [] diff --git a/scripts/benchmark/motion_generation/generalization.py b/scripts/benchmark/motion_generation/generalization.py index ff20904eb..9a2cc5aeb 100644 --- a/scripts/benchmark/motion_generation/generalization.py +++ b/scripts/benchmark/motion_generation/generalization.py @@ -162,9 +162,11 @@ def aggregate_generalization( """Aggregate versioned nominal, robustness, and held-out populations. Unsupported capability cases lower coverage and are excluded from the - success denominator. Runtime errors and ordinary planner failures remain - applicable cases and therefore count as failures. The returned rows are - sufficient to recompute the domain summary from raw trial records. + success denominator. Cases that bypass the selected planner are excluded + from planner attribution entirely. Runtime errors and ordinary planner + failures remain applicable cases and therefore count as failures. The + returned rows are sufficient to recompute the domain summary from raw + trial records. Args: records: Raw lifecycle and measured trial records. @@ -201,17 +203,31 @@ def aggregate_generalization( for planner in metadata_list: for population_key, population_cases in sorted(cases_by_population.items()): track, domain_id, domain_version, domain_kind = population_key + bypassed = { + case.case_id + for case in population_cases + if any( + not record.uses_selected_planner_backend + for record in by_algorithm_case[ + (planner.algorithm_id, case.case_id) + ] + ) + } + attributed_cases = [ + case for case in population_cases if case.case_id not in bypassed + ] population_records = [ record - for case in population_cases + for case in attributed_cases for record in by_algorithm_case[(planner.algorithm_id, case.case_id)] + if record.uses_selected_planner_backend ] unsupported = _unsupported_case_ids(population_records) applicable_cases = [ - case for case in population_cases if case.case_id not in unsupported + case for case in attributed_cases if case.case_id not in unsupported ] expected_rows = ( - sum(case.batch_size for case in population_cases) * measured_trials + sum(case.batch_size for case in attributed_cases) * measured_trials ) observed_rows = sum( len(record.outcomes) @@ -244,7 +260,9 @@ def aggregate_generalization( "domain_version": domain_version, "domain_kind": domain_kind, "cases": len(population_cases), + "attributed_cases": len(attributed_cases), "applicable_cases": len(applicable_cases), + "bypassed_cases": len(bypassed), "unsupported_cases": len(unsupported), "coverage_rate": min(1.0, observed_rows / max(expected_rows, 1)), "success_rate": success_rate, diff --git a/scripts/benchmark/motion_generation/models.py b/scripts/benchmark/motion_generation/models.py index 1146cef1e..bf3e3ff35 100644 --- a/scripts/benchmark/motion_generation/models.py +++ b/scripts/benchmark/motion_generation/models.py @@ -262,6 +262,15 @@ class TrialRecord: track_group: str | None = None embodiment_id: str | None = None + @property + def uses_selected_planner_backend(self) -> bool: + """Return whether this trial exercised its attributed planner backend. + + Older artifacts predate explicit motion attribution, so a missing flag + retains their original planner-attributed behavior. + """ + return self.metadata.get("selected_planner_backend_used") is not False + def to_dict(self) -> dict[str, object]: """Return a JSON-serializable mapping while retaining numeric values.""" payload = cast(dict[str, object], _json_value(asdict(self))) diff --git a/scripts/benchmark/motion_generation/reporting.py b/scripts/benchmark/motion_generation/reporting.py index c46fe3729..14fbb131a 100644 --- a/scripts/benchmark/motion_generation/reporting.py +++ b/scripts/benchmark/motion_generation/reporting.py @@ -111,6 +111,8 @@ "model_revision", "planner_config_hash", "eligible", + "attributed_cases", + "bypassed_cases", "coverage_rate", "overall_success_rate", "planning_success_rate", @@ -170,6 +172,7 @@ def write_markdown_report( "", f"- suite: `{suite.name}`", f"- suite_version: `{suite.suite_version}`", + f"- report_schema_version: `{suite.protocol.report_schema_version}`", f"- profile: `{suite.profile}`", ( f"- external position threshold: `{suite.protocol.position_threshold_m} m` " @@ -205,7 +208,9 @@ def write_markdown_report( "", "Domain statistics are computed from the raw trial records. " "Unsupported capability cases lower coverage and are excluded " - "from success denominators.", + "from success denominators. Cases that bypassed the selected " + "planner backend are reported separately and excluded from " + "planner attribution.", "", ] ) @@ -220,6 +225,9 @@ def write_markdown_report( "domain_id", "domain_version", "domain_kind", + "cases", + "attributed_cases", + "bypassed_cases", "coverage_rate", "success_rate", "spatial_bucket_success_rates", diff --git a/scripts/benchmark/motion_generation/runner.py b/scripts/benchmark/motion_generation/runner.py index ab5cc0ca3..0931bafee 100644 --- a/scripts/benchmark/motion_generation/runner.py +++ b/scripts/benchmark/motion_generation/runner.py @@ -198,6 +198,7 @@ def _record_unavailable( reason: str, *, failure_code: str, + record_metadata: dict[str, object] | None = None, ) -> None: """Record an unsupported/unavailable planner without counting a failure.""" self._append( @@ -207,6 +208,7 @@ def _record_unavailable( status="unsupported", failure_code=failure_code, failure_message=reason, + metadata={} if record_metadata is None else dict(record_metadata), ), ) note = f"{metadata.algorithm_id} skipped for B={case.batch_size}: {reason}" @@ -220,12 +222,16 @@ def _record_timed_lifecycle( case: BenchmarkCase, phase: TrialPhase, callable_fn: "Callable[[], object]", + *, + record_metadata: dict[str, object] | None = None, ) -> tuple[object | None, Exception | None]: """Measure a construct/prepare operation and persist its outcome.""" measured = timed_call(lambda: _capture(callable_fn)) result, error = measured.result status = "error" if error is not None else "ok" - phase_metadata = result if isinstance(result, dict) else {} + phase_metadata = dict(result) if isinstance(result, dict) else {} + if error is not None and record_metadata is not None: + phase_metadata.update(record_metadata) self._append( writer, TrialRecord( @@ -247,6 +253,30 @@ def _record_timed_lifecycle( ) return result, error + def _record_lifecycle_failures( + self, + writer: TrialJsonlWriter, + metadata: PlannerMetadata, + cases: list[BenchmarkCase], + phase: TrialPhase, + *, + failure_code: str, + failure_message: str, + record_metadata: dict[str, dict[str, object]], + ) -> None: + """Replicate a shared lifecycle failure across every affected case.""" + for case in cases: + self._append( + writer, + TrialRecord( + **self._base_record(metadata, case, phase), + status="error", + failure_code=failure_code, + failure_message=failure_message, + metadata=dict(record_metadata[case.case_id]), + ), + ) + def _run_plan_call( self, writer: TrialJsonlWriter, @@ -260,6 +290,7 @@ def _run_plan_call( repeat: int, ) -> None: """Time one plan, validate outside timing, and persist the record.""" + record_metadata = dict(provider.trial_metadata(case)) provider.reset_case(sim, robot, case, self.control_part) measured = timed_call( lambda: _capture(lambda: provider.plan_case(adapter, case)) @@ -285,7 +316,7 @@ def _run_plan_call( outcomes = () else: try: - evaluation = provider.evaluate_case( + candidate_evaluation = provider.evaluate_case( result, case, robot, @@ -293,6 +324,18 @@ def _run_plan_call( self.suite, planning_time_ms=measured.cost_time_ms, ) + conflicts = { + key: (record_metadata[key], value) + for key, value in candidate_evaluation.metadata.items() + if key in record_metadata and record_metadata[key] != value + } + if conflicts: + raise ValueError( + "Scenario evaluation changed pre-plan trial metadata: " + f"{conflicts}." + ) + record_metadata.update(candidate_evaluation.metadata) + evaluation = candidate_evaluation outcomes = evaluation.outcomes except Exception as exc: # noqa: BLE001 - metric failure is recorded status = "error" @@ -300,7 +343,6 @@ def _run_plan_call( failure_message = str(exc) outcomes = provider.failure_outcomes(case, failure_code) - record_metadata = {} if evaluation is None else dict(evaluation.metadata) video_path = self._maybe_record_replay( provider, result, case, evaluation, metadata.algorithm_id, phase ) @@ -418,6 +460,7 @@ def _run_adapter( "missing required capabilities: " + ", ".join(missing_case_capabilities), failure_code="unsupported_capability", + record_metadata=provider.trial_metadata(case), ) continue motion_capability, modality_reason = _case_motion_capability(case) @@ -432,6 +475,7 @@ def _run_adapter( modality_reason or f"case requires planner capability {motion_capability!r}", failure_code="unsupported_capability", + record_metadata=provider.trial_metadata(case), ) continue supported, reason = adapter.supports_case(case) @@ -444,19 +488,26 @@ def _run_adapter( case, reason or "case is outside planner capacity", failure_code="unsupported_capacity", + record_metadata=provider.trial_metadata(case), ) if not supported_cases: return first_case = supported_cases[0] + lifecycle_metadata = { + case.case_id: dict(provider.trial_metadata(case)) + for case in supported_cases + } available, reason = adapter.availability() if not available: - self._record_unavailable( - writer, - metadata, - first_case, - reason or "runtime unavailable", - failure_code="runtime_unavailable", - ) + for case in supported_cases: + self._record_unavailable( + writer, + metadata, + case, + reason or "runtime unavailable", + failure_code="runtime_unavailable", + record_metadata=lifecycle_metadata[case.case_id], + ) return _, build_error = self._record_timed_lifecycle( @@ -465,8 +516,18 @@ def _run_adapter( first_case, TrialPhase.CONSTRUCT, adapter.build, + record_metadata=lifecycle_metadata[first_case.case_id], ) if build_error is not None: + self._record_lifecycle_failures( + writer, + metadata, + supported_cases[1:], + TrialPhase.CONSTRUCT, + failure_code="planner_exception", + failure_message=str(build_error), + record_metadata=lifecycle_metadata, + ) adapter.close() return scenario_prepared = False @@ -478,22 +539,32 @@ def _run_adapter( first_case, TrialPhase.PREPARE, lambda: adapter.prepare_cases(supported_cases), + record_metadata=lifecycle_metadata[first_case.case_id], ) if prepare_error is not None: + self._record_lifecycle_failures( + writer, + metadata, + supported_cases[1:], + TrialPhase.PREPARE, + failure_code="planner_exception", + failure_message=str(prepare_error), + record_metadata=lifecycle_metadata, + ) return try: provider.prepare_planner(adapter, first_case) scenario_prepared = True except Exception as exc: # noqa: BLE001 - recorded benchmark failure - self._append( + self._record_lifecycle_failures( writer, - TrialRecord( - **self._base_record(metadata, first_case, TrialPhase.PREPARE), - status="error", - failure_code="scenario_prepare_error", - failure_message=str(exc), - ), + metadata, + supported_cases, + TrialPhase.PREPARE, + failure_code="scenario_prepare_error", + failure_message=str(exc), + record_metadata=lifecycle_metadata, ) self.notes.append( f"{metadata.algorithm_id} scenario prepare failed for " @@ -657,7 +728,13 @@ def run(self) -> BenchmarkRunResult: self.cases, self.suite.protocol.measured_trials, ) - write_json(run_dir / "aggregates.json", aggregates) + write_json( + run_dir / "aggregates.json", + { + "report_schema_version": self.suite.protocol.report_schema_version, + **aggregates, + }, + ) enabled_scenarios = {track.scenario for track in enabled_tracks} track_notes: list[str] = [] if "free_space" in enabled_scenarios: diff --git a/scripts/benchmark/motion_generation/scenarios/atomic_skill_providers.py b/scripts/benchmark/motion_generation/scenarios/atomic_skill_providers.py index 0160943e7..915c474e9 100644 --- a/scripts/benchmark/motion_generation/scenarios/atomic_skill_providers.py +++ b/scripts/benchmark/motion_generation/scenarios/atomic_skill_providers.py @@ -63,6 +63,7 @@ _TOP_DOWN_ROTATION, _ExecutionObservation, _atomic_case_id, + _benchmark_motion_policy, _canonical_case_qpos, _case_name, _case_pose, @@ -855,9 +856,8 @@ def build_invocation( open_fraction=float(case.case_parameters["open_fraction"]), ), control_parts=_primary_control_parts(scenario, "OpenDoor"), - motion_policy=MotionPolicy( - strategy="motion_gen", - sample_count=int(case.case_parameters["sample_count"]), + motion_policy=_benchmark_motion_policy( + self.skill_id, int(case.case_parameters["sample_count"]) ), skill_options=OpenDoorOptions( hand_interp_steps=int(case.case_parameters["hand_interp_steps"]), @@ -1928,9 +1928,8 @@ def build_invocation( target_pose=_case_pose(case, "target_object_pose", device=device), ), control_parts=_primary_control_parts(scenario, "PushObject"), - motion_policy=MotionPolicy( - strategy="motion_gen", - sample_count=int(parameters["sample_count"]), + motion_policy=_benchmark_motion_policy( + self.skill_id, int(parameters["sample_count"]) ), skill_options=PushObjectOptions( hand_interp_steps=int(parameters["hand_interp_steps"]), diff --git a/scripts/benchmark/motion_generation/scenarios/atomic_task.py b/scripts/benchmark/motion_generation/scenarios/atomic_task.py index ff7874630..9bcda2a91 100644 --- a/scripts/benchmark/motion_generation/scenarios/atomic_task.py +++ b/scripts/benchmark/motion_generation/scenarios/atomic_task.py @@ -132,6 +132,41 @@ "coordinated_placement", } _CASE_QPOS_RESOLUTION_RAD = 1.0e-4 +_EXACT_CARTESIAN_SKILLS = frozenset( + {"open_door", "press", "push_object", "slide", "twist"} +) + + +def _benchmark_motion_policy_strategy( + skill_id: str, +) -> Literal["motion_gen", "ik_interp"]: + """Return the auditable motion strategy required by one benchmark skill.""" + if skill_id in _EXACT_CARTESIAN_SKILLS: + return "ik_interp" + return "motion_gen" + + +def _benchmark_motion_policy(skill_id: str, sample_count: int) -> MotionPolicy: + """Build the benchmark policy without hiding exact-path planner bypasses.""" + return MotionPolicy( + strategy=_benchmark_motion_policy_strategy(skill_id), + sample_count=sample_count, + ) + + +def _benchmark_motion_attribution(skill_id: str) -> dict[str, object]: + """Describe whether the selected planner backend produced the motion.""" + strategy = _benchmark_motion_policy_strategy(skill_id) + selected_backend_used = strategy == "motion_gen" + return { + "motion_policy_strategy": strategy, + "selected_planner_backend_used": selected_backend_used, + "effective_motion_algorithm": ( + "selected_planner_backend" + if selected_backend_used + else "sequential_ik_interpolation" + ), + } @dataclass(frozen=True) @@ -3624,9 +3659,8 @@ def build_invocation( "grasp": scenario.end_effector_part, } }, - motion_policy=MotionPolicy( - strategy="motion_gen", - sample_count=int(case.case_parameters["sample_count"]), + motion_policy=_benchmark_motion_policy( + self.skill_id, int(case.case_parameters["sample_count"]) ), skill_options=PressOptions( hand_interp_steps=int(case.case_parameters["hand_interp_steps"]), @@ -3901,9 +3935,8 @@ def build_invocation( "grasp": scenario.end_effector_part, } }, - motion_policy=MotionPolicy( - strategy="motion_gen", - sample_count=int(case.case_parameters["sample_count"]), + motion_policy=_benchmark_motion_policy( + self.skill_id, int(case.case_parameters["sample_count"]) ), skill_options=SlideOptions( direction=_slide_direction(case.case_parameters["direction"]), @@ -4168,9 +4201,8 @@ def build_invocation( "grasp": scenario.end_effector_part, } }, - motion_policy=MotionPolicy( - strategy="motion_gen", - sample_count=int(case.case_parameters["sample_count"]), + motion_policy=_benchmark_motion_policy( + self.skill_id, int(case.case_parameters["sample_count"]) ), skill_options=TwistOptions( hand_interp_steps=int(case.case_parameters["hand_interp_steps"]), @@ -4263,6 +4295,10 @@ def batch_sizes(self, suite: SuiteCfg, track: TrackCfg) -> list[int]: del suite return resolve_atomic_batch_sizes(track) + def trial_metadata(self, case: BenchmarkCase) -> dict[str, object]: + """Expose planner attribution before planning or evaluation can fail.""" + return _benchmark_motion_attribution(case.skill_id) + def create_runtime_entities( self, simulation: SimulationManager, @@ -4808,9 +4844,11 @@ def plan_case(self, adapter: PlannerAdapter, case: BenchmarkCase) -> object: provider = self._case_providers[case.case_id] invocation = provider.build_invocation(self, case, adapter) policy = invocation.motion_policy - if policy.strategy != "motion_gen": + expected_strategy = _benchmark_motion_policy_strategy(case.skill_id) + if policy.strategy != expected_strategy: raise RuntimeError( - "Atomic Task invocations must use the motion_gen strategy." + f"Atomic Task skill {case.skill_id!r} requires strategy " + f"{expected_strategy!r}, got {policy.strategy!r}." ) if engine.motion_generator is not adapter.require_motion_generator(): raise RuntimeError( @@ -5121,11 +5159,14 @@ def evaluate_case( trajectory_waypoints=result.trajectory.waypoint_count, metadata={ "timing_scope": "atomic_action_compile", - "motion_policy_strategy": "motion_gen", + **_benchmark_motion_attribution(case.skill_id), "physics_validation": "common_joint_target_replay", "constraint_information": ( - "empty external world; manipulated target excluded from " - "cuRobo collision obstacles" + "exact Cartesian samples through sequential IK; selected " + "planner backend bypassed" + if _benchmark_motion_policy_strategy(case.skill_id) == "ik_interp" + else "empty external world; manipulated target excluded from " + "planner collision obstacles" ), }, ) diff --git a/scripts/benchmark/motion_generation/scenarios/base.py b/scripts/benchmark/motion_generation/scenarios/base.py index b9210e345..982587376 100644 --- a/scripts/benchmark/motion_generation/scenarios/base.py +++ b/scripts/benchmark/motion_generation/scenarios/base.py @@ -101,6 +101,11 @@ def prepare_planner( def close_planner(self, adapter: "PlannerAdapter") -> None: """Release scenario resources that retain a planner adapter.""" + def trial_metadata(self, case: "BenchmarkCase") -> dict[str, object]: + """Return scenario attribution known before a planning call.""" + del case + return {} + def reset_case( self, simulation: "SimulationManager", diff --git a/tests/benchmark/motion_generation/test_atomic_task_benchmark.py b/tests/benchmark/motion_generation/test_atomic_task_benchmark.py index a557fa713..f8c2882d3 100644 --- a/tests/benchmark/motion_generation/test_atomic_task_benchmark.py +++ b/tests/benchmark/motion_generation/test_atomic_task_benchmark.py @@ -29,11 +29,17 @@ import pytest import torch -from embodichain.lab.sim.atomic_actions import ArticulationAffordanceGeometry +from embodichain.lab.sim.atomic_actions import ( + ArticulationAffordanceGeometry, + MotionPolicy, +) from embodichain.lab.sim.atomic_actions.plans import CompiledTrajectory, TimedTrajectory from embodichain.utils.math import axis_angle_to_rotation_matrix from scripts.benchmark.motion_generation.aggregation import aggregate_results -from scripts.benchmark.motion_generation.artifacts import write_case_manifest +from scripts.benchmark.motion_generation.artifacts import ( + TrialJsonlWriter, + write_case_manifest, +) from scripts.benchmark.motion_generation.config import load_suite from scripts.benchmark.motion_generation.contracts import ReplayObservationTrace from scripts.benchmark.motion_generation.models import ( @@ -56,6 +62,8 @@ _ExecutionObservation, _atomic_case_id, _articulation_effect_success, + _benchmark_motion_attribution, + _benchmark_motion_policy_strategy, _canonical_case_qpos, _case_generation_seed, _randomization_parameters, @@ -1130,6 +1138,68 @@ def test_atomic_invocation_uses_motion_generator_policy(): assert kwargs["control_parts"] == {"primary": {"motion": "manipulator"}} +@pytest.mark.parametrize( + "skill_id", ["open_door", "press", "push_object", "slide", "twist"] +) +def test_contact_benchmarks_use_attributed_ik_interpolation(skill_id: str) -> None: + assert _benchmark_motion_policy_strategy(skill_id) == "ik_interp" + expected = { + "motion_policy_strategy": "ik_interp", + "selected_planner_backend_used": False, + "effective_motion_algorithm": "sequential_ik_interpolation", + } + assert _benchmark_motion_attribution(skill_id) == expected + assert ( + AtomicTaskScenario().trial_metadata(replace(_atomic_case(), skill_id=skill_id)) + == expected + ) + + +def test_noncontact_benchmarks_retain_selected_planner_attribution() -> None: + assert _benchmark_motion_policy_strategy("move_end_effector") == "motion_gen" + assert _benchmark_motion_attribution("move_end_effector") == { + "motion_policy_strategy": "motion_gen", + "selected_planner_backend_used": True, + "effective_motion_algorithm": "selected_planner_backend", + } + + +@pytest.mark.parametrize( + "skill_id", ["open_door", "press", "push_object", "slide", "twist"] +) +def test_atomic_scenario_accepts_required_contact_strategy(skill_id: str) -> None: + case = replace( + _atomic_case(), + case_id=f"atomic-task:{skill_id}:simple:s11", + scenario_id=skill_id, + skill_id=skill_id, + ) + motion_generator = Mock() + adapter = Mock() + adapter.require_motion_generator.return_value = motion_generator + adapter.atomic_plan_options.return_value = None + invocation = SimpleNamespace( + motion_policy=MotionPolicy(strategy="ik_interp", sample_count=80) + ) + provider = Mock(samples_grasps=False) + provider.build_invocation.return_value = invocation + provider.initial_task_state.return_value = None + provider.prefix_invocations.return_value = () + provider.initial_scene.return_value = None + provider.frozen_grasps.return_value = {} + compiled = object() + engine = Mock(motion_generator=motion_generator) + engine.compile.return_value = compiled + scenario = AtomicTaskScenario() + scenario.simulation = SimpleNamespace(sim_config=SimpleNamespace(physics_dt=0.01)) + scenario.robot = SimpleNamespace(device=torch.device("cpu")) + scenario._engine = engine + scenario._case_providers[case.case_id] = provider + + assert scenario.plan_case(adapter, case) is compiled + engine.compile.assert_called_once() + + def test_atomic_skill_and_object_extensions_are_registry_driven(): assert atomic_skill_provider_names() == ( "axis_align", @@ -1743,6 +1813,136 @@ def test_atomic_primary_success_and_execution_efficiency_aggregate(): assert leaderboard["task_success_rate"] == pytest.approx(1.0) +def test_planner_aggregates_exclude_atomic_cases_that_bypass_backend(): + selected_case = _atomic_case() + bypassed_case = replace( + selected_case, + scenario_id="press", + case_id="atomic-task:press:simple:s11", + skill_id="press", + ) + metadata = [ + PlannerMetadata( + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + adapter="curobo", + config_hash="abc", + capabilities=frozenset({"eef_waypoint", "atomic_action"}), + supported_robots=("franka_pgi",), + ) + ] + selected_record = TrialRecord( + suite_version=selected_case.suite_version, + track=selected_case.track, + scenario_id=selected_case.scenario_id, + case_id=selected_case.case_id, + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + model_revision="curobo-v2", + planner_config_hash="abc", + seed=selected_case.seed, + repeat=0, + batch_size=selected_case.batch_size, + waypoint_count=selected_case.num_waypoints, + path_shape=selected_case.path_shape, + start_state_bin=selected_case.start_state_bin, + phase=TrialPhase.MEASURED, + cost_time_ms=20.0, + robot_id=selected_case.robot_id, + skill_id=selected_case.skill_id, + task_difficulty=selected_case.task_difficulty, + primary_success=selected_case.primary_success, + metadata=_benchmark_motion_attribution(selected_case.skill_id), + outcomes=(replace(_atomic_outcome(), task_success=False),), + ) + bypassed_record = replace( + selected_record, + scenario_id=bypassed_case.scenario_id, + case_id=bypassed_case.case_id, + skill_id=bypassed_case.skill_id, + cost_time_ms=1.0, + metadata=_benchmark_motion_attribution(bypassed_case.skill_id), + outcomes=(_atomic_outcome(),), + ) + + aggregates = aggregate_results( + [selected_record, bypassed_record], + metadata, + [selected_case, bypassed_case], + measured_trials=1, + ) + + performance = aggregates["time_and_memory"] + metrics = aggregates["success_and_metrics"] + leaderboard = aggregates["leaderboard"][0] + assert [row["skill"] for row in performance] == ["move_end_effector"] + assert [row["skill"] for row in metrics] == ["move_end_effector"] + assert leaderboard["attributed_cases"] == 1 + assert leaderboard["bypassed_cases"] == 1 + assert leaderboard["coverage_rate"] == pytest.approx(1.0) + assert leaderboard["overall_success_rate"] == pytest.approx(0.0) + assert leaderboard["latency_p95_ms"] == pytest.approx(20.0) + + bypass_only = aggregate_results( + [bypassed_record], metadata, [bypassed_case], measured_trials=1 + ) + bypass_leaderboard = bypass_only["leaderboard"][0] + assert bypass_only["success_and_metrics"] == [] + assert bypass_leaderboard["eligible"] is False + assert bypass_leaderboard["attributed_cases"] == 0 + assert bypass_leaderboard["bypassed_cases"] == 1 + assert bypass_leaderboard["coverage_rate"] == pytest.approx(0.0) + assert bypass_leaderboard["overall_success_rate"] == pytest.approx(0.0) + assert bypass_leaderboard["latency_p95_ms"] is None + + +def test_runner_preserves_preplan_attribution_after_planner_error(tmp_path): + case = replace( + _atomic_case(), + scenario_id="press", + case_id="atomic-task:press:simple:s11", + skill_id="press", + ) + provider = Mock(spec=ScenarioProvider) + provider.trial_metadata.return_value = _benchmark_motion_attribution(case.skill_id) + provider.plan_case.side_effect = RuntimeError("planner failed") + provider.failure_outcomes.return_value = () + runner = object.__new__(BenchmarkRunner) + runner.control_part = "arm" + runner.suite = Mock() + runner.video = VideoRecordCfg() + runner.records = [] + runner._run_dir = None + runner._video_paths = [] + planner_metadata = PlannerMetadata( + algorithm_id="curobo", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + adapter="curobo", + config_hash="abc", + capabilities=frozenset({"eef_waypoint", "atomic_action"}), + ) + + runner._run_plan_call( + TrialJsonlWriter(tmp_path / "trials.jsonl"), + Mock(), + Mock(), + Mock(), + planner_metadata, + case, + provider, + TrialPhase.MEASURED, + repeat=0, + ) + + record = runner.records[0] + assert record.status == "error" + assert record.failure_code == "planner_exception" + assert record.metadata["selected_planner_backend_used"] is False + assert ( + record.metadata["effective_motion_algorithm"] == "sequential_ik_interpolation" + ) + + def test_atomic_case_manifest_retains_robot_skill_object_and_parameters(tmp_path): case = _atomic_case() path = write_case_manifest(tmp_path / "case_manifest.json", [case]) @@ -2085,6 +2285,13 @@ def run_adapter(writer, sim, robot, spec, cases, capabilities, scenario): assert manifests[0] == manifests[1] assert all(record.domain.version == "v1" for record in result.records) assert provider.generate_cases.return_value[0].domain is None + aggregate_payload = json.loads( + (result.run_dir / "aggregates.json").read_text(encoding="utf-8") + ) + assert aggregate_payload["report_schema_version"] == "2" + assert "- report_schema_version: `2`" in result.report_path.read_text( + encoding="utf-8" + ) def test_case_snapshot_uses_one_read_and_runtime_joint_order() -> None: diff --git a/tests/benchmark/motion_generation/test_generalization.py b/tests/benchmark/motion_generation/test_generalization.py index ca75ef837..223b3d340 100644 --- a/tests/benchmark/motion_generation/test_generalization.py +++ b/tests/benchmark/motion_generation/test_generalization.py @@ -227,6 +227,57 @@ def test_generalization_reports_row_level_spatial_bucket_rates() -> None: assert row["spatial_bucket_failure_stages"] == {"mid": {"physical_release": 1}} +def test_generalization_excludes_cases_that_bypass_selected_planner() -> None: + domain = EvaluationDomain(id="objects", version="v1", kind="held_out") + attributed_case = replace(_case("planner-case", domain), track="pick@objects") + bypassed_case = replace(attributed_case, case_id="ik-interpolation-case", seed=23) + attributed_record = _record(attributed_case, _outcome(False)) + bypassed_record = replace( + _record(bypassed_case, _outcome(True)), + metadata={ + "selected_planner_backend_used": False, + "effective_motion_algorithm": "sequential_ik_interpolation", + }, + ) + metadata = [ + PlannerMetadata( + algorithm_id="baseline", + algorithm_role=AlgorithmRole.PRIMARY_BASELINE, + adapter="ik_interpolate", + config_hash="abc", + capabilities=frozenset({"eef_waypoint"}), + ) + ] + + rows = aggregate_generalization( + [attributed_record, bypassed_record], + metadata, + [attributed_case, bypassed_case], + measured_trials=1, + ) + + assert len(rows) == 1 + row = rows[0] + assert row["cases"] == 2 + assert row["attributed_cases"] == 1 + assert row["applicable_cases"] == 1 + assert row["bypassed_cases"] == 1 + assert row["coverage_rate"] == pytest.approx(1.0) + assert row["success_rate"] == pytest.approx(0.0) + bypassed_rows = aggregate_generalization( + [bypassed_record], metadata, [bypassed_case], measured_trials=1 + ) + assert len(bypassed_rows) == 1 + bypassed_row = bypassed_rows[0] + assert bypassed_row["cases"] == 1 + assert bypassed_row["attributed_cases"] == 0 + assert bypassed_row["applicable_cases"] == 0 + assert bypassed_row["bypassed_cases"] == 1 + assert bypassed_row["coverage_rate"] == pytest.approx(0.0) + assert bypassed_row["success_rate"] is None + assert bypassed_row["generalization_eligible"] is False + + def test_spatial_bucket_rates_count_missing_repeats_as_failures() -> None: """Missing expected rows remain in every bucket's success denominator.""" domain = EvaluationDomain(id="workspace_shell", version="v1", kind="robustness") diff --git a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py index 6dd35af80..250dc70ec 100644 --- a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py +++ b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py @@ -1056,7 +1056,7 @@ def test_atomic_task_press_invocation_uses_articulated_target_manifest(): assert isinstance(invocation.goal, PressGoal) assert invocation.goal.semantics.entity_id == "benchmark_microwave:button_cap" - assert invocation.motion_policy.strategy == "motion_gen" + assert invocation.motion_policy.strategy == "ik_interp" assert invocation.motion_policy.sample_count == 140 assert type(invocation.skill_options) is PressOptions assert invocation.skill_options.hand_interp_steps == 12 @@ -1133,7 +1133,7 @@ def test_atomic_task_slide_invocation_uses_tutorial_options(): assert isinstance(invocation.goal, SlideGoal) assert invocation.goal.semantics.entity_id == "benchmark_drawer:large_handle_bar" - assert invocation.motion_policy.strategy == "motion_gen" + assert invocation.motion_policy.strategy == "ik_interp" assert invocation.motion_policy.sample_count == 140 assert type(invocation.skill_options) is SlideOptions assert invocation.skill_options.direction == "pull" @@ -1210,7 +1210,7 @@ def test_atomic_task_twist_invocation_uses_tutorial_options(): assert isinstance(invocation.goal, TwistGoal) assert invocation.goal.semantics.entity_id == "benchmark_microwave:cap_1" - assert invocation.motion_policy.strategy == "motion_gen" + assert invocation.motion_policy.strategy == "ik_interp" assert invocation.motion_policy.sample_count == 140 assert type(invocation.skill_options) is TwistOptions assert invocation.skill_options.hand_interp_steps == 12 @@ -1971,6 +1971,7 @@ def test_report_contains_exactly_three_markdown_tables(tmp_path): assert text.count("## Time & Memory") == 1 assert text.count("## Success & Other Metrics") == 1 assert text.count("## Leaderboard") == 1 + assert "- report_schema_version: `2`" in text assert "motion-validity gate" in text assert "n_valid" in text assert 'spatial_bucket_success_rates={"far":0.0,"near":1.0}' in text @@ -2327,6 +2328,122 @@ def plan(self, case: BenchmarkCase): # noqa: ARG002 unregister_planner_adapter(name) +@pytest.mark.parametrize( + ("failure_mode", "failure_phase", "failure_code"), + [ + ("availability", TrialPhase.AVAILABILITY, "runtime_unavailable"), + ("build", TrialPhase.CONSTRUCT, "planner_exception"), + ("prepare", TrialPhase.PREPARE, "planner_exception"), + ("scenario", TrialPhase.PREPARE, "scenario_prepare_error"), + ], +) +def test_runner_preserves_case_attribution_across_lifecycle_failures( + tmp_path, failure_mode, failure_phase, failure_code +): + """Every early exit preserves planner attribution for every case.""" + from scripts.benchmark.motion_generation.artifacts import TrialJsonlWriter + from scripts.benchmark.motion_generation.config import ( + PlannerSpecCfg, + ProtocolCfg, + SuiteCfg, + ) + from scripts.benchmark.motion_generation.planners.base import PlannerAdapter + from scripts.benchmark.motion_generation.registry import ( + register_planner_adapter, + unregister_planner_adapter, + ) + from scripts.benchmark.motion_generation.runner import BenchmarkRunner + + adapter_name = "fake_lifecycle_failure" + + class _LifecycleFailureFake(PlannerAdapter): + capabilities = frozenset({"eef_waypoint"}) + separate_prepare = True + + def availability(self) -> tuple[bool, str | None]: + if self.spec.config["failure_mode"] == "availability": + return False, "runtime missing" + return True, None + + def build(self) -> None: + if self.spec.config["failure_mode"] == "build": + raise RuntimeError("build failed") + + def prepare_cases(self, cases: list[BenchmarkCase]) -> None: + if self.spec.config["failure_mode"] == "prepare": + raise RuntimeError("prepare failed") + + def plan(self, case: BenchmarkCase) -> PlanResult: # noqa: ARG002 + raise AssertionError("lifecycle failure must stop before planning") + + register_planner_adapter(adapter_name, _LifecycleFailureFake) + try: + suite = SuiteCfg( + name="motion_generation", + suite_version="test_lifecycle_v1", + profile="smoke", + protocol=ProtocolCfg(warmup_trials=0, measured_trials=1), + ) + spec = PlannerSpecCfg( + id="candidate", + adapter=adapter_name, + role=AlgorithmRole.CANDIDATE.value, + enabled=True, + config={"failure_mode": failure_mode}, + ) + runner = BenchmarkRunner(suite, [spec], device="cpu", output_root=tmp_path) + attributed = replace( + _case(), suite_version=suite.suite_version, case_id="attributed" + ) + bypassed = replace(attributed, case_id="bypassed", seed=23) + provider = Mock() + provider.trial_metadata.side_effect = lambda case: { + "selected_planner_backend_used": case.case_id == attributed.case_id, + "effective_motion_algorithm": ( + adapter_name + if case.case_id == attributed.case_id + else "sequential_ik_interpolation" + ), + } + provider.prepare_planner.side_effect = ( + RuntimeError("scenario prepare failed") + if failure_mode == "scenario" + else None + ) + writer = TrialJsonlWriter(tmp_path / "trials.jsonl") + + runner._run_adapter( + writer, + Mock(), + Mock(device=torch.device("cpu")), + spec, + [attributed, bypassed], + frozenset({"eef_waypoint"}), + provider, + ) + + failures = [ + record + for record in runner.records + if record.phase is failure_phase and record.failure_code == failure_code + ] + assert {record.case_id for record in failures} == {"attributed", "bypassed"} + assert { + record.case_id: record.metadata["selected_planner_backend_used"] + for record in failures + } == {"attributed": True, "bypassed": False} + leaderboard = aggregate_results( + runner.records, + list(runner.metadata.values()), + [attributed, bypassed], + measured_trials=1, + )["leaderboard"][0] + assert leaderboard["attributed_cases"] == 1 + assert leaderboard["bypassed_cases"] == 1 + finally: + unregister_planner_adapter(adapter_name) + + @pytest.mark.parametrize("full_state", [True, False]) def test_scenario_reset_publishes_frozen_start_without_stepping( full_state: bool, diff --git a/tests/compute/test_trajectory.py b/tests/compute/test_trajectory.py index b5c08a0e3..f584363ee 100644 --- a/tests/compute/test_trajectory.py +++ b/tests/compute/test_trajectory.py @@ -26,6 +26,96 @@ interpolate_with_distance, resample_with_distance, ) +from embodichain.compute.trajectory.interpolation import interpolate_minimum_jerk + + +def test_minimum_jerk_expands_sample_count_and_preserves_limits() -> None: + keyframes = torch.tensor([[[0.0], [1.0], [0.5]]]) + step_dt = 0.05 + + result = interpolate_minimum_jerk( + keyframes, + minimum_sample_count=5, + step_dt=step_dt, + velocity_limit=0.5, + acceleration_limit=1.0, + ) + + assert result.shape[1] > 5 + assert torch.equal(result[:, :1], keyframes[:, :1]) + assert torch.equal(result[:, -1:], keyframes[:, -1:]) + assert any( + torch.equal(result[:, index], keyframes[:, 1]) + for index in range(1, result.shape[1] - 1) + ) + interval_velocity = result.diff(dim=1) / step_dt + interval_acceleration = interval_velocity.diff(dim=1) / step_dt + assert interval_velocity.abs().max() <= 0.5 + 1.0e-5 + assert interval_acceleration.abs().max() <= 1.0 + 1.0e-4 + + +def test_minimum_jerk_uses_sample_count_as_a_lower_bound() -> None: + result = interpolate_minimum_jerk( + torch.tensor([[[0.0], [0.01]]]), + minimum_sample_count=20, + step_dt=0.1, + velocity_limit=1.0, + acceleration_limit=2.0, + ) + + assert result.shape == (1, 20, 1) + + +def test_minimum_jerk_validates_dense_float32_output_derivatives() -> None: + step_dt = 0.001 + velocity_limit = 0.2 + acceleration_limit = 0.5 + + result = interpolate_minimum_jerk( + torch.tensor([[[0.0], [1.0]]], dtype=torch.float32), + minimum_sample_count=3, + step_dt=step_dt, + velocity_limit=velocity_limit, + acceleration_limit=acceleration_limit, + ) + + interval_velocity = result.diff(dim=1) / step_dt + boundary_acceleration = ( + torch.cat( + [ + interval_velocity[:, :1], + interval_velocity.diff(dim=1), + -interval_velocity[:, -1:], + ], + dim=1, + ) + / step_dt + ) + assert torch.all(result.diff(dim=1) >= 0.0) + assert interval_velocity.abs().max() <= velocity_limit * (1.0 + 1.0e-5) + assert boundary_acceleration.abs().max() <= acceleration_limit * (1.0 + 1.0e-5) + + +def test_minimum_jerk_allows_only_stationary_zero_speed_joints() -> None: + keyframes = torch.tensor([[[0.0, 0.0], [0.0, 1.0]]]) + + result = interpolate_minimum_jerk( + keyframes, + minimum_sample_count=3, + step_dt=0.05, + velocity_limit=torch.tensor([0.0, 0.2]), + acceleration_limit=0.5, + ) + + assert torch.equal(result[..., 0], torch.zeros_like(result[..., 0])) + with pytest.raises(ValueError, match="zero-velocity joint"): + interpolate_minimum_jerk( + torch.tensor([[[0.0, 0.0], [0.1, 1.0]]]), + minimum_sample_count=3, + step_dt=0.05, + velocity_limit=torch.tensor([0.0, 0.2]), + acceleration_limit=0.5, + ) def test_interpolate_with_distance_preserves_intermediate_keyframe() -> None: diff --git a/tests/gym/envs/task_program/test_configured_integration.py b/tests/gym/envs/task_program/test_configured_integration.py index 19a073677..96e804d45 100644 --- a/tests/gym/envs/task_program/test_configured_integration.py +++ b/tests/gym/envs/task_program/test_configured_integration.py @@ -1517,6 +1517,83 @@ def test_motion_velocity_target_policy_rejects_unknown_mode(mode: str) -> None: ) +def test_motion_limit_policy_decodes() -> None: + from embodichain.lab.task_program.integrations.configured import ( + _decode_motion_policy, + ) + + policy = _decode_motion_policy( + { + "sample_count": 40, + "velocity_limit": 0.2, + "acceleration_limit": 0.5, + }, + path="motion", + ) + + assert policy.velocity_limit == pytest.approx(0.2) + assert policy.acceleration_limit == pytest.approx(0.5) + + +def test_motion_limit_policy_preserves_omitted_velocity_limit() -> None: + from embodichain.lab.task_program.integrations.configured import ( + _decode_motion_policy, + ) + + policy = _decode_motion_policy( + {"sample_count": 40, "acceleration_limit": 0.5}, + path="motion", + ) + + assert policy.velocity_limit is None + + +@pytest.mark.parametrize("field", ["velocity_limit", "acceleration_limit"]) +@pytest.mark.parametrize("value", [0.0, -0.1, float("inf")]) +def test_motion_limit_policy_rejects_nonpositive_or_nonfinite_values( + field: str, + value: float, +) -> None: + from embodichain.lab.task_program.integrations.configured import ( + _decode_motion_policy, + ) + + with pytest.raises(ValueError, match=field): + _decode_motion_policy( + {"sample_count": 40, field: value}, + path="motion", + ) + + +def test_pour_dwell_duration_decodes() -> None: + from embodichain.lab.task_program.integrations.configured import ( + _decode_action_options, + ) + + options = _decode_action_options( + {"kind": "pour", "dwell_duration": 0.75}, + path="options", + ) + + assert isinstance(options, PourOptions) + assert options.dwell_duration == pytest.approx(0.75) + + +@pytest.mark.parametrize("value", [-0.1, float("inf")]) +def test_pour_dwell_duration_rejects_negative_or_nonfinite_values( + value: float, +) -> None: + from embodichain.lab.task_program.integrations.configured import ( + _decode_action_options, + ) + + with pytest.raises(ValueError, match="dwell_duration"): + _decode_action_options( + {"kind": "pour", "dwell_duration": value}, + path="options", + ) + + @pytest.mark.parametrize( ("task_name", "backend"), [ diff --git a/tests/lab/task_program/test_semantic_executor.py b/tests/lab/task_program/test_semantic_executor.py index 655c10437..b0f49fc3b 100644 --- a/tests/lab/task_program/test_semantic_executor.py +++ b/tests/lab/task_program/test_semantic_executor.py @@ -1986,6 +1986,33 @@ def test_runtime_invokes_step_observer() -> None: assert observed_steps +def test_motion_policy_metadata_includes_timing_limits() -> None: + from embodichain.lab.task_program.runtime.results import ( + _motion_policy_to_metadata, + ) + + metadata = _motion_policy_to_metadata( + MotionPolicy( + sample_count=7, + velocity_limit=0.2, + acceleration_limit=0.5, + ) + ) + + assert metadata["velocity_limit"] == pytest.approx(0.2) + assert metadata["acceleration_limit"] == pytest.approx(0.5) + + +def test_motion_policy_metadata_preserves_omitted_velocity_limit() -> None: + from embodichain.lab.task_program.runtime.results import ( + _motion_policy_to_metadata, + ) + + metadata = _motion_policy_to_metadata(MotionPolicy(sample_count=7)) + + assert metadata["velocity_limit"] is None + + def test_result_metadata_is_json_safe_and_contains_typed_runtime_trace() -> None: system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 9a545a327..29f936f8b 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -134,6 +134,7 @@ HAND_DOF = 2 ROBOT_DOF = ARM_DOF + HAND_DOF CONTROL_DT = 1.0 / 60.0 +TEST_JOINT_VELOCITY_LIMIT = 100.0 DUAL_ARM_DOF = 2 * ARM_DOF DUAL_ROBOT_DOF = DUAL_ARM_DOF + 2 * HAND_DOF DOOR_ENTITY_ID = "door" @@ -279,6 +280,24 @@ def interpolate( "embodichain.lab.sim.motion.motion_generator.interpolate_with_distance", interpolate, ) + monkeypatch.setattr( + "embodichain.lab.sim.motion.motion_generator.interpolate_minimum_jerk", + lambda trajectory, minimum_sample_count, step_dt, velocity_limit, acceleration_limit: interpolate( + trajectory, minimum_sample_count, trajectory.device + ), + ) + monkeypatch.setattr( + MotionGenerator, + "_interpolate_cartesian_with_limits", + lambda self, target_poses, solved_keyframes, options: ( + torch.ones( + solved_keyframes.shape[0], + dtype=torch.bool, + device=solved_keyframes.device, + ), + solved_keyframes, + ), + ) monkeypatch.setattr( "embodichain.lab.sim.atomic_actions.primitives._helpers.resample_with_distance", interpolate, @@ -310,6 +329,13 @@ def get_joint_ids(name: str | None = None) -> list[int]: return list(range(ARM_DOF, ROBOT_DOF)) return list(range(ROBOT_DOF)) + def get_qvel_limits( + name: str | None = None, + joint_ids: list[int] | None = None, + ) -> torch.Tensor: + ids = get_joint_ids(name) if joint_ids is None else joint_ids + return torch.ones(NUM_ENVS, len(ids)) + def compute_ik( pose: torch.Tensor | None = None, name: str | None = None, @@ -338,6 +364,7 @@ def compute_batch_fk( robot.get_qpos.side_effect = get_qpos robot.get_joint_ids.side_effect = get_joint_ids + robot.get_qvel_limits.side_effect = get_qvel_limits robot.compute_ik.side_effect = compute_ik def compute_batch_ik( @@ -402,6 +429,20 @@ def _bind_action( control_profiles: dict[str, ControlPartCommandProfile] | None = None, ) -> ActionT: """Bind one configured action to an engine-owned test backend.""" + if isinstance(action, CoordinatedPickment): + generator.robot.get_qvel_limits.side_effect = ( + lambda name=None, joint_ids=None: torch.full( + ( + NUM_ENVS, + len( + generator.robot.get_joint_ids(name) + if joint_ids is None + else joint_ids + ), + ), + TEST_JOINT_VELOCITY_LIMIT, + ) + ) profiles = { name: ControlPartCommandProfile.joint_positions( open=torch.zeros(len(generator.robot.get_joint_ids(name=name))), @@ -660,16 +701,22 @@ def compute_batch_ik( pose: torch.Tensor, name: str, joint_seed: torch.Tensor, + continuous: bool = False, **_: object, ) -> tuple[torch.Tensor, torch.Tensor]: - results = [ - robot.compute_ik( + results = [] + current_seed = joint_seed + for index in range(pose.shape[1]): + sample_seed = current_seed if continuous else joint_seed[:, index] + result = robot.compute_ik( pose=pose[:, index], name=name, - joint_seed=joint_seed[:, index], + joint_seed=sample_seed, ) - for index in range(pose.shape[1]) - ] + results.append(result) + if continuous: + valid, qpos = result + current_seed = torch.where(valid[:, None], qpos, current_seed) return ( torch.stack([valid for valid, _ in results], dim=1), torch.stack([qpos for _, qpos in results], dim=1), @@ -685,6 +732,10 @@ def compute_fk( robot.get_qpos.side_effect = get_qpos robot.get_joint_ids.side_effect = get_joint_ids + robot.get_qvel_limits.side_effect = lambda name=None, joint_ids=None: torch.ones( + NUM_ENVS, + len(get_joint_ids(name) if joint_ids is None else joint_ids), + ) robot.get_solver.return_value = SimpleNamespace(supports_continuous_batch_ik=False) robot.compute_ik.side_effect = compute_ik robot.compute_batch_ik.side_effect = compute_batch_ik @@ -1552,8 +1603,15 @@ def compute_ik( skill_id="pour", goal=PourGoal(), binding=_binding(action), - motion_policy=MotionPolicy(sample_count=10), - skill_options=PourOptions(rotate_angle=math.pi / 2.0), + motion_policy=MotionPolicy( + sample_count=10, + velocity_limit=100.0, + acceleration_limit=1000.0, + ), + skill_options=PourOptions( + rotate_angle=math.pi / 2.0, + dwell_duration=2.0 / 60.0, + ), ), context, ) @@ -1566,9 +1624,17 @@ def compute_ik( assert plan.plan_success.tolist() == [True, True] assert trajectory.velocities is not None assert torch.count_nonzero(trajectory.velocities[:, :, ARM_DOF:]) == 0 - assert trajectory.positions.shape == (NUM_ENVS, 10, ROBOT_DOF) - assert trajectory.duration.tolist() == pytest.approx([9.0 / 60.0] * NUM_ENVS) - assert [segment.name for segment in plan.segments] == ["pour"] + assert trajectory.positions.shape == (NUM_ENVS, 12, ROBOT_DOF) + assert trajectory.duration.tolist() == pytest.approx([11.0 / 60.0] * NUM_ENVS) + assert [segment.name for segment in plan.segments] == [ + "pour", + "dwell", + "return", + ] + assert torch.equal( + trajectory.positions[:, 4:7, :ARM_DOF], + trajectory.positions[:, 4:5, :ARM_DOF].expand(-1, 3, -1), + ) assert torch.allclose( solved_poses[0][:, :3, :3], expected_rotation.expand(NUM_ENVS, -1, -1), @@ -1727,11 +1793,17 @@ def test_pour_requires_exclusively_held_axis_align_affordance() -> None: ) -def test_pour_options_only_contain_rotate_angle_and_require_finite_value() -> None: - assert set(PourOptions.__dataclass_fields__) == {"rotate_angle"} +def test_pour_options_validate_rotation_and_dwell_duration() -> None: + assert set(PourOptions.__dataclass_fields__) == { + "rotate_angle", + "dwell_duration", + } assert PourOptions().rotate_angle == pytest.approx(math.pi / 4.0) + assert PourOptions().dwell_duration == pytest.approx(0.5) with pytest.raises(ValueError, match="rotate_angle must be finite"): PourOptions(rotate_angle=float("nan")) + with pytest.raises(ValueError, match="dwell_duration"): + PourOptions(dwell_duration=-0.1) def test_move_held_object_moves_only_exclusively_held_rows() -> None: @@ -1775,6 +1847,10 @@ def move_ik( context.robot.qpos[0].unsqueeze(0).expand(trajectory.waypoint_count, -1), ) assert not torch.allclose(trajectory.positions[1], context.robot.qpos[1]) + torch.testing.assert_close( + trajectory.positions[1, :, ARM_DOF:], + context.robot.qpos[1, ARM_DOF:].expand(trajectory.waypoint_count, -1), + ) def test_strategy_and_sample_count_are_not_action_config_fields() -> None: @@ -3371,6 +3447,60 @@ def fail_release(*args, **kwargs): ) +def test_slide_expands_hand_opening_to_match_longer_release_trajectory() -> None: + generator = _motion_generator() + action = _bind_action(generator, Slide()) + _GRASP_GENERATORS[id(action)].get_valid_grasp_poses = Mock( + return_value=[ + (torch.eye(4).unsqueeze(0), torch.zeros(1)) for _ in range(NUM_ENVS) + ] + ) + original_plan = action._plan_pose_segment + + def expand_release(*args, **kwargs): + success, positions = original_plan(*args, **kwargs) + if args[4] == 3: + positions = torch.cat( + [positions, positions[:, -1:].expand(-1, 2, -1)], + dim=1, + ) + return success, positions + + action._plan_pose_segment = expand_release + semantics = ObjectSemantics( + affordance=SlideAffordance( + mesh_vertices=torch.zeros(3, 3), + mesh_triangles=torch.tensor([[0, 1, 2]]), + translation_axis=torch.tensor([0.0, -1.0, 0.0]), + ), + geometry={}, + entity_id="handle", + ) + + plan = _plan_action( + action, + ActionInvocation( + skill_id="slide", + goal=SlideGoal(semantics, torch.eye(4)), + binding=_binding(action), + motion_policy=MotionPolicy(sample_count=24), + skill_options=SlideOptions( + hand_interp_steps=3, + release_retreat_distance=0.04, + ), + ), + _context(), + ) + + trajectory = _joint_trajectory(plan) + open_segment = plan.segment("open") + assert open_segment.stop - open_segment.start == 5 + assert trajectory.positions.shape[1] == 26 + assert torch.all( + trajectory.positions[:, plan.segment("open").stop - 1, ARM_DOF:] == 0.0 + ) + + @pytest.mark.parametrize( "kwargs", [ @@ -4136,6 +4266,115 @@ def test_open_door_holds_failed_grasp_environment() -> None: ) +@pytest.mark.parametrize("action_kind", ["open_door", "twist"]) +def test_exact_contact_path_holds_row_after_partial_ik_failure( + action_kind: str, + monkeypatch: pytest.MonkeyPatch, +) -> None: + generator = _motion_generator() + + def position_ik( + pose: torch.Tensor, + name: str, + joint_seed: torch.Tensor, + **_: object, + ) -> tuple[torch.Tensor, torch.Tensor]: + del name + qpos = joint_seed.clone() + qpos[:, :3] = pose[:, :3, 3] + return torch.tensor([True, False]), qpos + + def position_fk( + qpos: torch.Tensor, + name: str, + to_matrix: bool, + ) -> torch.Tensor: + del name, to_matrix + pose = torch.eye(4).repeat(qpos.shape[0], 1, 1) + pose[:, :3, 3] = qpos[:, :3] + return pose + + def validate_exact_start( + self: MotionGenerator, + target_poses: torch.Tensor, + solved_keyframes: torch.Tensor, + options, + ) -> tuple[torch.Tensor, torch.Tensor]: + del target_poses + if options.cartesian_path_sampler is not None: + sampled = options.cartesian_path_sampler(torch.tensor([0.0, 1.0])) + start_pose = self.robot.compute_fk( + qpos=solved_keyframes[:, 0], + name=options.control_part, + to_matrix=True, + ) + if not torch.allclose(sampled[:, 0], start_pose, atol=1.0e-6): + raise ValueError("cartesian_path_sampler start differs from FK") + return ( + torch.ones(solved_keyframes.shape[0], dtype=torch.bool), + solved_keyframes, + ) + + generator.robot.compute_ik.side_effect = position_ik + generator.robot.compute_fk.side_effect = position_fk + monkeypatch.setattr( + MotionGenerator, + "_interpolate_cartesian_with_limits", + validate_exact_start, + ) + if action_kind == "open_door": + action = _bind_action(generator, OpenDoor()) + grasp_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + grasp_pose[:, 0, 3] = 1.0 + _GRASP_GENERATORS[id(action)].get_valid_grasp_poses = Mock( + return_value=[ + (grasp_pose[index : index + 1], torch.zeros(1)) + for index in range(NUM_ENVS) + ] + ) + semantics = ObjectSemantics( + affordance=_door_affordance(), + geometry={}, + entity_id=DOOR_ENTITY_ID, + ) + goal = OpenDoorGoal(semantics, torch.eye(4), open_fraction=0.5) + options = OpenDoorOptions(hand_interp_steps=3, door_waypoint_count=4) + context = _context(scene=_door_scene()) + else: + action = _bind_action(generator, Twist()) + semantics = ObjectSemantics( + affordance=TwistAffordance( + grasp_position=(0.0, 0.0, 0.0), + axis_origin=(0.0, 0.0, 0.0), + twist_axis=torch.tensor([0.0, 0.0, 1.0]), + ), + geometry={}, + entity_id="knob", + ) + goal = TwistGoal(semantics, torch.eye(4)) + options = TwistOptions(hand_interp_steps=3, twist_waypoint_count=4) + context = _context() + + plan = _plan_action( + action, + ActionInvocation( + skill_id=action.skill_id, + goal=goal, + binding=_binding(action), + motion_policy=MotionPolicy(sample_count=24), + skill_options=options, + ), + context, + ) + + assert plan.plan_success.tolist() == [True, False] + trajectory = _joint_trajectory(plan) + assert torch.allclose( + trajectory.positions[1], + context.robot.qpos[1].unsqueeze(0).expand_as(trajectory.positions[1]), + ) + + def test_open_door_fails_row_whose_target_would_close_hinge() -> None: semantics = ObjectSemantics( affordance=_door_affordance(), @@ -4419,62 +4658,9 @@ def test_press_plans_close_approach_press_and_retract( assert torch.allclose(planned_targets[-1][:, :3, 3], expected_approach) -@pytest.mark.parametrize( - ("failed_second_row", "preserve_failure_evidence"), - ((False, False), (True, False), (True, True)), -) -@pytest.mark.parametrize("slow_backend", (False, True)) -def test_press_motion_gen_plans_four_ordered_goals_in_one_backend_call( - failed_second_row: bool, - preserve_failure_evidence: bool, - slow_backend: bool, -) -> None: - """Use the real facade, with a Cartesian backend double and mixed row failures.""" +def test_press_motion_gen_rejects_uncertified_contact_path() -> None: generator = _motion_generator() - generator.planner.supports_move_type.side_effect = ( - lambda move_type: move_type is MoveType.EEF_MOVE - ) - generator.planner.preserve_plan_samples = True - generator.planner.preserve_failed_plan_positions = preserve_failure_evidence - generator.generate = Mock(wraps=generator.generate) - generator._generate_ik_interpolation = Mock( - side_effect=AssertionError("motion_gen must not fall back to IK interpolation") - ) - native_path = torch.zeros(NUM_ENVS, 7, ARM_DOF) - # Different arrival times and repeated poses require ordered splits. - native_path[0, :, 0] = torch.tensor([0.0, -0.05, -0.1, 0.0, 0.02, 0.0, -0.1]) - native_path[1, :, 0] = torch.tensor([0.0, -0.1, -0.05, 0.0, 0.01, 0.02, -0.1]) - native_path[:, :, 3] = torch.arange(7) * 0.01 - native_dt = torch.full((NUM_ENVS, native_path.shape[1]), CONTROL_DT) - native_dt[:, 0] = 0.0 - if slow_backend: - # Deliberately exceed the fixed phase-count budget with uneven timing. - native_dt = CONTROL_DT * torch.tensor( - [[0, 47, 7, 23, 71, 11, 67], [0, 13, 57, 41, 29, 17, 83]] - ) - - def backend_plan(*, target_states, options) -> PlanResult: - assert len(target_states) == 4 - assert all(state.move_type is MoveType.EEF_MOVE for state in target_states) - - def path_fk(*, qpos, name, to_matrix): - pose = ( - target_states[0].xpos[:, None].expand(-1, qpos.shape[1], -1, -1).clone() - ) - pose[:, :, :3, 3] = qpos[:, :, :3] - return pose - - generator.robot.compute_batch_fk.side_effect = path_fk - success = torch.ones(NUM_ENVS, dtype=torch.bool) - if failed_second_row: - success[1] = False - return PlanResult( - success=success, - positions=native_path.clone(), - dt=native_dt.clone(), - ) - - generator.planner.plan.side_effect = backend_plan + action = _bind_action(generator, Press()) semantics = ObjectSemantics( affordance=PressAffordance( press_axis=torch.tensor([1.0, 0.0, 0.0]), @@ -4484,80 +4670,20 @@ def path_fk(*, qpos, name, to_matrix): label="button", entity_id="button", ) - action = _bind_action(generator, Press()) - context = _context() - initial_qpos = context.robot.qpos.clone() - sample_count = 80 - plan = _plan_action( - action, - ActionInvocation( - skill_id="press", - goal=PressGoal(semantics, torch.eye(4)), - binding=_binding(action), - motion_policy=MotionPolicy( - strategy="motion_gen", sample_count=sample_count - ), - skill_options=PressOptions(approach_distance=0.1, press_distance=0.02), - ), - context, - ) - generator.planner.plan.assert_called_once() - generator.generate.assert_called_once() - generator._generate_ik_interpolation.assert_not_called() - generator.robot.compute_ik.assert_not_called() - assert plan.plan_success.tolist() == [True, not failed_second_row] - options = generator.generate.call_args.kwargs["options"] - assert options.strategy == "motion_gen" - assert options.sample_count is None - assert not options.is_linear and not options.preserve_cartesian_samples - expected_x = (-0.1, 0.0, 0.02, -0.1) - for state, x in zip( - generator.planner.plan.call_args.kwargs["target_states"], expected_x - ): - pose = state.xpos - torch.testing.assert_close( - pose[:, :3, 3], torch.tensor([x, 0.0, 0.0]).expand(NUM_ENVS, -1) - ) - torch.testing.assert_close(options.start_qpos, initial_qpos[:, :ARM_DOF]) - trajectory = _joint_trajectory(plan) - if slow_backend: - assert trajectory.waypoint_count > sample_count - else: - assert trajectory.positions.shape == (NUM_ENVS, sample_count, ROBOT_DOF) - # Closing and phase-junction holds are additional to the backend duration. - close_duration = (PressOptions().hand_interp_steps - 1) * CONTROL_DT - assert torch.all( - trajectory.duration + 1e-6 >= native_dt.sum(dim=1) + close_duration - ) - for row, end_indices in enumerate(((2, 3, 4, 6), (1, 3, 5, 6))): - if row == 1 and failed_second_row and not preserve_failure_evidence: - continue - start_index = 0 - for name, end_index in zip( - ("approach", "contact", "press", "retract"), end_indices - ): - segment = plan.segment(name) - torch.testing.assert_close( - trajectory.positions[row, segment.start, :ARM_DOF], - native_path[row, start_index], - ) - torch.testing.assert_close( - trajectory.positions[row, segment.stop - 1, :ARM_DOF], - native_path[row, end_index], - ) - source_duration = native_dt[row, start_index + 1 : end_index + 1].sum() - phase_duration = trajectory.dt[row, segment.start + 1 : segment.stop].sum() - assert phase_duration + 1e-6 >= source_duration - start_index = end_index - if failed_second_row and not preserve_failure_evidence: - torch.testing.assert_close( - trajectory.positions[1], - initial_qpos[1].expand(trajectory.waypoint_count, -1), + with pytest.raises(ValueError, match="requires strategy='ik_interp'"): + _plan_action( + action, + ActionInvocation( + skill_id="press", + goal=PressGoal(semantics, torch.eye(4)), + binding=_binding(action), + motion_policy=MotionPolicy(strategy="motion_gen", sample_count=80), + ), + _context(), ) - for frame in plan.commands.frames: - assert frame.active_mask.tolist() == [True, not failed_second_row] - torch.testing.assert_close(context.robot.qpos, initial_qpos) + + generator.planner.plan.assert_not_called() def test_press_combined_split_accepts_stationary_single_sample() -> None: @@ -5222,7 +5348,7 @@ def plan_from_start( trajectory = _joint_trajectory(plan) assert plan.plan_success.tolist() == [True, True] - assert trajectory.positions.shape == (NUM_ENVS, 24, DUAL_ROBOT_DOF) + assert trajectory.positions.shape == (NUM_ENVS, 28, DUAL_ROBOT_DOF) assert planned_parts == ["left_arm", "left_arm", "right_arm", "right_arm"] assert [segment.name for segment in plan.segments] == [ "pickup_approach", @@ -5230,6 +5356,7 @@ def plan_from_start( "pickup_transport", "receive_approach", "receive_close", + "receive_hold", "handover_release", "place", "receive_release", @@ -5265,6 +5392,7 @@ def plan_from_start( pickup_close_end = plan.segment("pickup_close").stop - 1 receive_close_end = plan.segment("receive_close").stop - 1 + receive_hold = plan.segment("receive_hold") handover_release_end = plan.segment("handover_release").stop - 1 receive_release_end = plan.segment("receive_release").stop - 1 positions = trajectory.positions @@ -5273,6 +5401,9 @@ def plan_from_start( ) assert torch.all(positions[:, pickup_close_end, DUAL_ARM_DOF + 2 :] == 0) assert torch.all(positions[:, receive_close_end, DUAL_ARM_DOF:] == 1) + assert torch.all( + positions[:, receive_hold.start : receive_hold.stop, DUAL_ARM_DOF:] == 1 + ) assert torch.all( positions[:, handover_release_end, DUAL_ARM_DOF : DUAL_ARM_DOF + 2] == 0 ) @@ -5297,6 +5428,73 @@ def plan_from_start( } +@pytest.mark.parametrize("hold_steps", [0, 4]) +def test_handover_mixed_directions_pad_each_phase_to_shared_lengths( + hold_steps: int, +) -> None: + action = _bind_action(_dual_motion_generator(), HandOver()) + semantics, _ = _handover_semantics() + object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + object_pose[:, 0, 3] = torch.tensor([-0.8, 0.8]) + context = _handover_context(object_pose) + options = HandOverOptions(hand_interp_steps=2, hold_steps=hold_steps) + requested = action._compute_segment_lengths(24, options) + first_lengths = dict(requested) + second_lengths = dict(requested) + first_lengths["pickup_approach"] += 2 + second_lengths["pickup_transport"] += 3 + + def directional_plan( + lengths: dict[str, int], + value_offset: float, + ) -> SimpleNamespace: + parts = [ + torch.full( + (NUM_ENVS, length, DUAL_ROBOT_DOF), + value_offset + index, + ) + for index, length in enumerate(lengths.values()) + ] + pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + return SimpleNamespace( + success=torch.ones(NUM_ENVS, dtype=torch.bool), + trajectory=torch.cat(parts, dim=1), + segment_lengths=lengths, + handover_object_to_eef=pose, + handover_grasp_xpos=pose, + receive_object_to_eef=pose, + receive_grasp_xpos=pose, + affordance_samples={}, + ) + + action._plan_direction = Mock( + side_effect=( + directional_plan(first_lengths, 10.0), + directional_plan(second_lengths, 20.0), + ) + ) + invocation = ActionInvocation( + skill_id="hand_over", + goal=HandOverGoal(semantics, target_pose=torch.eye(4)), + binding=_dual_binding(action, "source", "destination"), + motion_policy=MotionPolicy(sample_count=24), + skill_options=options, + ) + + plan = _plan_action(action, invocation, context) + + shared_lengths = { + name: max(first_lengths[name], second_lengths[name]) for name in requested + } + assert plan.plan_success.tolist() == [True, True] + assert {segment.name: segment.waypoint_count for segment in plan.segments} == { + name: length for name, length in shared_lengths.items() if length + } + assert _joint_trajectory(plan).waypoint_count == sum(shared_lengths.values()) + first_call, second_call = action._plan_direction.call_args_list + assert first_call.args[-2] is not second_call.args[-2] + + def test_handover_can_end_with_receiving_resource_holding_object( monkeypatch: pytest.MonkeyPatch, ) -> None: @@ -5362,6 +5560,7 @@ def plan_from_start( "pickup_transport", "receive_approach", "receive_close", + "receive_hold", "handover_release", "source_retreat", ] @@ -6288,9 +6487,9 @@ def reject_distant_candidate( projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, True] - # Both arms screen approach/grasp/lift plus the configured object-motion - # continuation, rather than jumping directly from lift to the final pose. - assert generator.robot.compute_batch_ik.call_count == 10 + # Both arms screen candidates and then project every lift/move sample back + # onto the shared-object closed-chain constraint. + assert generator.robot.compute_batch_ik.call_count == 14 left_held = projected.get_held_object("left_arm") right_held = projected.get_held_object("right_arm") assert left_held is not None and right_held is not None @@ -6642,6 +6841,74 @@ def fail_second_environment( assert right_held.env_mask.tolist() == [True, False] +def test_coordinated_pick_holds_environment_with_dense_ik_joint_jump() -> None: + generator = _dual_motion_generator() + generator.robot.get_solver.return_value.supports_continuous_batch_ik = True + original_compute_batch_ik = generator.robot.compute_batch_ik.side_effect + excessive_joint_step = 2.0 + + def inject_dense_joint_jump( + pose: torch.Tensor, + name: str, + joint_seed: torch.Tensor, + **kwargs: object, + ) -> tuple[torch.Tensor, torch.Tensor]: + success, qpos = original_compute_batch_ik( + pose=pose, + name=name, + joint_seed=joint_seed, + **kwargs, + ) + if name == "left_arm" and pose.shape[1] > 1: + qpos = qpos.clone() + qpos[1, pose.shape[1] // 2] += excessive_joint_step + return success, qpos + + generator.robot.compute_batch_ik.side_effect = inject_dense_joint_jump + action = _bind_action( + generator, + CoordinatedPickment( + default_options=CoordinatedPickmentOptions( + hand_interp_steps=4, + hold_steps=2, + object_motion_keyframes=3, + ), + ), + ) + _stub_dual_arm_grasp_poses(action) + invocation = ActionInvocation( + skill_id="coordinated_pickment", + goal=CoordinatedPickGoal( + semantics=ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + entity_id="tray", + label="tray", + ), + object_target_pose=torch.eye(4), + object_initial_pose=torch.eye(4), + ), + binding=_dual_binding(action, "left", "right"), + motion_policy=MotionPolicy(sample_count=30), + ) + context = _dual_context() + + plan = _plan_action(action, invocation, context) + + assert plan.plan_success.tolist() == [True, False] + trajectory = _joint_trajectory(plan) + assert not torch.allclose(trajectory.positions[0], context.robot.qpos[0]) + assert torch.allclose( + trajectory.positions[1], + context.robot.qpos[1].unsqueeze(0).repeat(30, 1), + ) + assert all(not frame.active_mask[1].item() for frame in plan.commands.frames) + assert any( + call.kwargs.get("continuous") is True + for call in generator.robot.compute_batch_ik.call_args_list + ) + + def test_coordinated_pick_fails_when_affordance_has_no_grasp() -> None: generator = _dual_motion_generator() action = _bind_action( @@ -6959,6 +7226,8 @@ def test_composed_actions_materialize_velocity_and_end_in_zero_hold( # Constant joint intervals must not acquire spurious velocity from a neighbor. stationary = trajectory.positions[:, 1:] == trajectory.positions[:, :-1] assert torch.count_nonzero(trajectory.velocities[:, :-1][stationary]) == 0 + assert torch.count_nonzero(trajectory.velocities[:, 1:][stationary]) == 0 + assert torch.count_nonzero(trajectory.velocities[:, 0]) == 0 def test_position_only_motion_policy_emits_explicit_zero_velocity() -> None: @@ -7353,8 +7622,8 @@ def generate(target_states, options=None) -> PlanResult: return calls -def test_open_door_motion_gen_sends_contact_segment_endpoints() -> None: - """motion_gen gets each reach/retract endpoint, not exact line samples.""" +def test_open_door_motion_gen_rejects_uncertified_contact_path() -> None: + """Reject endpoint-only planning for a constrained door path.""" semantics = ObjectSemantics( affordance=_door_affordance(), geometry={}, @@ -7380,47 +7649,28 @@ def sample_grasp(*, obj_poses: torch.Tensor, **_: object): retract_distance=0.1, ) - plan = _plan_action( - action, - ActionInvocation( - skill_id="open_door", - goal=OpenDoorGoal( - semantics, - SceneEntityPose("target"), - open_fraction=1.0 / 3.0, - ), - binding=_binding(action), - motion_policy=MotionPolicy(strategy="motion_gen", sample_count=24), - skill_options=options, + invocation = ActionInvocation( + skill_id="open_door", + goal=OpenDoorGoal( + semantics, + SceneEntityPose("target"), + open_fraction=1.0 / 3.0, ), - _context(scene=_door_scene(torch.eye(4).repeat(NUM_ENVS, 1, 1))), - ) - - assert plan.plan_success.tolist() == [True, True] - assert [len(states) for states, _ in calls] == [1, 1, 4, 1] - assert not any(call_options.is_linear for _, call_options in calls) - assert torch.allclose( - calls[1][0][0].xpos[:, :3, 3], - torch.tensor([1.0, 0.0, 0.0]).expand(NUM_ENVS, -1), - atol=1.0e-6, - ) - angle = math.pi / 6 - expected_retract = torch.tensor( - [ - math.cos(angle) - options.retract_distance * math.sin(angle), - math.sin(angle) + options.retract_distance * math.cos(angle), - 0.0, - ] - ) - assert torch.allclose( - calls[3][0][0].xpos[:, :3, 3], - expected_retract.expand(NUM_ENVS, -1), - atol=1.0e-6, + binding=_binding(action), + motion_policy=MotionPolicy(strategy="motion_gen", sample_count=24), + skill_options=options, ) + with pytest.raises(ValueError, match="requires strategy='ik_interp'"): + _plan_action( + action, + invocation, + _context(scene=_door_scene(torch.eye(4).repeat(NUM_ENVS, 1, 1))), + ) + assert calls == [] -def test_push_object_motion_gen_sends_contact_segment_endpoints() -> None: - """motion_gen gets the contact, pushed and retract endpoints.""" +def test_push_object_motion_gen_rejects_uncertified_contact_path() -> None: + """Reject endpoint-only planning for a constrained push path.""" generator = _motion_generator() calls = _record_motion_generation(generator) action = _bind_action(generator, PushObject()) @@ -7437,34 +7687,21 @@ def test_push_object_motion_gen_sends_contact_segment_endpoints() -> None: }, ) - plan = _plan_action( - action, - ActionInvocation( - skill_id="push_object", - goal=PushObjectGoal( - _semantics(entity_id="utensil"), - SceneEntityPose("slot"), - ), - binding=_binding(action), - motion_policy=MotionPolicy(strategy="motion_gen", sample_count=24), - skill_options=PushObjectOptions( - hand_interp_steps=3, - approach_height=0.1, - retract_height=0.08, - contact_distance=0.03, - ), + invocation = ActionInvocation( + skill_id="push_object", + goal=PushObjectGoal( + _semantics(entity_id="utensil"), + SceneEntityPose("slot"), + ), + binding=_binding(action), + motion_policy=MotionPolicy(strategy="motion_gen", sample_count=24), + skill_options=PushObjectOptions( + hand_interp_steps=3, + approach_height=0.1, + retract_height=0.08, + contact_distance=0.03, ), - _context(scene=scene), ) - - assert plan.plan_success.tolist() == [True, True] - assert [len(states) for states, _ in calls] == [1, 1, 1, 1] - assert not any(call_options.is_linear for _, call_options in calls) - direction = torch.tensor([[0.0, 1.0, 0.0], [0.0, -1.0, 0.0]]) - contact = object_pose[:, :3, 3] - direction * 0.03 - pushed = contact + direction * 0.12 - goals = [states[0].xpos[:, :3, 3] for states, _ in calls] - assert torch.allclose(goals[0], contact + torch.tensor([0.0, 0.0, 0.1])) - assert torch.allclose(goals[1], contact) - assert torch.allclose(goals[2], pushed) - assert torch.allclose(goals[3], pushed + torch.tensor([0.0, 0.0, 0.08])) + with pytest.raises(ValueError, match="requires strategy='ik_interp'"): + _plan_action(action, invocation, _context(scene=scene)) + assert calls == [] diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 48662b737..3c9cff007 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -534,6 +534,22 @@ def test_motion_policy_normalizes_dynamic_collision_mode() -> None: MotionPolicy(dynamic_collision_mode=object()) # type: ignore[arg-type] +def test_motion_policy_preserves_existing_positional_constructor_order() -> None: + policy = MotionPolicy( + "ik_interp", + 50, + "zero", + DynamicCollisionMode.OFF, + None, + ) + + assert policy.velocity_targets == "zero" + assert policy.dynamic_collision_mode is DynamicCollisionMode.OFF + assert policy.plan_opts is None + assert policy.velocity_limit is None + assert policy.acceleration_limit == pytest.approx(0.5) + + def test_motion_policy_maps_to_motion_generator_strategy() -> None: planner_options = ToppraPlanOptions( constraints={"velocity": 0.2, "acceleration": 0.5} @@ -560,7 +576,7 @@ def test_motion_policy_maps_to_motion_generator_strategy() -> None: assert options.control_part == "arm" assert options.interpolation_dt == pytest.approx(0.02) assert options.velocity_limit is None - assert options.acceleration_limit is None + assert options.acceleration_limit == pytest.approx(0.5) assert isinstance(options.plan_opts, ToppraPlanOptions) assert options.plan_opts.constraints == {"velocity": 0.2, "acceleration": 0.5} @@ -1594,6 +1610,13 @@ def test_interpolation_policy_rejects_backend_options_at_construction() -> None: MotionPolicy(strategy="ik_interp", plan_opts=ToppraPlanOptions()) +@pytest.mark.parametrize("value", [0.0, -1.0, float("inf"), float("nan")]) +@pytest.mark.parametrize("name", ["velocity_limit", "acceleration_limit"]) +def test_motion_policy_rejects_invalid_motion_limits(name: str, value: float) -> None: + with pytest.raises(ValueError, match=name): + MotionPolicy(**{name: value}) + + def test_fixed_cartesian_policy_requires_explicit_interpolation_strategy() -> None: with pytest.raises(ValueError, match="cartesian_linear|preserve_cartesian_samples"): MotionPolicy(strategy="motion_gen").to_motion_gen_options( diff --git a/tests/sim/atomic_actions/test_primitives_helpers.py b/tests/sim/atomic_actions/test_primitives_helpers.py index 6474db31f..24716fb42 100644 --- a/tests/sim/atomic_actions/test_primitives_helpers.py +++ b/tests/sim/atomic_actions/test_primitives_helpers.py @@ -89,6 +89,38 @@ def test_resample_planned_trajectory_fits_native_path_to_phase_length() -> None: torch.testing.assert_close(result[:, -1], trajectory[:, -1]) +def test_resample_planned_trajectory_preserves_native_duration_on_control_grid() -> ( + None +): + trajectory = torch.tensor([[[0.0], [0.5], [1.0]]]) + dt = torch.tensor([[0.0, 0.2, 0.3]]) + + result = resample_planned_trajectory( + trajectory, + sample_count=3, + dt=dt, + control_dt=0.1, + ) + + assert result.shape == (1, 6, 1) + assert torch.equal(result[:, 0], trajectory[:, 0]) + assert torch.equal(result[:, -1], trajectory[:, -1]) + + +def test_resample_planned_trajectory_pads_but_never_shortens_native_timing() -> None: + trajectory = torch.tensor([[[0.0], [1.0]]]) + + result = resample_planned_trajectory( + trajectory, + sample_count=5, + dt=torch.tensor([[0.0, 0.1]]), + control_dt=0.1, + ) + + assert result.shape == (1, 5, 1) + assert torch.equal(result[:, -3:], torch.ones(1, 3, 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_runner.py b/tests/sim/atomic_actions/test_runner.py index fb84d8941..0abcc41d5 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -18,6 +18,7 @@ from __future__ import annotations +import math from collections import deque from typing import ClassVar from unittest.mock import Mock @@ -155,6 +156,21 @@ def advance(self, duration: float) -> None: self.time += duration +class QuantizedFakeClock(FakeClock): + """Clock that rejects sleeps outside its representable time grid.""" + + def __init__(self, sleep_quantum: float) -> None: + super().__init__() + self.sleep_quantum = sleep_quantum + + def sleep(self, duration: float) -> None: + """Advance only by an integer number of clock quanta.""" + ratio = duration / self.sleep_quantum + if not math.isclose(ratio, round(ratio), rel_tol=1.0e-6, abs_tol=1.0e-9): + raise ValueError("duration must align to the clock quantum") + super().sleep(duration) + + class FakeObservationProvider: """In-memory robot observation provider.""" @@ -362,10 +378,12 @@ def __init__( *, with_effect: bool = False, with_phase_gate: bool = False, + timing_fault: str | None = None, ) -> None: super().__init__() self.with_effect = with_effect self.with_phase_gate = with_phase_gate + self.timing_fault = timing_fault self.plan_count = 0 def _plan( @@ -385,8 +403,12 @@ def _plan( [[0.0, FIRST_INTERVAL, SECOND_INTERVAL]], dtype=torch.float32, ).repeat(context.batch_size, 1) - if context.batch_size > 1: - dt[1, 1:] *= 2.0 + if self.timing_fault == "first_mismatch": + dt[1, 1] *= 2.0 + elif self.timing_fault == "later_mismatch": + dt[1, 2] *= 2.0 + elif self.timing_fault == "later_short": + dt[:, 2] = MINIMUM_CYCLE_TIME / 2.0 trajectory = TimedTrajectory.from_positions( positions, env_ids=context.env_ids, @@ -438,6 +460,9 @@ def _make_runner( hold_on_completion: bool = True, hold_during_effect_verification: bool = True, physical_evidence_provider: PhysicalEvidenceProvider | None = None, + clock: FakeClock | None = None, + minimum_cycle_time: float = MINIMUM_CYCLE_TIME, + timing_fault: str | None = None, ) -> tuple[ ExecutionRunner, FakeClock, @@ -445,8 +470,8 @@ def _make_runner( FakeCommandSink, TimedAction, ]: - clock = FakeClock() - provider = FakeObservationProvider(clock, batch_size) + active_clock = FakeClock() if clock is None else clock + provider = FakeObservationProvider(active_clock, batch_size) sink = FakeCommandSink(provider) robot = Mock() robot.device = torch.device("cpu") @@ -463,6 +488,7 @@ def _make_runner( action = TimedAction( with_effect=with_effect, with_phase_gate=with_phase_gate, + timing_fault=timing_fault, ) engine = AtomicActionEngine(generator, tracking_runtime=tracking_runtime) engine.register(action) @@ -496,15 +522,15 @@ def _make_runner( session, provider, sink, - clock=clock, + clock=active_clock, cfg=ExecutionRunnerCfg( - minimum_cycle_time=MINIMUM_CYCLE_TIME, + minimum_cycle_time=minimum_cycle_time, hold_on_completion=hold_on_completion, hold_during_effect_verification=hold_during_effect_verification, ), physical_evidence_provider=physical_evidence_provider, ) - return runner, clock, provider, sink, action + return runner, active_clock, provider, sink, action def _successful_effect_result( @@ -765,15 +791,50 @@ def test_session_active_commands_return_an_owned_endpoint_snapshot() -> None: assert torch.all(current_payload.positions >= 0.0) -def test_runner_uses_the_longest_active_batch_interval_as_a_barrier() -> None: - runner, clock, _, _, _ = _make_runner(batch_size=2) +def test_runner_rejects_mismatched_active_batch_intervals_before_dispatch() -> None: + runner, _, _, sink, _ = _make_runner( + batch_size=2, + timing_fault="first_mismatch", + ) + + failed = runner.step() + + assert failed.status is RunnerStatus.FAILED + assert sink.sent == [] + assert [item.operation for item in failed.dispatches] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + + +@pytest.mark.parametrize("timing_fault", ["later_mismatch", "later_short"]) +def test_runner_safely_stops_after_later_interval_validation_failure( + timing_fault: str, +) -> None: + runner, clock, _, sink, _ = _make_runner( + batch_size=2, + timing_fault=timing_fault, + ) first = runner.step() - clock.advance(2.0 * FIRST_INTERVAL) - second = runner.step() + clock.advance(FIRST_INTERVAL) + failed = runner.step() + repeated = runner.step() - assert first.wait_duration == pytest.approx(2.0 * FIRST_INTERVAL) - assert second.wait_duration == pytest.approx(2.0 * SECOND_INTERVAL) + assert first.status is RunnerStatus.RUNNING + assert failed.status is RunnerStatus.FAILED + assert failed.command_count == 1 + assert len(sink.sent) == 1 + assert [item.operation for item in failed.dispatches] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert [target.target_id for target in sink.cancelled[0]] == ["arm"] + assert [target.target_id for target in sink.held[0][0]] == ["arm"] + assert repeated.status is RunnerStatus.FAILED + assert repeated.dispatches == () + assert len(sink.sent) == 1 def test_runner_completes_and_holds_after_last_command_settles() -> None: @@ -1115,6 +1176,34 @@ def test_blocking_runner_verifies_effect_before_committing_task_state() -> None: assert completed.tick.task_state.get_held_object("arm") is not None +def test_blocking_runner_aligns_passive_polling_to_clock_quantum() -> None: + clock = QuantizedFakeClock(0.01) + runner, _, _, _, _ = _make_runner( + with_effect=True, + clock=clock, + minimum_cycle_time=0.001, + ) + + completed = runner.run_until_blocked( + effect_verifier=_successful_effect_result, + ) + + assert completed.status is RunnerStatus.COMPLETED + assert any( + math.isclose(duration, 0.01, rel_tol=1.0e-6, abs_tol=1.0e-9) + for duration in clock.sleeps + ) + assert all( + math.isclose( + duration / clock.sleep_quantum, + round(duration / clock.sleep_quantum), + rel_tol=1.0e-6, + abs_tol=1.0e-9, + ) + for duration in clock.sleeps + ) + + def test_blocking_runner_resumes_a_stored_effect_verification_boundary() -> None: runner, _, _, _, _ = _make_runner(with_effect=True) diff --git a/tests/sim/atomic_actions/test_sim_adapter.py b/tests/sim/atomic_actions/test_sim_adapter.py index 590351cde..d118ff71f 100644 --- a/tests/sim/atomic_actions/test_sim_adapter.py +++ b/tests/sim/atomic_actions/test_sim_adapter.py @@ -325,14 +325,22 @@ def test_simulation_adapter_cancel_validates_transport_targets() -> None: assert "outside robot DOF" in invalid.message -def test_simulation_adapter_sleep_advances_integral_physics_steps() -> None: +def test_simulation_adapter_sleep_rejects_off_grid_duration() -> None: simulation, robot = _simulation_and_robot() adapter = SimulationExecutionAdapter(simulation, robot) - adapter.sleep(0.025) + with pytest.raises(ValueError, match="integer multiple"): + adapter.sleep(0.025) - simulation.update.assert_called_once_with(physics_dt=PHYSICS_DT, step=3) - assert adapter.now() == pytest.approx(0.03) + simulation.update.assert_not_called() + assert adapter.now() == 0.0 + + +def test_simulation_adapter_rejects_off_grid_control_period() -> None: + simulation, robot = _simulation_and_robot() + + with pytest.raises(ValueError, match="control_dt.*integer multiple"): + SimulationExecutionAdapter(simulation, robot, control_dt=0.025) def test_simulation_adapter_sleep_absorbs_float32_roundoff() -> None: diff --git a/tests/sim/atomic_actions/test_trajectory_ops.py b/tests/sim/atomic_actions/test_trajectory_ops.py index a4e7e7ce6..559c577a3 100644 --- a/tests/sim/atomic_actions/test_trajectory_ops.py +++ b/tests/sim/atomic_actions/test_trajectory_ops.py @@ -401,6 +401,15 @@ def test_interpolate_hand_qpos_preserves_endpoints(): assert torch.allclose(out[:, -1], end) +def test_interpolate_hand_qpos_one_waypoint_reaches_command() -> None: + start = torch.tensor([[0.0, 0.0]]) + end = torch.tensor([[1.0, 1.0]]) + + out = interpolate_hand_qpos(start, end, n_waypoints=1) + + assert torch.equal(out[:, 0], end) + + class TestInterpolateJointTrajectory: def test_interpolates_start_to_target(self): start = torch.zeros(2, 6) diff --git a/tests/sim/motion/planners/test_toppra_batched.py b/tests/sim/motion/planners/test_toppra_batched.py index e351ee90f..38d6a1fc0 100644 --- a/tests/sim/motion/planners/test_toppra_batched.py +++ b/tests/sim/motion/planners/test_toppra_batched.py @@ -27,6 +27,22 @@ from embodichain.lab.sim.motion.planners.utils import TrajectorySampleMethod +class _DeviceBoundTensor(torch.Tensor): + """CPU-backed stand-in that rejects direct NumPy conversion like CUDA.""" + + @staticmethod + def __new__(cls, value: list[float]) -> _DeviceBoundTensor: + return torch.Tensor._make_subclass(cls, torch.as_tensor(value), False) + + def __array__(self, dtype=None, copy=None): + del dtype, copy + raise TypeError("move this device-bound tensor to CPU before NumPy conversion") + + def cpu(self, *args, **kwargs) -> torch.Tensor: + del args, kwargs + return self.as_subclass(torch.Tensor).clone() + + # Note: TOPPRA's ParametrizeConstAccel robustly avoids returning None for smooth # splines (verified empirically across tiny limits, huge displacements, zigzags, # and plateaus), so the ``jnt_traj is None`` branch in ``_toppra_solve_one_env`` @@ -130,6 +146,56 @@ def test_mp_context_auto_resolution(self): assert resolve("fork", torch.device("cuda", 0)) == "fork" +@pytest.mark.parametrize("batch_size", [1, 2]) +def test_plan_materializes_device_bound_constraints_before_numpy_worker( + batch_size: int, +) -> None: + from embodichain.lab.sim.motion.planners.toppra_planner import ( + ToppraPlanner, + ToppraPlannerCfg, + ToppraPlanOptions, + ) + from embodichain.lab.sim.motion.planners.utils import PlanState + + planner = object.__new__(ToppraPlanner) + planner.cfg = ToppraPlannerCfg(robot_uid="test", max_workers=batch_size) + planner.device = torch.device("cpu") + planner._pool = None + planner._mp_context = "fork" + if batch_size > 1: + + class _ImmediateFuture: + def __init__(self, result: dict) -> None: + self._result = result + + def result(self) -> dict: + return self._result + + class _InspectingPool: + @staticmethod + def submit(fn, *args) -> _ImmediateFuture: + assert isinstance(args[1], np.ndarray) + assert isinstance(args[2], np.ndarray) + return _ImmediateFuture(fn(*args)) + + planner._get_pool = lambda _: _InspectingPool() + qpos = torch.zeros(batch_size, 2) + states = [PlanState.from_qpos(qpos), PlanState.from_qpos(qpos.clone())] + options = ToppraPlanOptions( + sample_method=TrajectorySampleMethod.QUANTITY, + sample_interval=2, + constraints={"velocity": 0.2, "acceleration": 0.5}, + ) + options.constraints["velocity"] = _DeviceBoundTensor([0.2, 0.3]) + options.constraints["acceleration"] = _DeviceBoundTensor([0.5, 0.6]) + + result = planner.plan(states, options) + + assert result.success.tolist() == [True] * batch_size + assert result.positions is not None + assert result.positions.shape == (batch_size, 2, 2) + + class TestToppraPlanBatched: def _make_planner(self): from embodichain.lab.sim.motion.planners.toppra_planner import ( diff --git a/tests/sim/motion/solvers/test_base_solver.py b/tests/sim/motion/solvers/test_base_solver.py index 9ecf6242c..9cb829b06 100644 --- a/tests/sim/motion/solvers/test_base_solver.py +++ b/tests/sim/motion/solvers/test_base_solver.py @@ -91,9 +91,11 @@ def test_invalid_tcp_update_preserves_numerical_solver_state(solver_type, tcp): def test_continuous_batch_ik_is_an_optional_solver_capability() -> None: # Capability discovery must not initialize kinematics or simulator resources. - ordinary = object.__new__(PytorchSolver) + ordinary = object.__new__(DifferentialSolver) + pytorch = object.__new__(PytorchSolver) opw = object.__new__(OPWSolver) assert not ordinary.supports_continuous_batch_ik + assert pytorch.supports_continuous_batch_ik assert opw.supports_continuous_batch_ik with pytest.raises(NotImplementedError, match="continuous batch IK"): ordinary._select_continuous_ik_path( diff --git a/tests/sim/motion/solvers/test_pytorch_solver.py b/tests/sim/motion/solvers/test_pytorch_solver.py index e7d08cadd..215626722 100644 --- a/tests/sim/motion/solvers/test_pytorch_solver.py +++ b/tests/sim/motion/solvers/test_pytorch_solver.py @@ -73,6 +73,80 @@ def capture_target(target_pose, joint_seed): torch.testing.assert_close(target, original_target, atol=0, rtol=0) np.testing.assert_array_equal(solver.get_tcp(), tcp) + valid, candidates = solver.get_ik( + target, + qpos_seed=torch.zeros((2, 1)), + return_all_solutions=True, + ) + assert valid.shape == (2, 1) + assert candidates.shape == (2, 1, 1) + + +@pytest.mark.no_sim +def test_continuous_path_selection_tracks_previous_solution() -> None: + """Manipulability ordering cannot make a continuous path jump branches.""" + solver = object.__new__(PytorchSolver) + solver.device = torch.device("cpu") + solver.dof = 1 + solver.ik_nearest_weight = torch.ones(1) + candidates = torch.tensor( + [[[[0.1], [2.8]], [[2.9], [0.2]], [[0.3], [3.0]]]], + dtype=torch.float32, + ) + valid = torch.ones(1, 3, 2, dtype=torch.bool) + + path_valid, path = solver._select_continuous_ik_path( + candidates, valid, torch.zeros(1, 1) + ) + + assert path_valid.all() + torch.testing.assert_close(path, torch.tensor([[[0.1], [0.2], [0.3]]])) + + +@pytest.mark.no_sim +def test_return_all_solutions_preserves_candidate_validity(monkeypatch) -> None: + solver = object.__new__(PytorchSolver) + solver.device = torch.device("cpu") + solver.dof = 1 + solver._num_samples = 2 + solver._seed_sampler = None + solver.lower_qpos_limits = torch.tensor([-2.0]) + solver.upper_qpos_limits = torch.tensor([2.0]) + solver.set_tcp(np.eye(4)) + convergence = torch.tensor([True, False, False, True]) + solutions = torch.tensor([[0.1], [0.2], [0.3], [0.4]]) + monkeypatch.setattr( + solver, + "_compute_inverse_kinematics", + lambda target_pose, joint_seed: (convergence, solutions), + ) + + valid, candidates = solver.get_ik( + torch.eye(4).repeat(2, 1, 1), + qpos_seed=torch.zeros(2, 1), + return_all_solutions=True, + ) + + assert valid.tolist() == [[True, False], [False, True]] + torch.testing.assert_close(candidates, solutions.reshape(2, 2, 1)) + + +@pytest.mark.no_sim +def test_continuous_path_selection_holds_last_solution_on_failure() -> None: + solver = object.__new__(PytorchSolver) + solver.device = torch.device("cpu") + solver.dof = 1 + solver.ik_nearest_weight = torch.ones(1) + candidates = torch.tensor([[[[0.1]], [[float("nan")]], [[0.3]]]]) + valid = torch.ones(1, 3, 1, dtype=torch.bool) + + path_valid, path = solver._select_continuous_ik_path( + candidates, valid, torch.zeros(1, 1) + ) + + assert path_valid.tolist() == [[True, False, True]] + torch.testing.assert_close(path, torch.tensor([[[0.1], [0.1], [0.3]]])) + def grid_sample_qpos_from_limits( qpos_limits: torch.Tensor, diff --git a/tests/sim/motion/test_motion_generator_batched.py b/tests/sim/motion/test_motion_generator_batched.py index a71158358..a2ec6274d 100644 --- a/tests/sim/motion/test_motion_generator_batched.py +++ b/tests/sim/motion/test_motion_generator_batched.py @@ -16,6 +16,7 @@ from __future__ import annotations +import math from types import SimpleNamespace from typing import Literal from unittest.mock import Mock, patch @@ -114,6 +115,9 @@ def _trapezoidal_generator() -> MotionGenerator: generator = object.__new__(MotionGenerator) generator.planner = planner generator.device = torch.device("cpu") + generator.robot = SimpleNamespace( + get_qvel_limits=lambda *, name=None: torch.full((2, 1), 10.0) + ) return generator @@ -172,7 +176,8 @@ def test_trapezoidal_generator_covers_dense_ik_waypoints() -> None: 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) + ).expand(qpos.shape[0], -1, -1), + get_qvel_limits=lambda *, name=None: torch.full((1, 2), 10.0), ) 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() @@ -630,6 +635,117 @@ def test_resolve_trapezoidal_limits_without_sample_count() -> None: assert options.constraints["acceleration"] == 0.02 +def test_resolve_trapezoidal_preserves_omitted_velocity_limit() -> None: + planner = Mock() + planner.cfg.planner_type = "trapezoidal" + generator = object.__new__(MotionGenerator) + generator.planner = planner + + options = generator.resolve_plan_options( + plan_opts=None, + sample_count=20, + velocity_limit=None, + acceleration_limit=None, + ) + + assert isinstance(options, TrapezoidalPlanOptions) + assert options.constraints["velocity"] is None + assert options.constraints["acceleration"] == pytest.approx(0.5) + + +def test_ik_velocity_limits_preserve_caps_and_intersect_scalar_request() -> None: + limits = torch.tensor([[0.08, 1.2], [0.05, 1.4]], dtype=torch.float64) + robot = SimpleNamespace( + joint_names=["slow", "fast"], + mimic_ids=[], + mimic_parents=[], + mimic_multipliers=[], + control_parts={"arm": object()}, + get_joint_ids=lambda name: [0, 1], + get_qvel_limits=lambda *, joint_ids: limits[:, joint_ids], + ) + generator = object.__new__(MotionGenerator) + generator.robot = robot + reference = torch.zeros(2, 2, dtype=torch.float64) + + omitted = generator._resolve_velocity_limits( + control_part="arm", + batch_size=2, + dof=2, + requested=None, + reference=reference, + ) + explicit = generator._resolve_velocity_limits( + control_part="arm", + batch_size=2, + dof=2, + requested=0.2, + reference=reference, + ) + + torch.testing.assert_close(omitted, torch.tensor([0.05, 1.2], dtype=torch.float64)) + torch.testing.assert_close(explicit, torch.tensor([0.05, 0.2], dtype=torch.float64)) + + +def test_ik_velocity_limits_apply_mimic_caps_in_control_part_order() -> None: + limits = torch.tensor([[1.2, 2.0, 0.1]], dtype=torch.float64) + robot = SimpleNamespace( + joint_names=["parent", "other", "follower"], + mimic_ids=[2], + mimic_parents=[0], + mimic_multipliers=[2.0], + control_parts={"arm": object()}, + get_joint_ids=lambda name: [1, 0], + get_qvel_limits=lambda *, joint_ids: limits[:, joint_ids], + ) + generator = object.__new__(MotionGenerator) + generator.robot = robot + + resolved = generator._resolve_velocity_limits( + control_part="arm", + batch_size=1, + dof=2, + requested=None, + reference=torch.zeros(1, 2, dtype=torch.float64), + ) + + torch.testing.assert_close(resolved, torch.tensor([2.0, 0.05], dtype=torch.float64)) + + +def test_ik_interpolation_respects_robot_per_joint_velocity_caps() -> None: + limits = torch.tensor([[0.05, 1.2], [0.08, 1.4]], dtype=torch.float64) + robot = SimpleNamespace( + joint_names=["slow", "fast"], + mimic_ids=[], + mimic_parents=[], + mimic_multipliers=[], + control_parts={"arm": object()}, + get_joint_ids=lambda name: [0, 1], + get_qvel_limits=lambda *, joint_ids: limits[:, joint_ids], + ) + generator = object.__new__(MotionGenerator) + generator.robot = robot + keyframes = torch.tensor( + [[[0.0, 0.0], [0.2, 0.2]], [[0.0, 0.0], [0.2, 0.2]]], + dtype=torch.float64, + ) + + positions = generator._interpolate_ik_keyframes( + keyframes, + MotionGenOptions( + strategy="ik_interp", + sample_count=3, + interpolation_dt=0.1, + control_part="arm", + acceleration_limit=100.0, + ), + ) + + peak_velocity = (positions.diff(dim=1) / 0.1).abs().amax(dim=(0, 1)) + assert peak_velocity[0] <= 0.05 + 1.0e-6 + assert peak_velocity[1] <= 1.2 + 1.0e-6 + + def _mock_generator( *, batch_size: int = 2, @@ -648,6 +764,10 @@ def _mock_generator( torch.ones(pose.shape[:2], dtype=torch.bool), torch.zeros(*pose.shape[:2], controlled_dof), ) + robot.get_qvel_limits.return_value = torch.full( + (batch_size, controlled_dof), + 10.0, + ) planner = Mock() planner.cfg.planner_type = "toppra" planner.robot = robot @@ -843,6 +963,167 @@ def encode_position( generator.robot.compute_ik.assert_not_called() generator.planner.plan.assert_not_called() + def test_limited_cartesian_motion_expands_exact_ik_path(self): + generator = _mock_generator() + generator.robot.compute_fk.return_value = torch.eye(4).repeat(BATCH_SIZE, 1, 1) + + def encode_position( + pose: torch.Tensor, + name: str, + joint_seed: torch.Tensor, + continuous: bool = False, + ) -> tuple[torch.Tensor, torch.Tensor]: + qpos = joint_seed.clone() + qpos[..., 0] = pose[..., 0, 3] + return torch.ones(pose.shape[:2], dtype=torch.bool), qpos + + generator.robot.compute_batch_ik.side_effect = encode_position + weights = torch.linspace(0.25, 1.0, 4) + targets = [] + for weight in weights: + pose = torch.eye(4).repeat(BATCH_SIZE, 1, 1) + pose[:, 0, 3] = weight + targets.append(PlanState.from_xpos(pose)) + + result = generator.generate( + targets, + MotionGenOptions( + strategy="ik_interp", + sample_count=5, + start_qpos=torch.zeros(BATCH_SIZE, CONTROLLED_DOF), + control_part="arm", + is_linear=True, + interpolation_dt=0.05, + preserve_cartesian_samples=True, + velocity_limit=1.0, + acceleration_limit=2.0, + ), + ) + + assert result.positions is not None + assert result.positions.shape[1] > 5 + interval_velocity = result.positions.diff(dim=1) / 0.05 + boundary_acceleration = ( + torch.cat( + [ + interval_velocity[:, :1], + interval_velocity.diff(dim=1), + -interval_velocity[:, -1:], + ], + dim=1, + ) + / 0.05 + ) + assert interval_velocity.abs().max() <= 1.0 + 1.0e-5 + assert boundary_acceleration.abs().max() <= 2.0 + 1.0e-4 + assert torch.all(result.positions[:, 1:, 0] >= result.positions[:, :-1, 0]) + generator.planner.plan.assert_not_called() + + def test_limited_cartesian_motion_resamples_external_rotation_exactly(self): + generator = _mock_generator() + start_pose = torch.eye(4).repeat(BATCH_SIZE, 1, 1) + start_pose[:, 0, 3] = 1.0 - 2.5e-4 + generator.robot.compute_fk.return_value = start_pose + generator.robot.get_solver.return_value = SimpleNamespace( + supports_continuous_batch_ik=False, + cfg=SimpleNamespace(pos_eps=5.0e-4, rot_eps=5.0e-4), + ) + + def encode_angle( + pose: torch.Tensor, + name: str, + joint_seed: torch.Tensor, + continuous: bool = False, + ) -> tuple[torch.Tensor, torch.Tensor]: + del name, continuous + qpos = joint_seed.clone() + qpos[..., 0] = torch.atan2(pose[..., 1, 3], pose[..., 0, 3]) + return torch.ones(pose.shape[:2], dtype=torch.bool), qpos + + sampled_paths: list[torch.Tensor] = [] + + def sample_quarter_circle(progress: torch.Tensor) -> torch.Tensor: + angles = (math.pi / 2.0) * progress + poses = torch.eye(4).repeat(BATCH_SIZE, progress.numel(), 1, 1) + poses[..., 0, 3] = torch.cos(angles) + poses[..., 1, 3] = torch.sin(angles) + sampled_paths.append(poses.clone()) + return poses + + generator.robot.compute_batch_ik.side_effect = encode_angle + targets = [] + for angle in torch.linspace(math.pi / 8.0, math.pi / 2.0, 4): + pose = torch.eye(4).repeat(BATCH_SIZE, 1, 1) + pose[:, 0, 3] = torch.cos(angle) + pose[:, 1, 3] = torch.sin(angle) + targets.append(PlanState.from_xpos(pose)) + + result = generator.generate( + targets, + MotionGenOptions( + strategy="ik_interp", + sample_count=5, + start_qpos=torch.zeros(BATCH_SIZE, CONTROLLED_DOF), + control_part="arm", + is_linear=True, + interpolation_dt=0.05, + preserve_cartesian_samples=True, + cartesian_path_sampler=sample_quarter_circle, + velocity_limit=1.0, + acceleration_limit=2.0, + ), + ) + + assert result.positions is not None + assert result.positions.shape[1] > 5 + assert sampled_paths + radii = torch.linalg.vector_norm(sampled_paths[-1][..., :2, 3], dim=-1) + assert torch.allclose(radii, torch.ones_like(radii)) + assert torch.allclose( + result.positions[:, -1, 0], + torch.full((BATCH_SIZE,), math.pi / 2.0), + ) + generator.planner.plan.assert_not_called() + + def test_dense_float32_cartesian_progress_is_monotonic_and_dtype_stable(self): + generator = _mock_generator() + sample_count = 10_001 + generator.robot.compute_fk.return_value = torch.eye( + 4, dtype=torch.float32 + ).repeat(BATCH_SIZE, 1, 1) + target_poses = torch.eye(4, dtype=torch.float32).repeat(BATCH_SIZE, 1, 1, 1) + target_poses[:, 0, 0, 3] = 1.0 + solved_keyframes = torch.zeros(BATCH_SIZE, 2, CONTROLLED_DOF) + solved_keyframes[:, 1, 0] = 1.0 + observed_progress = None + + def sample_path(progress: torch.Tensor) -> torch.Tensor: + nonlocal observed_progress + observed_progress = progress + poses = torch.eye(4, dtype=progress.dtype, device=progress.device).repeat( + BATCH_SIZE, progress.numel(), 1, 1 + ) + poses[..., 0, 3] = progress + return poses + + result = generator._sample_cartesian_path( + target_poses, + solved_keyframes, + sample_count, + "arm", + path_sampler=sample_path, + ) + + assert observed_progress is not None + assert observed_progress.dtype == target_poses.dtype + assert observed_progress.device == target_poses.device + assert torch.all(observed_progress.diff() >= 0.0) + assert observed_progress[0] == 0.0 + assert observed_progress[-1] == 1.0 + assert result.dtype == target_poses.dtype + assert result.device == target_poses.device + assert torch.equal(result[..., 0, 3], observed_progress.expand(BATCH_SIZE, -1)) + def test_motion_gen_delegates_and_resamples_backend_result(self): raw_sample_count = 5 generator = _mock_generator( @@ -996,7 +1277,10 @@ def test_ik_interpolation_materializes_reference_velocity() -> None: ), ) assert result.velocities is not None - torch.testing.assert_close(result.velocities, torch.ones_like(result.positions)) + expected = torch.ones_like(result.positions) + expected[:, 0] = 0.0 + expected[:, -1] = 0.0 + torch.testing.assert_close(result.velocities, expected) def test_normalization_preserves_native_derivatives_and_zeros_failed_rows() -> None: @@ -1050,6 +1334,10 @@ class _CartesianRobot: device = torch.device("cpu") num_instances = 2 + def get_qvel_limits(self, *, name: str | None = None) -> torch.Tensor: + del name + return torch.full((self.num_instances, 1), 10.0) + def get_solver(self, name: str) -> SimpleNamespace: return SimpleNamespace(supports_continuous_batch_ik=False) @@ -1403,25 +1691,34 @@ def test_motion_gen_rejects_fixed_cartesian_samples_instead_of_bypassing_limits( @pytest.mark.parametrize( - "unused_option", + "limit_option", [ - {"plan_opts": TrapezoidalPlanOptions()}, {"velocity_limit": 0.1}, {"acceleration_limit": 0.2}, ], ) -def test_ik_interp_rejects_unused_backend_constraints(unused_option: dict) -> None: +def test_ik_interp_consumes_backend_neutral_motion_limits(limit_option: dict) -> None: generator = _trapezoidal_generator() - with pytest.raises(ValueError, match="ik_interp"): - generator.generate( - [PlanState.from_qpos(torch.ones(1, 1))], - MotionGenOptions( - strategy="ik_interp", - start_qpos=torch.zeros(1, 1), - sample_count=3, - interpolation_dt=0.01, - **unused_option, - ), + result = generator.generate( + [PlanState.from_qpos(torch.ones(1, 1))], + MotionGenOptions( + strategy="ik_interp", + start_qpos=torch.zeros(1, 1), + sample_count=3, + interpolation_dt=0.01, + **limit_option, + ), + ) + + assert result.positions is not None + assert result.positions.shape[1] > 3 + + +def test_ik_interp_still_rejects_backend_specific_plan_options() -> None: + with pytest.raises(ValueError, match="plan_opts"): + MotionGenOptions( + strategy="ik_interp", + plan_opts=TrapezoidalPlanOptions(), ) diff --git a/tests/sim/objects/test_robot.py b/tests/sim/objects/test_robot.py index 1f1839131..7c9dcfdf0 100644 --- a/tests/sim/objects/test_robot.py +++ b/tests/sim/objects/test_robot.py @@ -108,6 +108,10 @@ def test_compute_batch_ik_continuous_reuses_all_solver_candidates() -> None: assert torch.equal(qpos, selected_qpos) solver.get_ik.assert_called_once() assert solver.get_ik.call_args.kwargs["return_all_solutions"] is True + torch.testing.assert_close( + solver.get_ik.call_args.kwargs["qpos_seed"], + seed[:, None].expand(2, 3, 2).reshape(6, 2), + ) solver._select_continuous_ik_path.assert_called_once()