diff --git a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py index fbaa8e7360..073e0fb38c 100644 --- a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py @@ -58,6 +58,32 @@ logger = setup_logger() +def claim_with_gripper(claim: ResourceClaim, gripper_joint: str | None) -> ResourceClaim: + """Extend an arm task claim with its configured gripper joint.""" + if gripper_joint is None: + return claim + return ResourceClaim( + joints=claim.joints | frozenset([gripper_joint]), + priority=claim.priority, + mode=claim.mode, + ) + + +def append_gripper_position( + output: JointCommandOutput | None, + gripper_joint: str | None, + position: float, +) -> JointCommandOutput | None: + """Append a configured gripper position to an arm task output.""" + if output is None or gripper_joint is None: + return output + return JointCommandOutput( + joint_names=[*output.joint_names, gripper_joint], + positions=[*(output.positions or []), position], + mode=output.mode, + ) + + @dataclass class CartesianIKTaskConfig: """Configuration for cartesian IK task. @@ -67,6 +93,7 @@ class CartesianIKTaskConfig: priority: Priority for arbitration (higher wins) timeout: If no command received for this many seconds, go inactive (0 = never) max_joint_delta_deg: Maximum allowed joint change per tick (safety limit) + max_tracking_error_deg: Maximum command-to-feedback error before rebasing """ joint_names: list[str] @@ -74,6 +101,7 @@ class CartesianIKTaskConfig: priority: int = 10 timeout: float = 0.5 max_joint_delta_deg: float = 15.0 # ~1500°/s at 100Hz + max_tracking_error_deg: float = 10.0 min_dt: FiniteFloat = 1e-4 max_dt: FiniteFloat = 0.05 @@ -93,8 +121,10 @@ class CartesianIKTask(BaseControlTask): """Cartesian control task with Pink differential IK. Accepts streaming cartesian poses via on_cartesian_command() and computes IK - internally to output joint commands. Pink re-anchors each solve to the - current joint state from CoordinatorState. + internally to output joint commands. Each accepted differential IK result + seeds the next solve while measured hardware remains within the configured + tracking-error bound. Lagging or stalled hardware automatically rebases the + solve to measured state. Unlike CartesianServoTask (which bypasses joint arbitration), this task outputs JointCommandOutput and participates in joint-level arbitration. @@ -132,6 +162,8 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: raise ValueError("CartesianIKTask timeout must be finite and non-negative") if not np.isfinite(config.max_joint_delta_deg) or config.max_joint_delta_deg <= 0.0: raise ValueError("CartesianIKTask max_joint_delta_deg must be positive and finite") + if not np.isfinite(config.max_tracking_error_deg) or config.max_tracking_error_deg <= 0.0: + raise ValueError("CartesianIKTask max_tracking_error_deg must be positive and finite") self._name = name self._config = config @@ -159,11 +191,13 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: self._target_pose: Pose | PoseStamped | None = None self._last_update_time: float = 0.0 self._active = False + self._last_commanded_joints: NDArray[np.float64] | None = None logger.info( - f"CartesianIKTask {name} initialized with model: " - f"{config.control_ik.robot_model.model_path}, " - f"joints={config.joint_names}" + "Cartesian IK task initialized", + task=name, + model_path=str(config.control_ik.robot_model.model_path), + joints=config.joint_names, ) def claim(self) -> ResourceClaim: @@ -186,7 +220,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: state: Current coordinator state (contains measured joint positions) Returns: - JointCommandOutput with positions or a measured-state hold after an + JointCommandOutput with positions or a solve-state hold after an expected runtime failure; None if inactive or timed out. """ with self._lock: @@ -197,21 +231,24 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: time_since_update = state.t_now - self._last_update_time if time_since_update > self._config.timeout: logger.warning( - f"CartesianIKTask {self._name} timed out " - f"(no update for {time_since_update:.3f}s)" + "Cartesian IK task timed out", + task=self._name, + seconds_since_update=time_since_update, ) self._active = False self._target_pose = None + self._last_commanded_joints = None self._on_timeout() return None - q_current = self._get_current_joints(state) - if q_current is None: - logger.debug(f"CartesianIKTask {self._name}: missing joint state for IK warm-start") + q_measured = self._get_current_joints(state) + if q_measured is None: + logger.debug("Missing joint state for IK warm-start", task=self._name) return None - if not np.all(np.isfinite(q_current)): - logger.error("CartesianIKTask %s: measured joint state is non-finite", self._name) + if not np.all(np.isfinite(q_measured)): + logger.error("Measured joint state is non-finite", task=self._name) return None + q_current = self._solve_seed(q_measured) raw_dt = state.dt if not np.isfinite(raw_dt) or raw_dt <= 0.0: return self._hold(q_current) @@ -219,7 +256,9 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: try: target_pose = self._prepare_target(state, q_current, dt) except (FloatingPointError, RuntimeError, ValueError) as exc: - logger.warning("CartesianIKTask %s: target preparation failed: %s", self._name, exc) + logger.warning( + "Cartesian IK target preparation failed", task=self._name, error=str(exc) + ) return self._hold(q_current) if target_pose is None: return self._hold(q_current) @@ -228,23 +267,28 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: try: result = self._ik.solve(target_pose, q_current, dt) except (FloatingPointError, RuntimeError, ValueError) as exc: - logger.warning("CartesianIKTask %s: IK solve failed: %s", self._name, exc) + logger.warning("Cartesian IK solve failed", task=self._name, error=str(exc)) return self._hold(q_current) q_solution = np.asarray(result.positions, dtype=np.float64).reshape(-1) if not np.all(np.isfinite(q_solution)) or q_solution.shape != q_current.shape: - logger.warning("CartesianIKTask %s: rejecting invalid IK output", self._name) + logger.warning("Rejecting invalid Cartesian IK output", task=self._name) return self._hold(q_current) # Safety check: reject if any joint delta exceeds limit if not check_joint_delta(q_solution, q_current, self._config.max_joint_delta_deg): worst_idx, worst_deg = get_worst_joint_delta(q_solution, q_current) logger.warning( - f"CartesianIKTask {self._name}: rejecting motion - " - f"joint {self._joint_names_list[worst_idx]} delta " - f"{worst_deg:.1f}° exceeds limit {self._config.max_joint_delta_deg}°" + "Rejecting Cartesian IK motion exceeding joint delta limit", + task=self._name, + joint=self._joint_names_list[worst_idx], + joint_delta_deg=worst_deg, + max_joint_delta_deg=self._config.max_joint_delta_deg, ) return self._hold(q_current) + with self._lock: + if self._active: + self._last_commanded_joints = q_solution.copy() return JointCommandOutput( joint_names=self._joint_names_list, positions=q_solution.flatten().tolist(), @@ -252,7 +296,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: ) def _hold(self, q_current: NDArray[np.float64]) -> JointCommandOutput: - """Keep the measured configuration under the task's servo contract.""" + """Keep the selected solve configuration under the task's servo contract.""" return JointCommandOutput( joint_names=self._joint_names_list, positions=q_current.tolist(), @@ -269,13 +313,44 @@ def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.float64] | positions.append(pos) return np.array(positions, dtype=np.float64) + def _solve_seed(self, q_measured: NDArray[np.float64]) -> NDArray[np.float64]: + """Return a bounded command seed, rebasing to feedback when tracking diverges.""" + with self._lock: + cached = ( + None if self._last_commanded_joints is None else self._last_commanded_joints.copy() + ) + if cached is None: + return q_measured + if cached.shape != q_measured.shape or not np.all(np.isfinite(cached)): + logger.error("Cached Cartesian IK joint command is invalid", task=self._name) + self._reset_command_state() + return q_measured + tracking_error_deg = np.rad2deg(np.abs(cached - q_measured)) + if np.any(tracking_error_deg > self._config.max_tracking_error_deg): + worst_index = int(np.argmax(tracking_error_deg)) + logger.warning( + "Rebasing Cartesian IK solve to measured state", + task=self._name, + joint=self._joint_names_list[worst_index], + tracking_error_deg=tracking_error_deg[worst_index], + max_tracking_error_deg=self._config.max_tracking_error_deg, + ) + self._reset_command_state() + return q_measured + return cached + + def _reset_command_state(self) -> None: + """Discard the retained differential-IK command seed.""" + with self._lock: + self._last_commanded_joints = None + def _prepare_target( self, state: CoordinatorState, q_current: NDArray[np.float64], dt: float, ) -> pinocchio.SE3 | None: - """Prepare one normalized target for the measured-state solve.""" + """Prepare one normalized target for the selected solve configuration.""" with self._lock: pose = self._target_pose if pose is None: @@ -313,8 +388,12 @@ def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: joints: Joints that were preempted """ if joints & self._joint_names: + self._reset_command_state() logger.warning( - f"CartesianIKTask {self._name} preempted by {by_task} on joints {joints}" + "Cartesian IK task preempted", + task=self._name, + preempting_task=by_task, + joints=joints, ) def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool: @@ -328,6 +407,8 @@ def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool: True if accepted """ with self._lock: + if not self._active: + self._last_commanded_joints = None self._target_pose = pose # Store raw, convert to SE3 in compute() self._last_update_time = t_now self._active = True @@ -337,22 +418,22 @@ def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool: def start(self) -> None: """Activate the task (start accepting and outputting commands).""" with self._lock: + self._last_commanded_joints = None self._active = True - logger.info(f"CartesianIKTask {self._name} started") def stop(self) -> None: """Deactivate the task (stop outputting commands).""" with self._lock: self._active = False self._target_pose = None - logger.info(f"CartesianIKTask {self._name} stopped") + self._last_commanded_joints = None def clear(self) -> None: """Clear current target and deactivate.""" with self._lock: self._target_pose = None self._active = False - logger.info(f"CartesianIKTask {self._name} cleared") + self._last_commanded_joints = None def is_tracking(self) -> bool: """Check if actively receiving and outputting commands.""" @@ -390,6 +471,9 @@ def forward_kinematics(self, joint_positions: NDArray[np.float64]) -> pinocchio. class CartesianIKTaskParams(BaseConfig): control_ik: PinkControlIKConfig + timeout: float = 0.5 + max_joint_delta_deg: float = 15.0 + max_tracking_error_deg: float = 10.0 min_dt: FiniteFloat = 1e-4 max_dt: FiniteFloat = 0.05 @@ -401,6 +485,9 @@ def create_task(cfg: TaskConfig, hardware: object) -> CartesianIKTask: CartesianIKTaskConfig( joint_names=cfg.joint_names, priority=cfg.priority, + timeout=params.timeout, + max_joint_delta_deg=params.max_joint_delta_deg, + max_tracking_error_deg=params.max_tracking_error_deg, min_dt=params.min_dt, max_dt=params.max_dt, control_ik=params.control_ik, diff --git a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py index 4cd9cdb2a7..ed028abc6d 100644 --- a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py @@ -28,8 +28,8 @@ try: from pink import Configuration, solve_ik - from pink.limits import ConfigurationLimit, VelocityLimit - from pink.tasks import FrameTask, PostureTask + from pink.limits import ConfigurationLimit + from pink.tasks import DampingTask, FrameTask, PostureTask except ModuleNotFoundError as exc: raise ModuleNotFoundError( f"{_PINK_INSTALL_ERROR} Missing module: {exc.name}", @@ -40,9 +40,6 @@ from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake from dimos.protocol.service.spec import BaseConfig -# Pink's integration/QP boundary tolerance is small but larger than machine epsilon. -_POSITION_LIMIT_EPSILON_RAD = 1e-5 - class PinkControlIKConfig(BaseConfig): """Typed configuration for the control IK backend.""" @@ -55,6 +52,10 @@ class PinkControlIKConfig(BaseConfig): position_cost: FiniteFloat = Field(1.0, ge=0.0) orientation_cost: FiniteFloat = Field(1.0, ge=0.0) posture_cost: FiniteFloat = Field(1e-3, ge=0.0) + joint_centering_cost: FiniteFloat = Field(0.0, ge=0.0) + damping_cost: FiniteFloat = Field(0.0, ge=0.0) + position_limit_margin: FiniteFloat = Field(1e-3, ge=0.0) + seed_limit_tolerance: FiniteFloat = Field(1e-2, ge=0.0) reference_q: list[float] | None = None qpsolver_options: dict[str, FiniteFloat] = Field(default_factory=dict) @@ -89,8 +90,11 @@ class _PinkRuntime: configuration: Configuration frame_task: FrameTask posture_task: PostureTask | None + joint_centering_task: PostureTask | None + damping_task: DampingTask | None tasks: list[object] limits: list[object] + velocity_limits: NDArray[np.float64] class _PinkControlIKBuilder: @@ -150,9 +154,29 @@ def build(self) -> _PinkRuntime: gain=config.task_gain, ) posture_task = PostureTask(cost=config.posture_cost) if config.posture_cost > 0.0 else None + joint_centering_task = ( + PostureTask(cost=config.joint_centering_cost) + if config.joint_centering_cost > 0.0 + else None + ) + if joint_centering_task is not None: + joint_centering_task.set_target(self._build_joint_center_q(model, mapping, reference_q)) + damping_task = DampingTask(cost=config.damping_cost) if config.damping_cost > 0.0 else None tasks: list[object] = [frame_task] if posture_task is not None: tasks.append(posture_task) + if joint_centering_task is not None: + tasks.append(joint_centering_task) + if damping_task is not None: + tasks.append(damping_task) + + velocity_limits = np.asarray(model.velocityLimit, dtype=np.float64).copy() + if ( + velocity_limits.size != model.nv + or not np.all(np.isfinite(velocity_limits)) + or np.any(velocity_limits <= 0.0) + ): + raise ValueError("effective Pink velocity limits are invalid") return _PinkRuntime( config=config, @@ -164,8 +188,11 @@ def build(self) -> _PinkRuntime: configuration=configuration, frame_task=frame_task, posture_task=posture_task, + joint_centering_task=joint_centering_task, + damping_task=damping_task, tasks=tasks, limits=limits, + velocity_limits=velocity_limits, ) @staticmethod @@ -255,6 +282,22 @@ def _uncontrolled_ee_chain( joint_id = int(model.parents[joint_id]) return False + @staticmethod + def _build_joint_center_q( + model: pinocchio.Model, + mapping: _CoordinateMapping, + reference_q: NDArray[np.float64], + ) -> NDArray[np.float64]: + center_q = reference_q.copy() + for q_index, width in zip(mapping.q_indices, mapping.q_widths, strict=True): + if width != 1: + continue + lower = model.lowerPositionLimit[q_index] + upper = model.upperPositionLimit[q_index] + if np.isfinite(lower) and np.isfinite(upper): + center_q[q_index] = (lower + upper) / 2.0 + return center_q + @staticmethod def _validate_frame(model: pinocchio.Model, frame_name: str) -> int: if not model.existFrame(frame_name): @@ -302,7 +345,18 @@ def _apply_limits( model.velocityLimit[index] = limit for index in mapping.v_indices: model.velocityLimit[index] = min(model.velocityLimit[index], self._config.max_velocity) - return [ConfigurationLimit(model), VelocityLimit(model)] + margin = self._config.position_limit_margin + for q_index, width in zip(mapping.q_indices, mapping.q_widths, strict=True): + if width != 1: + continue + lower = model.lowerPositionLimit[q_index] + upper = model.upperPositionLimit[q_index] + if np.isfinite(lower) and np.isfinite(upper) and upper - lower <= 2.0 * margin: + raise ValueError("position limit margin leaves no valid joint range") + # Keep position bounds in the QP, but apply velocity limits by uniformly + # scaling the solution. Tiny per-tick velocity boxes can make ProxQP + # misclassify feasible differential IK problems as primal-infeasible. + return [ConfigurationLimit(model)] class PinkControlIK: @@ -339,7 +393,8 @@ def solve( configuration = runtime.configuration frame_task = runtime.frame_task try: - configuration.update(self._full_q(measured)) + solve_seed = self._project_position_limits(measured, "solve seed") + configuration.update(self._full_q(solve_seed)) frame_task.set_target(target) if runtime.posture_task is not None: runtime.posture_task.set_target(configuration.q.copy()) @@ -355,11 +410,12 @@ def solve( velocity = np.asarray(velocity, dtype=np.float64).reshape(-1) if velocity.size != runtime.model.nv or not np.all(np.isfinite(velocity)): raise IKControlRuntimeError("Pink produced an invalid velocity") + velocity = self._scale_velocity(velocity) configuration.integrate_inplace(velocity, dt) - candidate = self._project_controlled_positions(configuration.q, measured) - if candidate.size != measured.size or not np.all(np.isfinite(candidate)): + candidate = self._project_controlled_positions(configuration.q, solve_seed) + if candidate.size != solve_seed.size or not np.all(np.isfinite(candidate)): raise IKControlRuntimeError("Pink produced an invalid joint candidate") - candidate = self._clamp_position_limits(candidate) + candidate = self._project_position_limits(candidate, "candidate") return ControlIKResult(candidate, self._controlled_velocity(velocity)) except IKControlRuntimeError: raise @@ -405,10 +461,30 @@ def _controlled_velocity(self, velocity: NDArray[np.float64]) -> NDArray[np.floa [velocity[index] for index in self._runtime.mapping.v_indices], dtype=np.float64 ) - def _clamp_position_limits(self, candidate: NDArray[np.float64]) -> NDArray[np.float64]: + def _scale_velocity(self, velocity: NDArray[np.float64]) -> NDArray[np.float64]: + """Uniformly scale a Pink solution to preserve its joint-space direction.""" + max_ratio = float(np.max(np.abs(velocity) / self._runtime.velocity_limits)) + if max_ratio <= 1.0: + return velocity + return velocity / max_ratio + + def _project_position_limits( + self, + positions: NDArray[np.float64], + source: str, + ) -> NDArray[np.float64]: + """Clamp small boundary drift but reject materially out-of-limit states. + + Pink requires a valid configuration before it can solve. Floating-point + and one-tick integration drift within ``seed_limit_tolerance`` is + projected back inside the configured margin; larger violations remain + visible as runtime failures so model or feedback problems are not hidden. + """ runtime = self._runtime mapping = runtime.mapping - bounded = candidate.copy() + bounded = positions.copy() + margin = runtime.config.position_limit_margin + tolerance = runtime.config.seed_limit_tolerance for index, width in enumerate(mapping.q_widths): if width != 1: continue @@ -416,16 +492,20 @@ def _clamp_position_limits(self, candidate: NDArray[np.float64]) -> NDArray[np.f lower = runtime.model.lowerPositionLimit[q_index] upper = runtime.model.upperPositionLimit[q_index] value = bounded[index] - if value < lower: - if lower - value <= _POSITION_LIMIT_EPSILON_RAD: - bounded[index] = lower - else: - raise IKControlRuntimeError("Pink produced an out-of-bounds joint candidate") - elif value > upper: - if value - upper <= _POSITION_LIMIT_EPSILON_RAD: - bounded[index] = upper - else: - raise IKControlRuntimeError("Pink produced an out-of-bounds joint candidate") + joint_name = mapping.joint_names[index] + if np.isfinite(lower) and value < lower - tolerance: + raise IKControlRuntimeError( + f"Pink {source} for {joint_name} violates lower position limit: " + f"{value} < {lower}" + ) + if np.isfinite(upper) and value > upper + tolerance: + raise IKControlRuntimeError( + f"Pink {source} for {joint_name} violates upper position limit: " + f"{value} > {upper}" + ) + safe_lower = lower + margin if np.isfinite(lower) else -np.inf + safe_upper = upper - margin if np.isfinite(upper) else np.inf + bounded[index] = np.clip(value, safe_lower, safe_upper) return bounded diff --git a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py index de7adada09..08672e1fb6 100644 --- a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py @@ -15,7 +15,7 @@ from pathlib import Path import subprocess import sys -from typing import cast +from typing import Any, cast import numpy as np import pinocchio @@ -56,9 +56,9 @@ def _robot(path: Path) -> RobotModelConfig: ) -def _state(t_now: float, dt: float = 0.01) -> CoordinatorState: +def _state(t_now: float, dt: float = 0.01, position: float = 0.0) -> CoordinatorState: return CoordinatorState( - joints=JointStateSnapshot(joint_positions={"joint1": 0.0}), t_now=t_now, dt=dt + joints=JointStateSnapshot(joint_positions={"joint1": position}), t_now=t_now, dt=dt ) @@ -66,13 +66,17 @@ class _FakeControlIK: nq = 1 def __init__(self) -> None: - self.target: object | None = None + self.target: Any | None = None self.dt: float | None = None + self.increment = 0.0 + self.solve_seeds: list[np.ndarray] = [] - def solve(self, target: object, measured: np.ndarray, dt: float) -> ControlIKResult: + def solve(self, target: Any, measured: np.ndarray, dt: float) -> ControlIKResult: self.target = target self.dt = dt - return ControlIKResult(measured.copy(), np.zeros(1)) + self.solve_seeds.append(measured.copy()) + positions = measured + self.increment + return ControlIKResult(positions, positions - measured) def test_cartesian_pipeline_passes_se3_target_and_bounded_dt(tmp_path: Path, mocker) -> None: @@ -167,7 +171,7 @@ def test_cartesian_runtime_error_is_a_measured_state_hold( ) -> None: backend = _FakeControlIK() - def fail(target: object, measured: np.ndarray, dt: float) -> ControlIKResult: + def fail(target: Any, measured: np.ndarray, dt: float) -> ControlIKResult: raise IKControlRuntimeError("solver failed") monkeypatch.setattr(backend, "solve", fail) @@ -187,3 +191,70 @@ def fail(target: object, measured: np.ndarray, dt: float) -> ControlIKResult: hold = task.compute(_state(1.01)) assert hold is not None assert hold.positions == [0.0] + + +def test_cartesian_pipeline_accumulates_from_accepted_commands_while_feedback_tracks( + tmp_path: Path, mocker +) -> None: + backend = _FakeControlIK() + backend.increment = 0.01 + mocker.patch( + "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik", + return_value=backend, + ) + task = CartesianIKTask( + "cartesian", + CartesianIKTaskConfig( + joint_names=["joint1"], + control_ik=PinkControlIKConfig(robot_model=_robot(tmp_path / "unused.urdf")), + max_tracking_error_deg=10.0, + ), + ) + assert task.on_cartesian_command(PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), 1.0) + + first = task.compute(_state(1.01)) + second = task.compute(_state(1.02)) + + assert first is not None + assert second is not None + assert first.positions == pytest.approx([0.01]) + assert second.positions == pytest.approx([0.02]) + np.testing.assert_allclose(backend.solve_seeds, [[0.0], [0.01]]) + + +def test_cartesian_pipeline_rebases_when_command_outpaces_feedback( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch, mocker +) -> None: + backend = _FakeControlIK() + backend.increment = 0.1 + mocker.patch( + "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik", + return_value=backend, + ) + task = CartesianIKTask( + "cartesian", + CartesianIKTaskConfig( + joint_names=["joint1"], + control_ik=PinkControlIKConfig(robot_model=_robot(tmp_path / "unused.urdf")), + max_tracking_error_deg=5.0, + ), + ) + assert task.on_cartesian_command(PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), 1.0) + + first = task.compute(_state(1.01)) + second = task.compute(_state(1.02)) + + assert first is not None + assert second is not None + assert first.positions == pytest.approx([0.1]) + assert second.positions == pytest.approx([0.1]) + np.testing.assert_allclose(backend.solve_seeds, [[0.0], [0.0]]) + + def fail(target: Any, measured: np.ndarray, dt: float) -> ControlIKResult: + raise IKControlRuntimeError("solver failed") + + monkeypatch.setattr(backend, "solve", fail) + hold = task.compute(_state(1.03)) + + assert hold is not None + assert hold.positions == [0.0] diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py index f4d976d6f7..32801fb1cb 100644 --- a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py +++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py @@ -13,10 +13,11 @@ # limitations under the License. from pathlib import Path +from typing import Any import numpy as np from pink import Configuration -from pink.tasks import PostureTask +from pink.tasks import DampingTask, FrameTask, PostureTask import pytest from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import CartesianIKTaskConfig @@ -118,8 +119,16 @@ def test_pink_settings_use_finite_declarative_validation(tmp_path: Path) -> None with pytest.raises(ValueError, match="finite"): PinkControlIKConfig(robot_model=robot, max_velocity=np.inf) + with pytest.raises(ValueError, match="greater than or equal to 0"): + PinkControlIKConfig(robot_model=robot, damping_cost=-1e-3) with pytest.raises(ValueError, match="finite"): PinkControlIKConfig(robot_model=robot, qpsolver_options={"eps": np.nan}) + with pytest.raises(ValueError, match="greater than or equal to 0"): + PinkControlIKConfig(robot_model=robot, joint_centering_cost=-1e-3) + with pytest.raises(ValueError, match="greater than or equal to 0"): + PinkControlIKConfig(robot_model=robot, position_limit_margin=-1e-3) + with pytest.raises(ValueError, match="greater than or equal to 0"): + PinkControlIKConfig(robot_model=robot, seed_limit_tolerance=-1e-3) with pytest.raises(ValueError, match="ordered"): CartesianIKTaskConfig( joint_names=["joint1", "joint2"], @@ -142,7 +151,7 @@ def test_pink_prepares_xacro_with_package_paths_and_arguments( "xacro_args": {"dof": "2"}, } ) - prepared: dict[str, object] = {} + prepared: dict[str, Any] = {} def prepare( path: Path, @@ -192,6 +201,18 @@ def test_pink_validates_named_frame_and_exact_joint_mapping(tmp_path: Path) -> N ) +def test_pink_rejects_position_margin_that_eliminates_valid_range(tmp_path: Path) -> None: + model_path = _write_urdf(tmp_path) + + with pytest.raises(ValueError, match="margin leaves no valid joint range"): + create_pink_control_ik( + PinkControlIKConfig( + robot_model=_robot(model_path), + position_limit_margin=2.0, + ) + ) + + def test_pink_reanchors_measured_state_and_runs_one_frame_task_step( tmp_path: Path, monkeypatch: pytest.MonkeyPatch ) -> None: @@ -201,10 +222,10 @@ def test_pink_reanchors_measured_state_and_runs_one_frame_task_step( ) measured = np.array([0.3, 0.1]) target = backend.forward_kinematics(measured) - calls: list[tuple[Configuration, list[object], float, dict[str, object]]] = [] + calls: list[tuple[Configuration, list[Any], float, dict[str, Any]]] = [] def solve( - configuration: Configuration, tasks: list[object], dt: float, **kwargs: object + configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any ) -> np.ndarray: calls.append((configuration, tasks, dt, kwargs)) return np.zeros(configuration.model.nv) @@ -223,7 +244,7 @@ def solve( assert kwargs["damping"] == 1e-4 assert kwargs["eps"] == 1e-6 assert isinstance(kwargs["limits"], list) - assert len(kwargs["limits"]) == 2 + assert len(kwargs["limits"]) == 1 def test_pink_solver_dependency_failure_is_translated_to_runtime_error( @@ -231,9 +252,7 @@ def test_pink_solver_dependency_failure_is_translated_to_runtime_error( ) -> None: backend = create_pink_control_ik(PinkControlIKConfig(robot_model=_robot(_write_urdf(tmp_path)))) - def solve( - configuration: object, tasks: list[object], dt: float, **kwargs: object - ) -> np.ndarray: + def solve(configuration: Any, tasks: list[Any], dt: float, **kwargs: Any) -> np.ndarray: raise RuntimeError("solver dependency failed") monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) @@ -252,7 +271,7 @@ def test_pink_receives_pre_bounded_dt_unchanged( calls: list[float] = [] def solve( - configuration: Configuration, tasks: list[object], dt: float, **kwargs: object + configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any ) -> np.ndarray: calls.append(dt) return np.zeros(configuration.model.nv) @@ -269,10 +288,10 @@ def test_pink_posture_task_can_be_disabled(tmp_path: Path, monkeypatch: pytest.M backend = create_pink_control_ik( PinkControlIKConfig(robot_model=_robot(model_path), posture_cost=0.0) ) - calls: list[list[object]] = [] + calls: list[list[Any]] = [] def solve( - configuration: Configuration, tasks: list[object], dt: float, **kwargs: object + configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any ) -> np.ndarray: calls.append(tasks) return np.zeros(configuration.model.nv) @@ -284,6 +303,72 @@ def solve( assert calls and len(calls[0]) == 1 +def test_pink_joint_centering_task_targets_position_limit_midpoints( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + model_path = _write_urdf(tmp_path) + robot = _robot(model_path).model_copy( + update={ + "joint_limits_lower": [-1.0, -0.25], + "joint_limits_upper": [0.5, 0.75], + } + ) + backend = create_pink_control_ik( + PinkControlIKConfig( + robot_model=robot, + posture_cost=0.0, + joint_centering_cost=1e-3, + ) + ) + calls: list[list[Any]] = [] + + def solve( + configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any + ) -> np.ndarray: + calls.append(tasks) + return np.zeros(configuration.model.nv) + + monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) + measured = np.array([0.1, 0.2]) + backend.solve(backend.forward_kinematics(measured), measured, 0.01) + + assert len(calls) == 1 + assert len(calls[0]) == 2 + assert isinstance(calls[0][0], FrameTask) + centering_task = calls[0][1] + assert isinstance(centering_task, PostureTask) + np.testing.assert_allclose(centering_task.target_q, [-0.25, 0.25]) + + +def test_pink_damping_task_replaces_posture_for_low_motion_policy( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + model_path = _write_urdf(tmp_path) + backend = create_pink_control_ik( + PinkControlIKConfig( + robot_model=_robot(model_path), + posture_cost=0.0, + damping_cost=1e-3, + ) + ) + calls: list[list[Any]] = [] + + def solve( + configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any + ) -> np.ndarray: + calls.append(tasks) + return np.zeros(configuration.model.nv) + + monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) + measured = np.array([0.3, 0.1]) + backend.solve(backend.forward_kinematics(measured), measured, 0.01) + + assert len(calls) == 1 + assert len(calls[0]) == 2 + assert isinstance(calls[0][0], FrameTask) + assert isinstance(calls[0][1], DampingTask) + + def test_pink_rejects_uncontrolled_end_effector_chain_without_reference( tmp_path: Path, ) -> None: @@ -314,7 +399,7 @@ def test_continuous_joint_scalar_limits_fail_with_actionable_diagnostic( angle = np.array([3.0]) def solve( - configuration: Configuration, tasks: list[object], dt: float, **kwargs: object + configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any ) -> np.ndarray: return np.zeros(configuration.model.nv) @@ -329,7 +414,11 @@ def test_pink_applies_position_velocity_limits_and_finite_output( ) -> None: model_path = _write_urdf(tmp_path) robot = _robot(model_path).model_copy( - update={"joint_limits_lower": [-0.5, -0.25], "joint_limits_upper": [0.5, 0.25]} + update={ + "joint_limits_lower": [-0.5, -0.25], + "joint_limits_upper": [0.5, 0.25], + "velocity_limits": [0.1, 1.0], + } ) backend = create_pink_control_ik( PinkControlIKConfig(robot_model=robot, max_velocity=0.2), @@ -337,7 +426,7 @@ def test_pink_applies_position_velocity_limits_and_finite_output( solver_inputs: dict[str, np.ndarray] = {} def solve( - configuration: Configuration, tasks: list[object], dt: float, **kwargs: object + configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any ) -> np.ndarray: solver_inputs["lower_position"] = configuration.model.lowerPositionLimit.copy() solver_inputs["velocity"] = configuration.model.velocityLimit.copy() @@ -349,12 +438,37 @@ def solve( ) assert np.array_equal(solver_inputs["lower_position"][:2], np.array([-0.5, -0.25])) - assert np.all(solver_inputs["velocity"][:2] <= 0.2) + assert np.array_equal(solver_inputs["velocity"][:2], np.array([0.1, 0.2])) assert result.positions.shape == (2,) assert np.all(np.isfinite(result.positions)) -def test_pink_clamps_tiny_position_limit_overshoot( +def test_pink_uniformly_scales_solver_velocity_before_integration( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + model_path = _write_urdf(tmp_path) + robot = _robot(model_path).model_copy(update={"velocity_limits": [0.1, 1.0]}) + backend = create_pink_control_ik(PinkControlIKConfig(robot_model=robot, max_velocity=0.2)) + measured = np.array([0.3, 0.1]) + calls: list[dict[str, Any]] = [] + + def solve( + configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any + ) -> np.ndarray: + calls.append(kwargs) + return np.array([1.0, 0.5]) + + monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) + + result = backend.solve(backend.forward_kinematics(measured), measured, 0.01) + + assert np.allclose(result.velocity, [0.1, 0.05]) + assert np.allclose(result.positions, [0.301, 0.1005]) + assert isinstance(calls[0]["limits"], list) + assert len(calls[0]["limits"]) == 1 + + +def test_pink_projects_seed_and_solution_to_inward_position_limit_margin( tmp_path: Path, monkeypatch: pytest.MonkeyPatch ) -> None: model_path = _write_urdf(tmp_path) @@ -362,21 +476,36 @@ def test_pink_clamps_tiny_position_limit_overshoot( update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]} ) backend = create_pink_control_ik(PinkControlIKConfig(robot_model=robot)) - measured = np.array([1.22, 0.1]) + measured = np.array([1.221940718699932, 0.1]) + solver_seed: list[np.ndarray] = [] def solve( - configuration: object, tasks: list[object], dt: float, **kwargs: object + configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any ) -> np.ndarray: - return np.array([0.00013784674535, -0.2]) + solver_seed.append(configuration.q.copy()) + return np.array([0.5, -0.2]) monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) result = backend.solve(backend.forward_kinematics(measured), measured, 0.01) - assert np.array_equal(result.positions, np.array([1.22, 0.098])) - assert np.array_equal(result.velocity, np.array([0.00013784674535, -0.2])) + np.testing.assert_allclose(solver_seed, [[1.219, 0.1]]) + np.testing.assert_allclose(result.positions, [1.219, 0.098]) + np.testing.assert_allclose(result.velocity, [0.5, -0.2]) -def test_pink_rejects_material_position_limit_violation( +def test_pink_rejects_seed_beyond_position_limit_tolerance(tmp_path: Path) -> None: + model_path = _write_urdf(tmp_path) + robot = _robot(model_path).model_copy( + update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]} + ) + backend = create_pink_control_ik(PinkControlIKConfig(robot_model=robot)) + measured = np.array([1.231, 0.1]) + + with pytest.raises(IKControlRuntimeError, match="solve seed.*joint1"): + backend.solve(backend.forward_kinematics(measured), measured, 0.01) + + +def test_pink_rejects_candidate_beyond_position_limit_tolerance( tmp_path: Path, monkeypatch: pytest.MonkeyPatch ) -> None: model_path = _write_urdf(tmp_path) @@ -384,16 +513,14 @@ def test_pink_rejects_material_position_limit_violation( update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]} ) backend = create_pink_control_ik(PinkControlIKConfig(robot_model=robot)) - measured = np.array([1.22, 0.1]) + measured = np.array([1.2, 0.1]) - def solve( - configuration: object, tasks: list[object], dt: float, **kwargs: object - ) -> np.ndarray: - return np.array([0.01, -0.2]) + def solve(configuration: Any, tasks: list[Any], dt: float, **kwargs: Any) -> np.ndarray: + return np.array([10.0, 0.0]) monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve) - with pytest.raises(IKControlRuntimeError, match="out-of-bounds"): - backend.solve(backend.forward_kinematics(measured), measured, 0.01) + with pytest.raises(IKControlRuntimeError, match="candidate.*joint1"): + backend.solve(backend.forward_kinematics(measured), measured, 0.05) @pytest.mark.parametrize("legacy_field", ["backend", "ee_joint_id", "self_collision_enabled"]) diff --git a/dimos/control/tasks/eef_twist_task/eef_twist_task.py b/dimos/control/tasks/eef_twist_task/eef_twist_task.py index 0de24cd968..104ab1aed3 100644 --- a/dimos/control/tasks/eef_twist_task/eef_twist_task.py +++ b/dimos/control/tasks/eef_twist_task/eef_twist_task.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Measured-state end-effector twist control.""" +"""Command-integrating end-effector twist control.""" from __future__ import annotations @@ -22,16 +22,16 @@ import numpy as np import pinocchio -from pydantic import FiniteFloat from dimos.control.coordinator import TaskConfig from dimos.control.task import CoordinatorState, JointCommandOutput, ResourceClaim from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import ( CartesianIKTask, CartesianIKTaskConfig, + CartesianIKTaskParams, + append_gripper_position, + claim_with_gripper, ) -from dimos.control.tasks.cartesian_ik_task.pink_control_ik import PinkControlIKConfig -from dimos.protocol.service.spec import BaseConfig from dimos.utils.logging_config import setup_logger from dimos.utils.transform_utils import twist_to_numpy @@ -44,7 +44,7 @@ @dataclass class EEFTwistTaskConfig(CartesianIKTaskConfig): - """Configuration for measured-FK-relative EEF twist control.""" + """Configuration for command-relative EEF twist control.""" gripper_joint: str | None = None gripper_open_pos: float = 0.0 @@ -52,7 +52,7 @@ class EEFTwistTaskConfig(CartesianIKTaskConfig): class EEFTwistTask(CartesianIKTask): - """Cartesian task specialization whose target is prepared from a twist.""" + """Integrate twists from the last accepted command while the stream is active.""" _config: EEFTwistTaskConfig @@ -62,16 +62,10 @@ def __init__(self, name: str, config: EEFTwistTaskConfig) -> None: self._latest_twist: TwistStamped | None = None self._estopped = False self._gripper_target = config.gripper_open_pos + self._gripper_active = config.gripper_joint is not None def claim(self) -> ResourceClaim: - claim = super().claim() - if self._config.gripper_joint is None: - return claim - return ResourceClaim( - joints=claim.joints | frozenset([self._config.gripper_joint]), - priority=claim.priority, - mode=claim.mode, - ) + return claim_with_gripper(super().claim(), self._config.gripper_joint) def is_active(self) -> bool: with self._twist_lock: @@ -106,10 +100,14 @@ def on_ee_twist_command(self, twist: TwistStamped, t_now: float) -> bool: else: self._latest_twist = twist cleared = False + if cleared: + self._reset_command_state() if cleared and self._config.gripper_joint is None: super().clear() return True with self._lock: + if not self._active: + self._last_commanded_joints = None self._last_update_time = t_now self._active = True return True @@ -123,7 +121,10 @@ def on_gripper_command(self, msg: Bool, t_now: float) -> bool: self._gripper_target = ( self._config.gripper_closed_pos if msg.data else self._config.gripper_open_pos ) + self._gripper_active = True with self._lock: + if not self._active: + self._last_commanded_joints = None self._last_update_time = t_now self._active = True return True @@ -133,17 +134,19 @@ def set_estop(self, estopped: bool) -> None: self._estopped = estopped if estopped: self._latest_twist = None + self._gripper_active = False + if estopped: + super().clear() def compute(self, state: CoordinatorState) -> JointCommandOutput | None: output = super().compute(state) - if output is None or self._config.gripper_joint is None: - return output with self._twist_lock: gripper_target = self._gripper_target - return JointCommandOutput( - joint_names=[*output.joint_names, self._config.gripper_joint], - positions=[*(output.positions or []), gripper_target], - mode=output.mode, + gripper_joint = self._config.gripper_joint if self._gripper_active else None + return append_gripper_position( + output, + gripper_joint, + gripper_target, ) def _prepare_target( @@ -167,26 +170,26 @@ def _prepare_target( return pose def stop(self) -> None: + self._clear_inputs() + super().stop() + + def _clear_inputs(self) -> None: + """Discard twist and gripper commands owned by this specialization.""" with self._twist_lock: self._latest_twist = None - super().stop() + self._gripper_active = False def _on_timeout(self) -> None: with self._twist_lock: self._latest_twist = None def clear(self) -> None: - with self._twist_lock: - self._latest_twist = None + self._clear_inputs() super().clear() -class EEFTwistTaskParams(BaseConfig): +class EEFTwistTaskParams(CartesianIKTaskParams): timeout: float = 0.3 - max_joint_delta_deg: float = 15.0 - min_dt: FiniteFloat = 1e-4 - max_dt: FiniteFloat = 0.05 - control_ik: PinkControlIKConfig gripper_joint: str | None = None gripper_open_pos: float = 0.0 gripper_closed_pos: float = 0.0 @@ -201,6 +204,7 @@ def create_task(cfg: TaskConfig, hardware: object) -> EEFTwistTask: priority=cfg.priority, timeout=params.timeout, max_joint_delta_deg=params.max_joint_delta_deg, + max_tracking_error_deg=params.max_tracking_error_deg, min_dt=params.min_dt, max_dt=params.max_dt, control_ik=params.control_ik, diff --git a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py index 051b01d65d..c07bb26ca6 100644 --- a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py +++ b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py @@ -48,8 +48,10 @@ def __init__(self) -> None: self.nq = 3 self.fk_calls: list[np.ndarray] = [] self.solve_calls: list[FakePose] = [] + self.q_calls: list[np.ndarray] = [] self.dt_calls: list[float] = [] self.solution = np.array([0.01, 0.02, 0.03], dtype=np.float64) + self.increment: np.ndarray | None = None self.raise_runtime = False def forward_kinematics(self, q_current: NDArray[np.float64]) -> FakePose: @@ -60,8 +62,10 @@ def solve(self, pose: FakePose, q_current: NDArray[np.float64], dt: float) -> Co if self.raise_runtime: raise IKControlRuntimeError("synthetic solver failure") self.solve_calls.append(pose.copy()) + self.q_calls.append(q_current.copy()) self.dt_calls.append(dt) - return ControlIKResult(self.solution.copy(), self.solution - q_current) + solution = self.solution if self.increment is None else q_current + self.increment + return ControlIKResult(solution.copy(), solution - q_current) @pytest.fixture @@ -158,17 +162,23 @@ def test_ik_runtime_error_is_a_bounded_hold(task: EEFTwistTask, fake_ik: FakeIK) assert hold.positions == [0.0, 0.0, 0.0] -def test_integration_uses_current_fk_and_coordinator_dt( +def test_integration_uses_last_command_and_coordinator_dt_when_feedback_lags( task: EEFTwistTask, fake_ik: FakeIK ) -> None: assert task.on_ee_twist_command(_twist(1.0), t_now=1.0) + fake_ik.increment = np.array([0.01, 0.0, 0.0], dtype=np.float64) first = task.compute(_state(1.01, dt=0.01)) - fake_ik.solution = np.array([0.51, 0.0, 0.0], dtype=np.float64) - second = task.compute(_state(1.04, positions=[0.5, 0.0, 0.0], dt=0.01)) + second = task.compute(_state(1.02, dt=0.01)) assert first is not None assert second is not None + assert first.positions == pytest.approx([0.01, 0.0, 0.0]) + assert second.positions == pytest.approx([0.02, 0.0, 0.0]) + np.testing.assert_allclose( + fake_ik.q_calls, + np.array([[0.0, 0.0, 0.0], [0.01, 0.0, 0.0]]), + ) assert fake_ik.dt_calls == [0.02, 0.02] assert fake_ik.solve_calls[1].translation[0] > fake_ik.solve_calls[0].translation[0] @@ -225,7 +235,7 @@ def test_joint_delta_rejection_returns_a_hold(task: EEFTwistTask, fake_ik: FakeI assert rejected.positions == [0.0, 0.0, 0.0] -def test_timeout_and_zero_command_clear_then_next_nonzero_reseeds( +def test_timeout_and_zero_command_clear_then_next_nonzero_reseeds_from_measured( task: EEFTwistTask, fake_ik: FakeIK ) -> None: assert task.on_ee_twist_command(_twist(), t_now=1.0) @@ -237,11 +247,30 @@ def test_timeout_and_zero_command_clear_then_next_nonzero_reseeds( fake_ik.solution = np.array([1.01, 0.0, 0.0], dtype=np.float64) assert task.on_ee_twist_command(_twist(), t_now=2.0) assert task.compute(_state(2.01, positions=[1.0, 0.0, 0.0])) is not None + np.testing.assert_allclose(fake_ik.q_calls[-1], [1.0, 0.0, 0.0]) assert fake_ik.solve_calls[-1].translation[0] > 1.0 assert task.on_ee_twist_command(_twist(0.0), t_now=2.02) assert not task.is_active() + fake_ik.solution = np.array([2.01, 0.0, 0.0], dtype=np.float64) + assert task.on_ee_twist_command(_twist(), t_now=3.0) + assert task.compute(_state(3.01, positions=[2.0, 0.0, 0.0])) is not None + np.testing.assert_allclose(fake_ik.q_calls[-1], [2.0, 0.0, 0.0]) + + +def test_preemption_discards_last_commanded_solve_seed(task: EEFTwistTask, fake_ik: FakeIK) -> None: + fake_ik.increment = np.array([0.01, 0.0, 0.0], dtype=np.float64) + assert task.on_ee_twist_command(_twist(), t_now=1.0) + assert task.compute(_state(1.01)) is not None + + task.on_preempted("higher_priority", frozenset(["arm/joint1"])) + output = task.compute(_state(1.02, positions=[0.5, 0.0, 0.0])) + + assert output is not None + assert output.positions == pytest.approx([0.51, 0.0, 0.0]) + np.testing.assert_allclose(fake_ik.q_calls[-1], [0.5, 0.0, 0.0]) + @pytest.fixture def gripper_task(fake_ik: FakeIK) -> EEFTwistTask: @@ -288,6 +317,9 @@ def test_commands_during_estop_are_rejected(gripper_task: EEFTwistTask) -> None: assert not gripper_task.on_gripper_command(Bool(data=True), 1.0) gripper_task.set_estop(False) - output = gripper_task.compute(_state(2.0, positions=[0.1, 0.2, 0.3])) + assert gripper_task.compute(_state(2.0, positions=[0.1, 0.2, 0.3])) is None + + assert gripper_task.on_gripper_command(Bool(data=True), 2.1) + output = gripper_task.compute(_state(2.2, positions=[0.1, 0.2, 0.3])) assert output is not None - assert output.positions[-1] == 0.85 + assert output.positions[-1] == 0.0 diff --git a/dimos/control/tasks/teleop_task/teleop_task.py b/dimos/control/tasks/teleop_task/teleop_task.py index d7ba0709f5..e6bb46c4fb 100644 --- a/dimos/control/tasks/teleop_task/teleop_task.py +++ b/dimos/control/tasks/teleop_task/teleop_task.py @@ -12,38 +12,28 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Teleop cartesian control task with internal Pinocchio IK solver. - -Accepts streaming cartesian delta poses from teleoperation and computes -inverse kinematics internally to output joint commands. Deltas are applied -relative to the EE pose captured at engage time. - -Participates in joint-level arbitration. -""" +"""Engagement-relative teleop control through command-integrating Pink IK.""" from __future__ import annotations from dataclasses import dataclass -from pathlib import Path -import threading -from typing import TYPE_CHECKING, Any, Literal +from enum import Enum, auto +from typing import TYPE_CHECKING, Literal import numpy as np import pinocchio - -from dimos.control.task import ( - BaseControlTask, - ControlMode, - CoordinatorState, - JointCommandOutput, - ResourceClaim, -) -from dimos.manipulation.planning.kinematics.pinocchio_ik import ( - PinocchioIK, - check_joint_delta, - pose_to_se3, +from pydantic import Field, FiniteFloat + +from dimos.control.coordinator import TaskConfig +from dimos.control.task import CoordinatorState, JointCommandOutput, ResourceClaim +from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import ( + CartesianIKTask, + CartesianIKTaskConfig, + CartesianIKTaskParams, + append_gripper_position, + claim_with_gripper, ) -from dimos.protocol.service.spec import BaseConfig +from dimos.control.tasks.cartesian_ik_task.pink_control_ik import PinkControlIKConfig from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -56,337 +46,247 @@ logger = setup_logger() +class _EngagementState(Enum): + DISENGAGED = auto() + ENGAGED = auto() + WAITING_FOR_RELEASE = auto() + + +class TeleopControlIKConfig(PinkControlIKConfig): + """Pink control policy for engagement-relative arm teleoperation.""" + + max_velocity: FiniteFloat = Field(1.0, gt=0.0) + position_cost: FiniteFloat = Field(1.0, ge=0.0) + orientation_cost: FiniteFloat = Field(1.0, ge=0.0) + posture_cost: FiniteFloat = Field(0.0, ge=0.0) + joint_centering_cost: FiniteFloat = Field(1e-3, ge=0.0) + damping_cost: FiniteFloat = Field(1e-3, ge=0.0) + + @dataclass -class TeleopIKTaskConfig: - """Configuration for teleop IK task. - - Attributes: - joint_names: List of joint names this task controls (must match model DOF) - model_path: Path to URDF or MJCF file for IK solver - ee_joint_id: End-effector joint ID in the kinematic chain - priority: Priority for arbitration (higher wins) - timeout: If no command received for this many seconds, go inactive (0 = never) - max_joint_delta_deg: Maximum allowed joint change per tick (safety limit) - hand: "left" or "right" — which controller's primary button to listen to - gripper_joint: Optional joint name for the gripper (e.g. "arm/gripper"). - gripper_open_pos: Gripper position (adapter units) at trigger value 0.0 (no press). - gripper_closed_pos: Gripper position (adapter units) at trigger value 1.0 (full press). - """ - - joint_names: list[str] - model_path: str | Path - ee_joint_id: int - priority: int = 10 - timeout: float = 0.5 - max_joint_delta_deg: float = 5.0 # ~500°/s at 100Hz +class TeleopIKTaskConfig(CartesianIKTaskConfig): + """Configuration for engagement-relative teleop IK.""" + + max_joint_delta_deg: float = 5.0 hand: Literal["left", "right"] | None = None gripper_joint: str | None = None gripper_open_pos: float = 0.0 gripper_closed_pos: float = 0.0 -class TeleopIKTask(BaseControlTask): - """Teleop cartesian control task with internal Pinocchio IK solver. - - Accepts streaming cartesian delta poses via on_cartesian_command() and computes IK - internally to output joint commands. Deltas are applied relative to the EE pose - captured at engage time (first compute). - - Uses current joint state from CoordinatorState as IK warm-start for fast convergence. - Outputs JointCommandOutput and participates in joint-level arbitration. - - Example: - >>> from dimos.utils.data import get_data - >>> piper_path = get_data("piper_description") - >>> task = TeleopIKTask( - ... name="teleop_arm", - ... config=TeleopIKTaskConfig( - ... joint_names=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"], - ... model_path=piper_path / "mujoco_model" / "piper_no_gripper_description.xml", - ... ee_joint_id=6, - ... priority=10, - ... timeout=0.5, - ... hand="right", - ... ), - ... ) - >>> coordinator.add_task(task) - >>> task.start() - >>> - >>> # From teleop callback: - >>> task.on_cartesian_command(delta_pose, t_now=time.perf_counter()) - """ +class TeleopIKTask(CartesianIKTask): + """Cartesian IK specialization for engagement-relative teleoperation.""" + + _config: TeleopIKTaskConfig def __init__(self, name: str, config: TeleopIKTaskConfig) -> None: - """Initialize teleop IK task. - - Args: - name: Unique task name - config: Task configuration - """ - if not config.joint_names: - raise ValueError(f"TeleopIKTask '{name}' requires at least one joint") - if not config.model_path: - raise ValueError(f"TeleopIKTask '{name}' requires model_path for IK solver") if config.hand not in ("left", "right"): raise ValueError(f"TeleopIKTask '{name}' requires hand='left' or 'right'") - - self._name = name - self._config = config - self._joint_names = frozenset(config.joint_names) - self._joint_names_list = list(config.joint_names) - self._num_joints = len(config.joint_names) - - # Create IK solver from model - self._ik = PinocchioIK.from_model_path(config.model_path, config.ee_joint_id) - - # Validate DOF matches joint names - if self._ik.nq != self._num_joints: - logger.warning( - f"TeleopIKTask {name}: model DOF ({self._ik.nq}) != " - f"joint_names count ({self._num_joints})" - ) - - # Thread-safe target state - self._lock = threading.Lock() - self._target_pose: Pose | PoseStamped | None = None - self._last_update_time: float = 0.0 - self._active = False - self._estopped = False - - # Initial EE pose for delta application + super().__init__(name, config) self._initial_ee_pose: pinocchio.SE3 | None = None - self._prev_primary: bool = False - - self._gripper_target: float = config.gripper_open_pos - - logger.info( - f"TeleopIKTask {name} initialized with model: {config.model_path}, " - f"ee_joint_id={config.ee_joint_id}, joints={config.joint_names}" - ) + self._engagement = _EngagementState.DISENGAGED + self._primary_down = False + self._estopped = False + self._gripper_target = config.gripper_open_pos + self._gripper_active = config.gripper_joint is not None def claim(self) -> ResourceClaim: - """Declare resource requirements.""" - joints = self._joint_names - if self._config.gripper_joint: - joints = joints | frozenset([self._config.gripper_joint]) - return ResourceClaim( - joints=joints, - priority=self._config.priority, - mode=ControlMode.SERVO_POSITION, - ) + """Claim arm joints and the optional gripper joint.""" + return claim_with_gripper(super().claim(), self._config.gripper_joint) def is_active(self) -> bool: - """Check if task should run this tick.""" + """Run only when a non-E-STOPped pose target is active.""" with self._lock: return not self._estopped and self._active and self._target_pose is not None + def is_tracking(self) -> bool: + """Report whether teleop currently participates in control.""" + return self.is_active() + def set_estop(self, estopped: bool) -> None: - """Latch/clear E-STOP. On latch, disengage and drop the target so the - task goes inert (is_active() False → compute() is skipped).""" + """Latch or clear E-STOP without retaining replayable commands.""" with self._lock: self._estopped = estopped if estopped: + self._engagement = _EngagementState.WAITING_FOR_RELEASE self._active = False self._target_pose = None self._initial_ee_pose = None + self._last_commanded_joints = None + self._gripper_active = False + else: + self._engagement = ( + _EngagementState.WAITING_FOR_RELEASE + if self._primary_down + else _EngagementState.DISENGAGED + ) - def compute(self, state: CoordinatorState) -> JointCommandOutput | None: - """Compute IK and output joint positions. - - Args: - state: Current coordinator state (contains joint positions for IK warm-start) + def _prepare_target( + self, + state: CoordinatorState, + q_current: NDArray[np.float64], + dt: float, + ) -> pinocchio.SE3 | None: + """Compose the controller delta with the measured engagement baseline.""" + delta = super()._prepare_target(state, q_current, dt) + if delta is None: + return None - Returns: - JointCommandOutput with positions, or None if inactive/timed out/IK failed - """ with self._lock: - if not self._active or self._target_pose is None: + if self._estopped or self._target_pose is None: return None + baseline = self._initial_ee_pose - # Timeout safety: stop if teleop stream drops - if self._config.timeout > 0: - time_since_update = state.t_now - self._last_update_time - if time_since_update > self._config.timeout: - logger.warning( - f"TeleopIKTask {self._name} timed out " - f"(no update for {time_since_update:.3f}s)" - ) - self._target_pose = None - self._active = False - return None - raw_pose = self._target_pose - - # Convert to SE3 right before use - delta_se3 = pose_to_se3(raw_pose) - # Capture initial EE pose if not set (first command after engage) - with self._lock: - need_capture = self._initial_ee_pose is None - - if need_capture: - q_current = self._get_current_joints(state) - if q_current is None: - logger.debug( - f"TeleopIKTask {self._name}: cannot capture initial pose, joint state unavailable" - ) + if baseline is None: + captured = self.forward_kinematics(q_current) + values = np.concatenate((captured.translation, captured.rotation.reshape(-1))) + if not np.all(np.isfinite(values)): return None - initial_pose = self._ik.forward_kinematics(q_current) with self._lock: - self._initial_ee_pose = initial_pose - - # Apply delta to initial pose: target = initial + delta - with self._lock: - if self._initial_ee_pose is None: - return None - target_pose = pinocchio.SE3( - delta_se3.rotation @ self._initial_ee_pose.rotation, - self._initial_ee_pose.translation + delta_se3.translation, - ) - - # Get current joint positions for IK warm-start - q_current = self._get_current_joints(state) - if q_current is None: - logger.debug(f"TeleopIKTask {self._name}: missing joint state for IK warm-start") - return None - - # Compute IK - q_solution, converged, final_error = self._ik.solve(target_pose, q_current) - # Use the solution even if it didn't fully converge - if not converged: - logger.debug( - f"TeleopIKTask {self._name}: IK did not converge " - f"(error={final_error:.4f}), using partial solution" - ) - # Safety: reject if any joint would jump too far in one tick - if not check_joint_delta(q_solution, q_current, self._config.max_joint_delta_deg): - logger.warning( - f"TeleopIKTask {self._name}: joint delta exceeds " - f"{self._config.max_joint_delta_deg}°, rejecting solution" - ) - return None - - joint_names = list(self._joint_names_list) - positions = q_solution.flatten().tolist() + if self._estopped or self._target_pose is None: + return None + if self._initial_ee_pose is None: + self._initial_ee_pose = captured.copy() + baseline = self._initial_ee_pose - # Append gripper joint if configured — routed to ConnectedHardware by tick loop - if self._config.gripper_joint: - with self._lock: - gripper_pos = self._gripper_target - joint_names.append(self._config.gripper_joint) - positions.append(gripper_pos) - - return JointCommandOutput( - joint_names=joint_names, - positions=positions, - mode=ControlMode.SERVO_POSITION, + target = pinocchio.SE3( + delta.rotation @ baseline.rotation, + baseline.translation + delta.translation, ) + values = np.concatenate((target.translation, target.rotation.reshape(-1))) + if not np.all(np.isfinite(values)): + return None + return target - def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.floating[Any]] | None: - """Get current joint positions from coordinator state.""" - positions = [] - for joint_name in self._joint_names_list: - pos = state.joints.get_position(joint_name) - if pos is None: + def compute(self, state: CoordinatorState) -> JointCommandOutput | None: + """Run the inherited Pink solve and append the optional gripper target.""" + output = super().compute(state) + with self._lock: + if self._estopped: return None - positions.append(pos) - return np.array(positions) - - def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: - """Handle preemption by higher-priority task. - - Args: - by_task: Name of preempting task - joints: Joints that were preempted - """ - if joints & self._joint_names: - logger.warning(f"TeleopIKTask {self._name} preempted by {by_task} on joints {joints}") + gripper_target = self._gripper_target + gripper_joint = self._config.gripper_joint if self._gripper_active else None + return append_gripper_position(output, gripper_joint, gripper_target) def on_buttons(self, msg: Buttons) -> bool: - """Press-and-hold engage: hold primary button to track, release to stop.""" + """Use the configured primary button as press-and-hold engagement.""" is_left = self._config.hand == "left" primary = msg.left_primary if is_left else msg.right_primary + trigger = msg.left_trigger_analog if is_left else msg.right_trigger_analog - if primary and not self._prev_primary: - logger.info(f"TeleopIKTask {self._name}: engage") - with self._lock: + with self._lock: + was_primary_down = self._primary_down + self._primary_down = primary + if self._estopped: + return False + if self._engagement is _EngagementState.WAITING_FOR_RELEASE: + if not primary: + self._engagement = _EngagementState.DISENGAGED + elif ( + self._engagement is _EngagementState.DISENGAGED and primary and not was_primary_down + ): + self._engagement = _EngagementState.ENGAGED + self._active = False + self._target_pose = None self._initial_ee_pose = None - elif not primary and self._prev_primary: - logger.info(f"TeleopIKTask {self._name}: disengage") - with self._lock: + self._last_commanded_joints = None + elif self._engagement is _EngagementState.ENGAGED and not primary: + self._engagement = _EngagementState.DISENGAGED + self._active = False self._target_pose = None self._initial_ee_pose = None - self._prev_primary = primary + self._last_commanded_joints = None - if self._config.gripper_joint: - trigger = msg.left_trigger_analog if is_left else msg.right_trigger_analog + if self._config.gripper_joint is not None: self.on_gripper_trigger(trigger) - return True def on_teleop_buttons(self, msg: Buttons, t_now: float) -> bool: - """Uniform stream handler; ``on_buttons`` predates the (msg, t_now) contract.""" + """Uniform stream handler for broadcast controller buttons.""" return self.on_buttons(msg) def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool: - """Handle incoming cartesian command (delta pose from teleop)""" + """Accept an engagement-relative pose delta only while its button is held.""" with self._lock: - self._target_pose = pose # Store raw, convert to SE3 in compute() + if self._estopped or self._engagement is not _EngagementState.ENGAGED: + return False + if not self._active: + self._last_commanded_joints = None + self._target_pose = pose self._last_update_time = t_now self._active = True - return True def on_gripper_trigger(self, value: float, _t_now: float = 0.0) -> bool: - """Map analog trigger (0-1) to gripper position""" - if not self._config.gripper_joint: + """Map an analog trigger value onto the configured gripper range.""" + if self._config.gripper_joint is None or not np.isfinite(value): return False - clamped = max(0.0, min(1.0, value)) - pos = ( + position = ( self._config.gripper_open_pos + (self._config.gripper_closed_pos - self._config.gripper_open_pos) * clamped ) - with self._lock: - self._gripper_target = pos - + if self._estopped: + return False + self._gripper_target = position + self._gripper_active = True return True - def start(self) -> None: - """Activate the task (start accepting and outputting commands).""" - with self._lock: - self._active = True - logger.info(f"TeleopIKTask {self._name} started") + def _on_timeout(self) -> None: + """Discard the baseline while the parent holds the task lock.""" + self._initial_ee_pose = None + self._engagement = ( + _EngagementState.WAITING_FOR_RELEASE + if self._primary_down + else _EngagementState.DISENGAGED + ) def stop(self) -> None: - """Deactivate the task (stop outputting commands).""" + """Stop output and discard engagement-relative state.""" + super().stop() + self._reset_engagement_state() + + def _reset_engagement_state(self) -> None: + """Discard state owned specifically by engagement-relative teleop.""" with self._lock: - self._active = False - logger.info(f"TeleopIKTask {self._name} stopped") + self._initial_ee_pose = None + self._engagement = _EngagementState.DISENGAGED + self._primary_down = False + self._gripper_active = False + def clear(self) -> None: + """Clear output and discard engagement-relative state.""" + super().clear() + self._reset_engagement_state() -class TeleopIKTaskParams(BaseConfig): - model_path: str | Path - ee_joint_id: int = 6 + +class TeleopIKTaskParams(CartesianIKTaskParams): + control_ik: TeleopControlIKConfig hand: Literal["left", "right"] | None = None + max_joint_delta_deg: float = 5.0 gripper_joint: str | None = None gripper_open_pos: float = 0.0 gripper_closed_pos: float = 0.0 - max_joint_delta_deg: float = TeleopIKTaskConfig.max_joint_delta_deg -def create_task(cfg: Any, hardware: Any) -> TeleopIKTask: +def create_task(cfg: TaskConfig, hardware: object) -> TeleopIKTask: + """Create a Pink-backed teleop task from declarative configuration.""" params = TeleopIKTaskParams.model_validate(cfg.params) return TeleopIKTask( cfg.name, TeleopIKTaskConfig( joint_names=cfg.joint_names, - model_path=params.model_path, - ee_joint_id=params.ee_joint_id, + control_ik=params.control_ik, priority=cfg.priority, + timeout=params.timeout, + max_joint_delta_deg=params.max_joint_delta_deg, + max_tracking_error_deg=params.max_tracking_error_deg, + min_dt=params.min_dt, + max_dt=params.max_dt, hand=params.hand, gripper_joint=params.gripper_joint, gripper_open_pos=params.gripper_open_pos, gripper_closed_pos=params.gripper_closed_pos, - max_joint_delta_deg=params.max_joint_delta_deg, ), ) diff --git a/dimos/control/tasks/teleop_task/test_teleop_task.py b/dimos/control/tasks/teleop_task/test_teleop_task.py new file mode 100644 index 0000000000..3d39730c74 --- /dev/null +++ b/dimos/control/tasks/teleop_task/test_teleop_task.py @@ -0,0 +1,302 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path + +import numpy as np +from numpy.typing import NDArray +import pinocchio +import pytest +from pytest_mock import MockerFixture + +from dimos.control.coordinator import TaskConfig +from dimos.control.task import CoordinatorState, JointStateSnapshot +from dimos.control.tasks.cartesian_ik_task.pink_control_ik import ( + ControlIKResult, + IKControlRuntimeError, + PinkControlIKConfig, +) +from dimos.control.tasks.teleop_task.teleop_task import ( + TeleopIKTask, + TeleopIKTaskConfig, + create_task, +) +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.teleop.quest.quest_types import Buttons + + +@dataclass +class _FakePinkIK: + nq: int = 2 + + def __post_init__(self) -> None: + self.fk_calls: list[NDArray[np.float64]] = [] + self.solve_calls: list[tuple[pinocchio.SE3, NDArray[np.float64], float]] = [] + self.solution = np.array([0.01, 0.02], dtype=np.float64) + self.raise_runtime = False + + def forward_kinematics(self, q: NDArray[np.float64]) -> pinocchio.SE3: + self.fk_calls.append(q.copy()) + return pinocchio.SE3( + pinocchio.exp3(np.array([0.0, 0.0, 0.2], dtype=np.float64)), + np.array([q[0], q[1], 0.3], dtype=np.float64), + ) + + def solve( + self, + target: pinocchio.SE3, + measured: NDArray[np.float64], + dt: float, + ) -> ControlIKResult: + if self.raise_runtime: + raise IKControlRuntimeError("synthetic Pink failure") + self.solve_calls.append((target.copy(), measured.copy(), dt)) + return ControlIKResult(self.solution.copy(), self.solution - measured) + + +def _robot(path: Path) -> RobotModelConfig: + return RobotModelConfig( + name="arm", + model_path=path, + base_pose=PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), + joint_names=["joint1", "joint2"], + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=("joint1", "joint2"), + base_link="base", + tip_link="tool", + ) + ], + joint_name_mapping={"arm/joint1": "joint1", "arm/joint2": "joint2"}, + home_joints=[0.0, 0.0], + ) + + +def _pink_config(path: Path) -> PinkControlIKConfig: + return PinkControlIKConfig.model_validate({"robot_model": _robot(path)}) + + +def _state( + t_now: float, + positions: tuple[float, ...] = (0.0, 0.0), + *, + dt: float = 0.01, +) -> CoordinatorState: + return CoordinatorState( + joints=JointStateSnapshot( + joint_positions={ + f"arm/joint{index + 1}": position for index, position in enumerate(positions) + } + ), + t_now=t_now, + dt=dt, + ) + + +def _delta( + position: tuple[float, float, float] = (0.1, -0.2, 0.4), + angle: float = 0.3, +) -> PoseStamped: + quaternion = pinocchio.Quaternion(pinocchio.exp3(np.array([0.0, 0.0, angle]))) + return PoseStamped( + position=list(position), + orientation=[quaternion.x, quaternion.y, quaternion.z, quaternion.w], + ) + + +def _buttons(primary: bool) -> Buttons: + buttons = Buttons() + buttons.right_primary = primary + return buttons + + +def _engage(task: TeleopIKTask, t_now: float = 0.0) -> None: + assert task.on_teleop_buttons(_buttons(True), t_now) + + +@pytest.fixture +def fake_ik(mocker: MockerFixture) -> _FakePinkIK: + backend = _FakePinkIK() + mocker.patch( + "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik", + return_value=backend, + ) + return backend + + +@pytest.fixture +def task(tmp_path: Path, fake_ik: _FakePinkIK) -> TeleopIKTask: + return TeleopIKTask( + "teleop_arm", + TeleopIKTaskConfig( + joint_names=["arm/joint1", "arm/joint2"], + control_ik=_pink_config(tmp_path / "unused.urdf"), + hand="right", + min_dt=0.02, + max_dt=0.03, + max_joint_delta_deg=5.0, + ), + ) + + +@pytest.fixture +def gripper_task(tmp_path: Path, fake_ik: _FakePinkIK) -> TeleopIKTask: + return TeleopIKTask( + "teleop_arm", + TeleopIKTaskConfig( + joint_names=["arm/joint1", "arm/joint2"], + control_ik=_pink_config(tmp_path / "unused.urdf"), + hand="right", + gripper_joint="arm/gripper", + gripper_open_pos=0.8, + gripper_closed_pos=0.0, + ), + ) + + +def test_delta_is_composed_with_one_measured_engagement_baseline( + task: TeleopIKTask, fake_ik: _FakePinkIK +) -> None: + _engage(task) + assert task.on_cartesian_command(_delta(), t_now=1.0) + + first = task.compute(_state(1.01, (1.0, 2.0), dt=1.0)) + second = task.compute(_state(1.02, (1.5, 2.5), dt=0.001)) + + assert first is not None + assert second is not None + assert len(fake_ik.fk_calls) == 1 + first_target, first_measured, first_dt = fake_ik.solve_calls[0] + second_target, second_measured, second_dt = fake_ik.solve_calls[1] + baseline_rotation = pinocchio.exp3(np.array([0.0, 0.0, 0.2])) + delta_rotation = pinocchio.exp3(np.array([0.0, 0.0, 0.3])) + assert np.allclose(first_target.translation, [1.1, 1.8, 0.7]) + assert np.allclose(first_target.rotation, delta_rotation @ baseline_rotation) + assert np.allclose(second_target.translation, first_target.translation) + assert np.allclose(first_measured, [1.0, 2.0]) + assert np.allclose(second_measured, [1.5, 2.5]) + assert (first_dt, second_dt) == (0.03, 0.02) + + +def test_release_timeout_stop_and_clear_force_fresh_baselines( + task: TeleopIKTask, fake_ik: _FakePinkIK +) -> None: + pressed = Buttons() + pressed.right_primary = True + released = Buttons() + + assert task.on_teleop_buttons(pressed, 0.0) + assert task.on_cartesian_command(_delta(), 1.0) + assert task.compute(_state(1.01)) is not None + assert task.on_teleop_buttons(released, 1.02) + assert not task.is_active() + + assert task.on_teleop_buttons(pressed, 2.0) + assert task.on_cartesian_command(_delta(), 2.0) + assert task.compute(_state(2.01, (0.1, 0.2))) is not None + np.testing.assert_allclose(fake_ik.solve_calls[-1][1], [0.1, 0.2]) + assert task.compute(_state(3.0, (0.1, 0.2))) is None + + assert task.on_teleop_buttons(released, 3.1) + assert task.on_teleop_buttons(pressed, 3.2) + assert task.on_cartesian_command(_delta(), 4.0) + assert task.compute(_state(4.01, (0.2, 0.3))) is not None + np.testing.assert_allclose(fake_ik.solve_calls[-1][1], [0.2, 0.3]) + task.stop() + task.start() + assert task.on_teleop_buttons(released, 4.1) + assert task.on_teleop_buttons(pressed, 4.2) + assert task.on_cartesian_command(_delta(), 5.0) + assert task.compute(_state(5.01, (0.3, 0.4))) is not None + np.testing.assert_allclose(fake_ik.solve_calls[-1][1], [0.3, 0.4]) + task.clear() + assert not task.is_active() + assert not task.on_cartesian_command(_delta(), 6.0) + + +def test_estop_rejects_commands_and_never_replays_them( + gripper_task: TeleopIKTask, fake_ik: _FakePinkIK +) -> None: + _engage(gripper_task) + assert gripper_task.on_cartesian_command(_delta(), 1.0) + assert gripper_task.compute(_state(1.01)) is not None + + gripper_task.set_estop(True) + assert not gripper_task.on_cartesian_command(_delta((9.0, 0.0, 0.0)), 2.0) + assert not gripper_task.on_gripper_trigger(1.0) + assert not gripper_task.is_active() + + gripper_task.set_estop(False) + assert gripper_task.compute(_state(2.01)) is None + assert not gripper_task.on_cartesian_command(_delta(), 2.5) + assert gripper_task.on_teleop_buttons(_buttons(False), 2.6) + assert gripper_task.on_teleop_buttons(_buttons(True), 2.7) + assert gripper_task.on_cartesian_command(_delta(), 3.0) + assert gripper_task.compute(_state(3.01, (0.2, 0.3))) is not None + assert len(fake_ik.fk_calls) == 2 + + +def test_gripper_claim_interpolation_and_hold_output( + gripper_task: TeleopIKTask, fake_ik: _FakePinkIK +) -> None: + _engage(gripper_task) + assert gripper_task.on_gripper_trigger(0.25) + assert gripper_task.on_cartesian_command(_delta(), 1.0) + fake_ik.raise_runtime = True + + output = gripper_task.compute(_state(1.01, (0.4, 0.5))) + + assert gripper_task.claim().joints == frozenset({"arm/joint1", "arm/joint2", "arm/gripper"}) + assert output is not None + assert output.joint_names == ["arm/joint1", "arm/joint2", "arm/gripper"] + assert output.positions == pytest.approx([0.4, 0.5, 0.6]) + + +def test_pose_is_rejected_before_engage_and_after_release(task: TeleopIKTask) -> None: + assert not task.on_cartesian_command(_delta(), 1.0) + + _engage(task, 2.0) + assert task.on_cartesian_command(_delta(), 2.1) + assert task.on_teleop_buttons(_buttons(False), 2.2) + + assert not task.on_cartesian_command(_delta(), 2.3) + + +def test_factory_requires_pink_configuration_and_matching_model(tmp_path: Path) -> None: + legacy = TaskConfig( + name="teleop", + type="teleop_ik", + joint_names=["arm/joint1", "arm/joint2"], + params={"model_path": "legacy.xml", "ee_joint_id": 2, "hand": "right"}, + ) + with pytest.raises(ValueError, match="control_ik"): + create_task(legacy, {}) + + mismatched = TaskConfig( + name="teleop", + type="teleop_ik", + joint_names=["wrong/joint1", "wrong/joint2"], + params={ + "control_ik": {"robot_model": _robot(tmp_path / "unused.urdf")}, + "hand": "right", + }, + ) + with pytest.raises(ValueError, match="task joints must match"): + create_task(mismatched, {}) diff --git a/dimos/hardware/manipulators/galaxea_a1z/adapter.py b/dimos/hardware/manipulators/galaxea_a1z/adapter.py index 00eb9f13c6..1745b1692f 100644 --- a/dimos/hardware/manipulators/galaxea_a1z/adapter.py +++ b/dimos/hardware/manipulators/galaxea_a1z/adapter.py @@ -191,6 +191,8 @@ def _create_robot(self) -> ArmRobot: zero_gravity_mode=self._config.teaching is not None, control_freq_hz=_SDK_CONTROL_FREQ_HZ, urdf_path=self._config.urdf_path, + default_kp=np.asarray(self._config.default_kp, dtype=float), + default_kd=np.asarray(self._config.default_kd, dtype=float), with_gripper=gripper is not None, gripper_max_torque=gripper.max_torque if gripper else 0.5, ) diff --git a/dimos/hardware/manipulators/galaxea_a1z/config.py b/dimos/hardware/manipulators/galaxea_a1z/config.py index f3d52f62af..f73a410541 100644 --- a/dimos/hardware/manipulators/galaxea_a1z/config.py +++ b/dimos/hardware/manipulators/galaxea_a1z/config.py @@ -35,7 +35,7 @@ def _validate_optional_path( @attrs.frozen(slots=False) class A1ZGripperConfig: - """G1Z gripper configuration.""" + """A1Z gripper configuration.""" max_torque: float = attrs.field( default=0.5, @@ -71,6 +71,8 @@ class A1ZConfig: attrs.validators.le(1.0), ), ) + default_kp: tuple[float, ...] = (80.0, 80.0, 80.0, 50.0, 20.0, 20.0) + default_kd: tuple[float, ...] = (3.0, 3.0, 3.0, 0.7, 0.4, 0.4) urdf_path: str | Path | None = attrs.field( default=None, validator=_validate_optional_path, diff --git a/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py b/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py index 956469aaf8..560ac1118e 100644 --- a/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py +++ b/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py @@ -79,8 +79,8 @@ def __init__(self, **factory_kwargs: Any) -> None: self._running = False self._estopped = False self._bus = _FakeBus() - self._default_kp = np.array([30.0, 30.0, 30.0, 20.0, 5.0, 5.0]) - self._default_kd = np.array([1.0, 1.0, 1.0, 0.5, 0.5, 0.5]) + self._default_kp = np.asarray(factory_kwargs["default_kp"], dtype=float) + self._default_kd = np.asarray(factory_kwargs["default_kd"], dtype=float) self.actions: list[Any] = [] self.gravity_factor_history: list[float] = [] self.gravity_comp_factor = float(factory_kwargs["gravity_comp_factor"]) @@ -250,6 +250,8 @@ def _connected_adapter(module: ModuleType, **kwargs: Any) -> tuple[Any, _FakeArm config = A1ZConfig( gravity_comp_factor=kwargs.pop("gravity_comp_factor", 1.0), urdf_path=kwargs.pop("urdf_path", None), + default_kp=kwargs.pop("default_kp", (80.0, 80.0, 80.0, 50.0, 20.0, 20.0)), + default_kd=kwargs.pop("default_kd", (3.0, 3.0, 3.0, 0.7, 0.4, 0.4)), gripper=gripper, teaching=teaching, ) @@ -269,6 +271,18 @@ def test_connect_opens_bus_without_powering_motors( assert not adapter.read_enabled() +def test_connect_forwards_configured_arm_gains_to_sdk( + a1z_adapter_module: ModuleType, +) -> None: + kp = (70.0, 70.0, 70.0, 40.0, 15.0, 15.0) + kd = (2.5, 2.5, 2.5, 0.6, 0.3, 0.3) + + _, robot = _connected_adapter(a1z_adapter_module, default_kp=kp, default_kd=kd) + + assert robot.factory_kwargs["default_kp"] == pytest.approx(kp) + assert robot.factory_kwargs["default_kd"] == pytest.approx(kd) + + def test_safe_start_stages_measured_hold_before_gravity_feedforward( a1z_adapter_module: ModuleType, ) -> None: @@ -507,6 +521,15 @@ def test_gripper_round_trips_meters_to_normalized( assert robot.gripper_fraction == pytest.approx(1.0) +def test_connect_applies_configured_gripper_force( + a1z_adapter_module: ModuleType, +) -> None: + adapter, robot = _connected_adapter(a1z_adapter_module, gripper=True) + + assert adapter.is_connected() + assert robot.factory_kwargs["gripper_max_torque"] == pytest.approx(0.5) + + def test_configured_gripper_free_drive_tracks_adapter_lifecycle( a1z_adapter_module: ModuleType, ) -> None: diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index ecf75ac543..c67b10b549 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -39,6 +39,7 @@ "coordinator-piper": "dimos.robot.manipulators.piper.blueprints.basic:coordinator_piper", "coordinator-piper-xarm": "dimos.robot.manipulators.common.mixed:coordinator_piper_xarm", "coordinator-servo-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_servo_xarm6", + "coordinator-teleop-a1z": "dimos.robot.manipulators.a1z.blueprints.teleop:coordinator_teleop_a1z", "coordinator-teleop-dual": "dimos.robot.manipulators.common.mixed:coordinator_teleop_dual", "coordinator-teleop-piper": "dimos.robot.manipulators.piper.blueprints.teleop:coordinator_teleop_piper", "coordinator-teleop-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_teleop_xarm6", @@ -97,6 +98,7 @@ "teleop-phone": "dimos.teleop.phone.blueprints:teleop_phone", "teleop-phone-go2": "dimos.teleop.phone.blueprints:teleop_phone_go2", "teleop-phone-go2-fleet": "dimos.teleop.phone.blueprints:teleop_phone_go2_fleet", + "teleop-quest-a1z": "dimos.teleop.quest.blueprints:teleop_quest_a1z", "teleop-quest-dual": "dimos.teleop.quest.blueprints:teleop_quest_dual", "teleop-quest-go2": "dimos.teleop.quest.blueprints:teleop_quest_go2", "teleop-quest-piper": "dimos.teleop.quest.blueprints:teleop_quest_piper", diff --git a/dimos/robot/manipulators/a1z/blueprints/teleop.py b/dimos/robot/manipulators/a1z/blueprints/teleop.py index 6207746b64..4cc2a1a8da 100644 --- a/dimos/robot/manipulators/a1z/blueprints/teleop.py +++ b/dimos/robot/manipulators/a1z/blueprints/teleop.py @@ -23,7 +23,11 @@ a1z_hardware, make_a1z_model_config, ) -from dimos.robot.manipulators.common.blueprints import eef_twist_task, trajectory_task +from dimos.robot.manipulators.common.blueprints import ( + eef_twist_task, + teleop_ik_task, + trajectory_task, +) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule _a1z_keyboard_hw = a1z_hardware("arm") @@ -53,3 +57,33 @@ visualization={"backend": "viser"}, ), ) + + +_a1z_quest_hw = a1z_hardware("arm") +_a1z_quest_model = make_a1z_model_config() + +coordinator_teleop_a1z = autoconnect( + ControlCoordinator.blueprint( + hardware=[_a1z_quest_hw], + tasks=[ + teleop_ik_task( + _a1z_quest_hw, + hand="left", + name="teleop_a1z", + robot_model=_a1z_quest_model, + control_ik={"max_velocity": 2.0}, + priority=20, + params={ + "gripper_joint": _a1z_quest_hw.gripper_joints[0], + "gripper_open_pos": 1.0, + "gripper_closed_pos": 0.0, + }, + ), + trajectory_task(_a1z_quest_hw), + ], + ), + ManipulationModule.blueprint( + robots=[_a1z_quest_model], + visualization={"backend": "viser"}, + ), +) diff --git a/dimos/robot/manipulators/a1z/blueprints/test_teleop.py b/dimos/robot/manipulators/a1z/blueprints/test_teleop.py new file mode 100644 index 0000000000..a7b8b51cc1 --- /dev/null +++ b/dimos/robot/manipulators/a1z/blueprints/test_teleop.py @@ -0,0 +1,81 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from typing import Any, cast + +import pytest + +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.core.coordination.blueprints import Blueprint +from dimos.core.global_config import global_config +from dimos.robot.manipulators.a1z.blueprints.teleop import coordinator_teleop_a1z +from dimos.robot.manipulators.a1z.config import a1z_hardware +from dimos.teleop.quest.blueprints import teleop_quest_a1z +from dimos.teleop.quest.quest_extensions import ArmTeleopModule + + +def _coordinator_kwargs(blueprint: Blueprint) -> dict[str, Any]: + return next(atom.kwargs for atom in blueprint.blueprints if atom.module is ControlCoordinator) + + +def test_quest_teleop_uses_mock_a1z_hardware_and_gripper_by_default() -> None: + kwargs = _coordinator_kwargs(coordinator_teleop_a1z) + hardware = kwargs["hardware"][0] + tasks = cast("list[TaskConfig]", kwargs["tasks"]) + teleop = next(task for task in tasks if task.name == "teleop_a1z") + + assert hardware.adapter_type == "mock" + assert hardware.address is None + assert hardware.gripper_joints == ["arm/gripper"] + assert hardware.gripper_open_position == pytest.approx(0.1) + assert hardware.gripper_closed_position == pytest.approx(0.0) + assert teleop.params["gripper_joint"] == "arm/gripper" + assert teleop.params["gripper_open_pos"] == pytest.approx(1.0) + assert teleop.params["gripper_closed_pos"] == pytest.approx(0.0) + + +def test_quest_left_controller_routes_to_a1z_teleop() -> None: + arm_kwargs = next( + atom.kwargs for atom in teleop_quest_a1z.blueprints if atom.module is ArmTeleopModule + ) + + assert arm_kwargs["task_names"] == {"left": "teleop_a1z"} + assert teleop_quest_a1z.remapping_map == { + ("armteleopmodule", "left_controller_output"): "coordinator_cartesian_command" + } + + +def test_a1z_hardware_uses_mock_adapter_in_simulation(monkeypatch: pytest.MonkeyPatch) -> None: + monkeypatch.setattr(global_config, "can_port", "a1zcan") + monkeypatch.setattr(global_config, "simulation", "mujoco") + + hardware = a1z_hardware("arm") + + assert hardware.adapter_type == "mock" + assert hardware.address is None + assert hardware.adapter_kwargs == {} + assert hardware.gripper_joints == ["arm/gripper"] + + +def test_a1z_hardware_uses_real_adapter_when_can_port_is_selected( + monkeypatch: pytest.MonkeyPatch, +) -> None: + monkeypatch.setattr(global_config, "can_port", "a1zcan") + monkeypatch.setattr(global_config, "simulation", "") + + hardware = a1z_hardware("arm") + + assert hardware.adapter_type == "galaxea_a1z" + assert hardware.address == "a1zcan" + assert hardware.gripper_joints == ["arm/gripper"] diff --git a/dimos/robot/manipulators/a1z/config.py b/dimos/robot/manipulators/a1z/config.py index 30c40ef917..0c2b67f1c7 100644 --- a/dimos/robot/manipulators/a1z/config.py +++ b/dimos/robot/manipulators/a1z/config.py @@ -58,13 +58,13 @@ def a1z_hardware( dynamics_urdf_path: Path | None = None, adapter_config: A1ZConfig | None = None, ) -> HardwareComponent: - """Configure mock or real A1Z hardware from the resolved global settings.""" + """Configure mock A1Z hardware unless an explicit CAN port selects the real adapter.""" adapter_type = "mock" address = None adapter_kwargs: dict[str, object] = {} - if not global_config.simulation: + if not global_config.simulation and global_config.can_port: adapter_type = "galaxea_a1z" - address = global_config.can_port or "a1zcan" + address = global_config.can_port resolved_config = adapter_config or A1ZConfig( gripper=A1ZGripperConfig() if has_gripper else None, ) diff --git a/dimos/robot/manipulators/common/blueprints.py b/dimos/robot/manipulators/common/blueprints.py index 783943d9d1..725b1255ff 100644 --- a/dimos/robot/manipulators/common/blueprints.py +++ b/dimos/robot/manipulators/common/blueprints.py @@ -16,9 +16,8 @@ from __future__ import annotations -from collections.abc import Mapping, Sequence -from pathlib import Path -from typing import Any +from collections.abc import Sequence +from typing import Any, TypedDict from dimos.control.components import HardwareComponent from dimos.control.coordinator import ControlCoordinator, TaskConfig @@ -34,6 +33,32 @@ ) +class PinkControlIKOverrides(TypedDict, total=False): + """Pink tuning values that may be overridden by a manipulator blueprint.""" + + solver: str + max_velocity: float + lm_damping: float + task_gain: float + position_cost: float + orientation_cost: float + posture_cost: float + joint_centering_cost: float + damping_cost: float + position_limit_margin: float + seed_limit_tolerance: float + reference_q: list[float] | None + qpsolver_options: dict[str, float] + + +class GripperTaskOverrides(TypedDict, total=False): + """Optional gripper fields shared by teleop and EEF-twist tasks.""" + + gripper_joint: str + gripper_open_pos: float + gripper_closed_pos: float + + def trajectory_task( hardware: HardwareComponent, *additional_hardware: HardwareComponent, @@ -61,8 +86,8 @@ def trajectory_task( def _resolve_control_ik( hardware: HardwareComponent, robot_model: RobotModelConfig, - control_ik: Mapping[str, object] | None, -) -> dict[str, object]: + control_ik: PinkControlIKOverrides | None, +) -> dict[str, Any]: coordinator_joints = robot_model.get_coordinator_joint_names() if hardware.joints != coordinator_joints: raise ValueError("hardware joints must match RobotModelConfig coordinator joints") @@ -76,9 +101,12 @@ def cartesian_ik_task( *, name: str = CARTESIAN_IK_TASK_NAME, priority: int = 10, + timeout: float = 0.5, + max_joint_delta_deg: float = 15.0, + max_tracking_error_deg: float = 10.0, min_dt: float = 1e-4, max_dt: float = 0.05, - control_ik: Mapping[str, object] | None = None, + control_ik: PinkControlIKOverrides | None = None, robot_model: RobotModelConfig, ) -> TaskConfig: resolved_control_ik = _resolve_control_ik(hardware, robot_model, control_ik) @@ -89,6 +117,9 @@ def cartesian_ik_task( priority=priority, params={ "control_ik": resolved_control_ik, + "timeout": timeout, + "max_joint_delta_deg": max_joint_delta_deg, + "max_tracking_error_deg": max_tracking_error_deg, "min_dt": min_dt, "max_dt": max_dt, }, @@ -100,15 +131,21 @@ def eef_twist_task( *, name: str = EEF_TWIST_TASK_NAME, priority: int = 10, + timeout: float = 0.3, + max_joint_delta_deg: float = 15.0, + max_tracking_error_deg: float = 10.0, min_dt: float = 1e-4, max_dt: float = 0.05, - control_ik: Mapping[str, object] | None = None, + control_ik: PinkControlIKOverrides | None = None, robot_model: RobotModelConfig, - params: Mapping[str, object] | None = None, + params: GripperTaskOverrides | None = None, ) -> TaskConfig: resolved_control_ik = _resolve_control_ik(hardware, robot_model, control_ik) - task_params: dict[str, object] = { + task_params: dict[str, Any] = { "control_ik": resolved_control_ik, + "timeout": timeout, + "max_joint_delta_deg": max_joint_delta_deg, + "max_tracking_error_deg": max_tracking_error_deg, "min_dt": min_dt, "max_dt": max_dt, } @@ -126,17 +163,27 @@ def eef_twist_task( def teleop_ik_task( hardware: HardwareComponent, *, - model_path: Path, - ee_joint_id: int, hand: str, name: str, + robot_model: RobotModelConfig, priority: int = 10, - params: dict[str, Any] | None = None, + timeout: float = 0.5, + max_joint_delta_deg: float = 5.0, + max_tracking_error_deg: float = 10.0, + min_dt: float = 1e-4, + max_dt: float = 0.05, + control_ik: PinkControlIKOverrides | None = None, + params: GripperTaskOverrides | None = None, ) -> TaskConfig: + resolved_control_ik = _resolve_control_ik(hardware, robot_model, control_ik) task_params: dict[str, Any] = { - "model_path": model_path, - "ee_joint_id": ee_joint_id, + "control_ik": resolved_control_ik, "hand": hand, + "timeout": timeout, + "max_joint_delta_deg": max_joint_delta_deg, + "max_tracking_error_deg": max_tracking_error_deg, + "min_dt": min_dt, + "max_dt": max_dt, } if params: task_params.update(params) diff --git a/dimos/robot/manipulators/common/mixed.py b/dimos/robot/manipulators/common/mixed.py index 591f5fbfa0..fcd0615c2a 100644 --- a/dimos/robot/manipulators/common/mixed.py +++ b/dimos/robot/manipulators/common/mixed.py @@ -18,8 +18,15 @@ from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.core.global_config import global_config -from dimos.robot.manipulators.piper.config import PIPER_FK_MODEL, make_piper_hardware -from dimos.robot.manipulators.xarm.config import XARM6_FK_MODEL, make_xarm_hardware +from dimos.robot.manipulators.common.blueprints import teleop_ik_task +from dimos.robot.manipulators.piper.config import ( + make_piper_hardware, + make_piper_model_config, +) +from dimos.robot.manipulators.xarm.config import ( + make_xarm6_model_config, + make_xarm_hardware, +) _xarm6_dual = make_xarm_hardware( "xarm_arm", @@ -59,23 +66,25 @@ address=global_config.can_port or "can0", gripper=True, ) +_xarm6_teleop_model = make_xarm6_model_config(name="xarm_arm", add_gripper=False) +_piper_teleop_model = make_piper_model_config(name="piper_arm") coordinator_teleop_dual = ControlCoordinator.blueprint( hardware=[_xarm6_teleop_hw, _piper_teleop_hw], tasks=[ - TaskConfig( + teleop_ik_task( + _xarm6_teleop_hw, name="teleop_xarm", - type="teleop_ik", - joint_names=_xarm6_teleop_hw.joints, + hand="left", + robot_model=_xarm6_teleop_model, priority=10, - params={"model_path": XARM6_FK_MODEL, "ee_joint_id": 6, "hand": "left"}, ), - TaskConfig( + teleop_ik_task( + _piper_teleop_hw, name="teleop_piper", - type="teleop_ik", - joint_names=_piper_teleop_hw.joints, + hand="right", + robot_model=_piper_teleop_model, priority=10, - params={"model_path": PIPER_FK_MODEL, "ee_joint_id": 6, "hand": "right"}, ), ], ) diff --git a/dimos/robot/manipulators/piper/blueprints/teleop.py b/dimos/robot/manipulators/piper/blueprints/teleop.py index 8f3e17acc8..8933aa14b0 100644 --- a/dimos/robot/manipulators/piper/blueprints/teleop.py +++ b/dimos/robot/manipulators/piper/blueprints/teleop.py @@ -31,7 +31,6 @@ ) from dimos.robot.manipulators.common.sim import mujoco_if_sim from dimos.robot.manipulators.piper.config import ( - PIPER_FK_MODEL, PIPER_SIM_PATH, make_piper_hardware, make_piper_model_config, @@ -99,10 +98,9 @@ class _PiperTeleopCoordinator(ControlCoordinator): tasks=[ teleop_ik_task( _piper_teleop_hw, - model_path=PIPER_FK_MODEL, - ee_joint_id=6, hand="left", name="teleop_piper", + robot_model=_piper_model, params={ "gripper_joint": make_gripper_joints("arm")[0], "gripper_open_pos": 1.0, diff --git a/dimos/robot/manipulators/piper/config.py b/dimos/robot/manipulators/piper/config.py index 98943012ed..1d390aa553 100644 --- a/dimos/robot/manipulators/piper/config.py +++ b/dimos/robot/manipulators/piper/config.py @@ -41,7 +41,6 @@ "piper_description": LfsPath("piper_description"), "piper_gazebo": LfsPath("piper_description"), } -PIPER_FK_MODEL = LfsPath("piper_description/mujoco_model/piper_no_gripper_description.xml") PIPER_SIM_PATH = LfsPath("piper/scene.xml") PIPER_HOME_JOINTS = [ 0.793, diff --git a/dimos/robot/manipulators/xarm/blueprints/teleop.py b/dimos/robot/manipulators/xarm/blueprints/teleop.py index b41e615b37..ef28d2274f 100644 --- a/dimos/robot/manipulators/xarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/xarm/blueprints/teleop.py @@ -16,6 +16,8 @@ from __future__ import annotations +from typing import cast + from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config @@ -23,14 +25,14 @@ from dimos.manipulation.manipulation_module import ManipulationModule from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.manipulators.common.blueprints import ( + GripperTaskOverrides, eef_twist_task, teleop_ik_task, + trajectory_task, ) from dimos.robot.manipulators.common.sim import mujoco_if_sim from dimos.robot.manipulators.xarm.config import ( - XARM6_FK_MODEL, XARM6_SIM_PATH, - XARM7_FK_MODEL, XARM7_SIM_PATH, XARM_GRIPPER_PARAMS, make_xarm6_model_config, @@ -45,7 +47,7 @@ _xarm7_hw = xarm7_hardware("arm", gripper=True, mock_without_address=True) _xarm6_control_model = make_xarm6_model_config(add_gripper=False) _xarm7_control_model = make_xarm7_model_config(add_gripper=False) -_xarm_eef_params = {**XARM_GRIPPER_PARAMS, "timeout": 0.0} +_xarm_gripper_params = cast("GripperTaskOverrides", XARM_GRIPPER_PARAMS) keyboard_teleop_xarm6 = autoconnect( KeyboardTeleopModule.blueprint(), @@ -58,7 +60,8 @@ eef_twist_task( _xarm6_hw, robot_model=_xarm6_control_model, - params=_xarm_eef_params, + timeout=0.0, + params=_xarm_gripper_params, ) ], ), @@ -79,7 +82,8 @@ eef_twist_task( _xarm7_hw, robot_model=_xarm7_control_model, - params=_xarm_eef_params, + timeout=0.0, + params=_xarm_gripper_params, ) ], ), @@ -140,11 +144,21 @@ ) _xarm7_teleop_hw = xarm7_hardware( - "arm", gripper=True, gripper_open_position=0.85, gripper_closed_position=0.0 + "arm", + gripper=True, + gripper_open_position=0.85, + gripper_closed_position=0.0, + mock_without_address=True, ) _xarm6_teleop_hw = xarm6_hardware( - "arm", gripper=True, gripper_open_position=0.85, gripper_closed_position=0.0 + "arm", + gripper=True, + gripper_open_position=0.85, + gripper_closed_position=0.0, + mock_without_address=True, ) +_xarm7_teleop_model = make_xarm7_model_config(add_gripper=True) +_xarm6_teleop_model = make_xarm6_model_config(add_gripper=True) # Dual-input arm: VR (teleop_ik) preempts browser keyboard (eef_twist) via # higher priority; when VR is idle the always-active eef_twist holds/drives. @@ -164,21 +178,26 @@ class _XArm7TeleopCoordinator(ControlCoordinator): tasks=[ teleop_ik_task( _xarm7_teleop_hw, - model_path=XARM7_FK_MODEL, - ee_joint_id=7, hand="right", name="teleop_xarm", + robot_model=_xarm7_control_model, priority=20, - params=XARM_GRIPPER_PARAMS, + params=_xarm_gripper_params, ), eef_twist_task( _xarm7_teleop_hw, robot_model=_xarm7_control_model, priority=10, - params=_xarm_eef_params, + timeout=0.0, + params=_xarm_gripper_params, ), + trajectory_task(_xarm7_teleop_hw), ], ), + ManipulationModule.blueprint( + robots=[_xarm7_teleop_model], + visualization={"backend": "viser"}, + ), *mujoco_if_sim(XARM7_SIM_PATH, len(_xarm7_teleop_hw.joints)), ) @@ -188,20 +207,25 @@ class _XArm7TeleopCoordinator(ControlCoordinator): tasks=[ teleop_ik_task( _xarm6_teleop_hw, - model_path=XARM6_FK_MODEL, - ee_joint_id=6, hand="right", name="teleop_xarm", + robot_model=_xarm6_control_model, priority=20, - params=XARM_GRIPPER_PARAMS, + params=_xarm_gripper_params, ), eef_twist_task( _xarm6_teleop_hw, robot_model=_xarm6_control_model, priority=10, - params=_xarm_eef_params, + timeout=0.0, + params=_xarm_gripper_params, ), + trajectory_task(_xarm6_teleop_hw), ], ), + ManipulationModule.blueprint( + robots=[_xarm6_teleop_model], + visualization={"backend": "viser"}, + ), *mujoco_if_sim(XARM6_SIM_PATH, len(_xarm6_teleop_hw.joints)), ) diff --git a/dimos/robot/manipulators/xarm/config.py b/dimos/robot/manipulators/xarm/config.py index bb7d577927..0906610545 100644 --- a/dimos/robot/manipulators/xarm/config.py +++ b/dimos/robot/manipulators/xarm/config.py @@ -56,15 +56,12 @@ XARM_MODEL_PATH = LfsPath("xarm_description") / "urdf/xarm_device.urdf.xacro" XARM_PACKAGE_PATHS: dict[str, Path] = {"xarm_description": LfsPath("xarm_description")} -XARM6_FK_MODEL = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf") -XARM7_FK_MODEL = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf") XARM6_SIM_PATH = LfsPath("xarm6/scene.xml") XARM7_SIM_PATH = LfsPath("xarm7/scene.xml") XARM_GRIPPER_PARAMS = { "gripper_joint": make_gripper_joints("arm")[0], "gripper_open_pos": 0.85, "gripper_closed_pos": 0.0, - "max_joint_delta_deg": 50.0, } XARM7_SIM_HOME = [0.0, -0.247, 0.0, 0.909, 0.0, 1.15644, 0.0] diff --git a/dimos/teleop/quest/README.md b/dimos/teleop/quest/README.md index 4e8164ec9b..fc32904228 100644 --- a/dimos/teleop/quest/README.md +++ b/dimos/teleop/quest/README.md @@ -15,9 +15,16 @@ Quest Browser ──WebSocket──→ Embedded HTTPS Server ──→ Quest dimos run teleop-quest-rerun # Quest teleop + Rerun viz dimos run teleop-quest-xarm7 # XArm7 dimos run teleop-quest-piper # Piper +dimos run teleop-quest-a1z # A1Z with mock hardware dimos run teleop-quest-dual # Dual arm ``` +Select a CAN interface explicitly to control real A1Z hardware: + +```bash +dimos --can-port a1zcan run teleop-quest-a1z +``` + Open `https://:8443/teleop` on Quest browser. Accept cert, tap Connect. ## Subclassing diff --git a/dimos/teleop/quest/blueprints.py b/dimos/teleop/quest/blueprints.py index 0b6644960a..d85b9f99b5 100644 --- a/dimos/teleop/quest/blueprints.py +++ b/dimos/teleop/quest/blueprints.py @@ -25,6 +25,7 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.sensor_msgs.Image import Image +from dimos.robot.manipulators.a1z.blueprints.teleop import coordinator_teleop_a1z from dimos.robot.manipulators.common.mixed import coordinator_teleop_dual from dimos.robot.manipulators.piper.blueprints.teleop import coordinator_teleop_piper from dimos.robot.manipulators.xarm.blueprints.teleop import ( @@ -82,6 +83,13 @@ ).remappings([(ArmTeleopModule, "left_controller_output", "coordinator_cartesian_command")]) +# A1Z mock teleop: left controller -> A1Z arm +teleop_quest_a1z = autoconnect( + ArmTeleopModule.blueprint(task_names={"left": "teleop_a1z"}), + coordinator_teleop_a1z, +).remappings([(ArmTeleopModule, "left_controller_output", "coordinator_cartesian_command")]) + + # XArm6 teleop (sim with --simulation, real otherwise): right controller -> xarm6 teleop_quest_xarm6 = autoconnect( ArmTeleopModule.blueprint(task_names={"right": "teleop_xarm"}), diff --git a/docs/capabilities/manipulation/a1z.md b/docs/capabilities/manipulation/a1z.md index 1bc6941ee0..0cfeabb0e7 100644 --- a/docs/capabilities/manipulation/a1z.md +++ b/docs/capabilities/manipulation/a1z.md @@ -94,16 +94,20 @@ stopping DimOS. Disabling the motors makes the arm fall. dimos run keyboard-teleop-a1z ``` -This launches keyboard teleoperation, the control coordinator, trajectory -execution, and `ManipulationModule`. Startup waits for feedback from all six arm +This launches keyboard teleoperation with mock hardware, the control coordinator, +trajectory execution, and `ManipulationModule`. Select a CAN interface explicitly +to use the real arm. Real-hardware startup waits for feedback from all six arm motors, validates the measured state, holds the measured pose, and then ramps -gravity compensation. +gravity compensation: -On Linux, the blueprint uses `a1zcan` by default. If you configured another -verified SocketCAN interface, pass it explicitly: +```bash +dimos --can-port a1zcan run keyboard-teleop-a1z +``` + +On Linux, pass another verified SocketCAN interface instead if needed: ```bash -dimos run keyboard-teleop-a1z --can-port can0 +dimos --can-port can0 run keyboard-teleop-a1z ``` On macOS, the adapter selects the userspace USB transport automatically; omit diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index 832acb49ac..13d5ff826d 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -578,17 +578,23 @@ target frames. `base_link` is only the robot-scoped link placed by `base_pose`; do not use it as a substitute for planning-group chain metadata. See [Planning Groups](/docs/capabilities/manipulation/planning_groups.md). -### 4d. Configure Cartesian and EEF-twist control IK +### 4d. Configure Cartesian, EEF-twist, and teleop control IK -Cartesian and EEF-twist tasks use the direct URDF or Xacro in -`RobotModelConfig`. Set `package_paths` and `xacro_args` when needed, name the -end-effector link, and map coordinator joints to model joints. The task validates -the prepared model, frame, and joint mapping at startup. +Cartesian, EEF-twist, and engagement-relative teleop tasks use the direct URDF +or Xacro in `RobotModelConfig`. Set `package_paths` and `xacro_args` when needed, +name the end-effector link, and map coordinator joints to model joints. The task +validates the prepared model, frame, and joint mapping at startup. Teleop uses +the named frame and does not accept a separate model path or numeric +end-effector joint ID. Pass the same model configuration to the common helpers: ```python skip -from dimos.robot.manipulators.common.blueprints import cartesian_ik_task, eef_twist_task +from dimos.robot.manipulators.common.blueprints import ( + cartesian_ik_task, + eef_twist_task, + teleop_ik_task, +) cartesian_task = cartesian_ik_task( hardware, @@ -598,12 +604,20 @@ twist_task = eef_twist_task( hardware, robot_model=robot_model, ) +teleop_task = teleop_ik_task( + hardware, + name="teleop_arm", + hand="right", + robot_model=robot_model, +) ``` Each tick starts from measured joints and applies model position and velocity -limits. Twist targets are derived from measured forward kinematics. Invalid -models or mappings fail at startup; invalid runtime output holds the measured -position. Validate Cartesian and twist behavior in simulation or replay before +limits. Twist targets are derived from measured forward kinematics. Teleop +targets apply controller deltas to a measured engagement baseline and discard +that baseline across disengage, timeout, stop, clear, or E-STOP. Invalid models +or mappings fail at startup; invalid runtime output holds the measured position. +Validate Cartesian, twist, and teleop behavior in simulation or replay before hardware use. ## Step 5: Register Blueprints diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index 8319245371..fe4d8460db 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -153,10 +153,12 @@ tool, or CLI motion command yet. ### Cartesian control IK -Cartesian and keyboard EEF-twist tasks use the direct URDF/Xacro model from -`RobotModelConfig`. The configuration supplies package paths, Xacro arguments, -the named end-effector frame, and coordinator-to-model joint mapping. Invalid -models, frames, or mappings fail at startup. +Cartesian, keyboard EEF-twist, and engagement-relative teleop IK tasks use the +direct URDF/Xacro model from `RobotModelConfig`. The configuration supplies +package paths, Xacro arguments, the named end-effector frame, and +coordinator-to-model joint mapping. Invalid models, frames, or mappings fail at +startup; teleop configuration does not use a separate model path or numeric +end-effector joint ID. Each control tick starts from measured joints, applies model position and velocity limits, and holds the measured position when a solve cannot produce a @@ -166,16 +168,27 @@ does not use `WorldSpec` or provide world-obstacle avoidance. For a custom robot, pass the typed model configuration to the helper: ```python skip -from dimos.robot.manipulators.common.blueprints import cartesian_ik_task +from dimos.robot.manipulators.common.blueprints import cartesian_ik_task, teleop_ik_task task = cartesian_ik_task( hardware, robot_model=robot_model, ) +teleop_task = teleop_ik_task( + hardware, + name="teleop_arm", + hand="right", + robot_model=robot_model, +) ``` -Validate Cartesian and twist behavior in simulation or replay before hardware -use. +Teleop pose commands are deltas from an end-effector pose captured from measured +joints at engagement. Disengage, timeout, stop, clear, or E-STOP discards that +baseline; commands received during E-STOP are rejected rather than replayed +after clear. + +Validate Cartesian, twist, and teleop behavior in simulation or replay before +hardware use. Install the manipulation dependencies: diff --git a/stubs/a1z/robots/get_robot.pyi b/stubs/a1z/robots/get_robot.pyi index ec68cfd917..160a5ed390 100644 --- a/stubs/a1z/robots/get_robot.pyi +++ b/stubs/a1z/robots/get_robot.pyi @@ -1,4 +1,8 @@ from pathlib import Path +from typing import Any + +import numpy as np +import numpy.typing as npt from .arm_robot import ArmRobot @@ -8,6 +12,8 @@ def get_a1z_robot( zero_gravity_mode: bool = ..., control_freq_hz: int = ..., urdf_path: str | Path | None = ..., + default_kp: npt.NDArray[np.floating[Any]] | None = ..., + default_kd: npt.NDArray[np.floating[Any]] | None = ..., with_gripper: bool = ..., gripper_max_torque: float = ..., ) -> ArmRobot: ...