From fea963e31739d8952210c75c0aa97b1ea1e756ed Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Mon, 9 Feb 2026 15:47:25 -0800 Subject: [PATCH 01/27] initial commit --- dimos/control/tasks/teleop_task.py | 540 +++++++++++++++++++++++++++++ dimos/teleop/blueprints.py | 23 +- 2 files changed, 562 insertions(+), 1 deletion(-) create mode 100644 dimos/control/tasks/teleop_task.py diff --git a/dimos/control/tasks/teleop_task.py b/dimos/control/tasks/teleop_task.py new file mode 100644 index 0000000000..f73b0890e4 --- /dev/null +++ b/dimos/control/tasks/teleop_task.py @@ -0,0 +1,540 @@ +# Copyright 2025-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. + +"""Cartesian control task with internal Pinocchio IK solver. + +Accepts streaming cartesian poses (e.g., from teleoperation, visual servoing) +and computes inverse kinematics internally to output joint commands. +Participates in joint-level arbitration. + +CRITICAL: Uses t_now from CoordinatorState, never calls time.time() +""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +import threading +from typing import TYPE_CHECKING, Any + +import numpy as np +from numpy.linalg import norm, solve +import pinocchio # type: ignore[import-untyped] + +from dimos.control.task import ( + ControlMode, + ControlTask, + CoordinatorState, + JointCommandOutput, + ResourceClaim, +) +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from numpy.typing import NDArray + + from dimos.msgs.geometry_msgs import Pose, PoseStamped + +logger = setup_logger() + + +@dataclass +class CartesianIKTaskConfig: + """Configuration for cartesian 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) + ik_max_iter: Maximum IK solver iterations + ik_eps: IK convergence threshold (error norm in meters) + ik_damp: IK damping factor for singularity handling (higher = more stable) + ik_dt: IK integration step size + max_joint_delta_deg: Maximum allowed joint change per tick (safety limit) + max_velocity: Max joint velocity per IK iteration (rad/s) + """ + + joint_names: list[str] + model_path: str | Path + ee_joint_id: int + priority: int = 10 + timeout: float = 0.5 + ik_max_iter: int = 100 + ik_eps: float = 1e-4 + ik_damp: float = 1e-2 + ik_dt: float = 1.0 + max_joint_delta_deg: float = 20.0 # ~500°/s at 100Hz + max_velocity: float = 10.0 + + +class CartesianIKTask(ControlTask): + """Cartesian control task with internal Pinocchio IK solver. + + Accepts streaming cartesian poses via set_target_pose() and computes IK + internally to output joint commands. Uses current joint state from + CoordinatorState as IK warm-start for fast convergence. + + Unlike CartesianServoTask (which bypasses joint arbitration), this task + outputs JointCommandOutput and participates in joint-level arbitration. + + Example: + >>> from dimos.utils.data import get_data + >>> piper_path = get_data("piper_description") + >>> task = CartesianIKTask( + ... name="cartesian_arm", + ... config=CartesianIKTaskConfig( + ... 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, + ... ), + ... ) + >>> coordinator.add_task(task) + >>> task.start() + >>> + >>> # From teleop callback or other source: + >>> task.set_target_pose(pose_stamped, t_now=time.perf_counter()) + """ + + def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: + """Initialize cartesian IK task. + + Args: + name: Unique task name + config: Task configuration + """ + if not config.joint_names: + raise ValueError(f"CartesianIKTask '{name}' requires at least one joint") + if not config.model_path: + raise ValueError(f"CartesianIKTask '{name}' requires model_path for IK solver") + + 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) + + # Load Pinocchio model + model_path = Path(config.model_path) + if not model_path.exists(): + raise FileNotFoundError(f"Model file not found: {model_path}") + + if model_path.suffix == ".xml": + self._model = pinocchio.buildModelFromMJCF(str(model_path)) + else: + self._model = pinocchio.buildModelFromUrdf(str(model_path)) + + self._data = self._model.createData() + self._ee_joint_id = config.ee_joint_id + + # Validate DOF matches joint names + if self._model.nq != self._num_joints: + logger.warning( + f"CartesianIKTask {name}: model DOF ({self._model.nq}) != " + f"joint_names count ({self._num_joints})" + ) + + # Thread-safe target state + self._lock = threading.Lock() + self._target_pose: pinocchio.SE3 | None = None + self._last_update_time: float = 0.0 + self._active = False + + # Initial EE pose (captured when tracking starts, used for delta mode) + self._initial_ee_pose: pinocchio.SE3 | None = None + + # Cache last successful IK solution for warm-starting + self._last_q_solution: NDArray[np.floating[Any]] | None = None + + logger.info( + f"CartesianIKTask {name} initialized with model: {model_path}, " + f"ee_joint_id={config.ee_joint_id}, joints={config.joint_names}" + ) + + @property + def name(self) -> str: + """Unique task identifier.""" + return self._name + + def claim(self) -> ResourceClaim: + """Declare resource requirements.""" + return ResourceClaim( + joints=self._joint_names, + priority=self._config.priority, + mode=ControlMode.SERVO_POSITION, + ) + + def is_active(self) -> bool: + """Check if task should run this tick.""" + with self._lock: + return self._active and self._target_pose is not None + + 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) + + 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: + return None + + # Check timeout (safety stop if teleop crashes) + 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"CartesianIKTask {self._name} timed out " + f"(no update for {time_since_update:.3f}s)" + ) + self._active = False + return None + + delta_pose = self._target_pose + + # Capture initial EE pose if not set (first command after engage) + if self._initial_ee_pose is None: + q_current = self._get_current_joints(state) + if q_current is None: + logger.debug(f"CartesianIKTask {self._name}: cannot capture initial pose") + return None + + pinocchio.forwardKinematics(self._model, self._data, q_current) + with self._lock: + self._initial_ee_pose = self._data.oMi[self._ee_joint_id].copy() + logger.info( + f"CartesianIKTask {self._name}: captured initial EE pose at " + f"[{self._initial_ee_pose.translation[0]:.3f}, " + f"{self._initial_ee_pose.translation[1]:.3f}, " + f"{self._initial_ee_pose.translation[2]:.3f}]" + ) + + # Apply delta to initial pose: target = initial + delta + with self._lock: + target_pose = pinocchio.SE3( + delta_pose.rotation @ self._initial_ee_pose.rotation, + self._initial_ee_pose.translation + delta_pose.translation, + ) + + # Get current joint positions for IK warm-start + 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") + return None + + # Compute IK + q_solution, converged, final_error = self._solve_ik(target_pose, q_current) + + # Use the solution even if it didn't fully converge - the safety clamp + # will handle any large jumps. This prevents the arm from "sticking" + # when near singularities or workspace boundaries. + if not converged: + logger.debug( + f"CartesianIKTask {self._name}: IK did not converge " + f"(error={final_error:.4f}), using partial solution" + ) + + # Safety check: reject if joint delta too large + if not self._safety_check(q_solution, q_current): + return None + + # Cache solution for next warm-start + with self._lock: + self._last_q_solution = q_solution.copy() + + # DEBUG: Print FK'd EE pose instead of sending commands to arm + ee_pose = self.forward_kinematics(q_solution) + logger.info( + f"CartesianIKTask {self._name}: EE pose " + f"[{ee_pose.translation[0]:.4f}, {ee_pose.translation[1]:.4f}, {ee_pose.translation[2]:.4f}]" + ) + # return None # Don't send commands to hardware + + return JointCommandOutput( + joint_names=self._joint_names_list, + positions=q_solution.flatten().tolist(), + mode=ControlMode.SERVO_POSITION, + ) + + def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.floating[Any]] | None: + """Get current joint positions from coordinator state. + + Falls back to last IK solution if joint state unavailable. + """ + positions = [] + for joint_name in self._joint_names_list: + pos = state.joints.get_position(joint_name) + if pos is None: + # Fallback to last solution + if self._last_q_solution is not None: + return self._last_q_solution.copy() + return None + positions.append(pos) + return np.array(positions) + + def _solve_ik( + self, + target_pose: pinocchio.SE3, + q_init: NDArray[np.floating[Any]], + ) -> tuple[NDArray[np.floating[Any]], bool, float]: + """Solve IK using damped least-squares (Levenberg-Marquardt). + + Args: + target_pose: Target end-effector pose as SE3 + q_init: Initial joint configuration (warm-start) + + Returns: + Tuple of (joint_angles, converged, final_error) + """ + q = q_init.copy() + final_err = float("inf") + + for _ in range(self._config.ik_max_iter): + pinocchio.forwardKinematics(self._model, self._data, q) + iMd = self._data.oMi[self._ee_joint_id].actInv(target_pose) + + err = pinocchio.log(iMd).vector + final_err = float(norm(err)) + if final_err < self._config.ik_eps: + return q, True, final_err + + J = pinocchio.computeJointJacobian(self._model, self._data, q, self._ee_joint_id) + J = -np.dot(pinocchio.Jlog6(iMd.inverse()), J) + v = -J.T.dot(solve(J.dot(J.T) + self._config.ik_damp * np.eye(6), err)) + + # Clamp velocity to prevent explosion near singularities + v_norm = norm(v) + if v_norm > self._config.max_velocity: + v = v * (self._config.max_velocity / v_norm) + + q = pinocchio.integrate(self._model, q, v * self._config.ik_dt) + + return q, False, final_err + + def _safety_check( + self, q_new: NDArray[np.floating[Any]], q_current: NDArray[np.floating[Any]] + ) -> bool: + """Check if IK solution is safe to execute. + + Args: + q_new: Proposed joint positions from IK + q_current: Current joint positions + + Returns: + True if safe, False if motion should be rejected + """ + max_delta_rad = np.radians(self._config.max_joint_delta_deg) + joint_deltas = q_new - q_current + + if np.any(np.abs(joint_deltas) > max_delta_rad): + worst_joint = int(np.argmax(np.abs(joint_deltas))) + logger.warning( + f"CartesianIKTask {self._name}: rejecting motion - " + f"joint {self._joint_names_list[worst_joint]} delta " + f"{np.degrees(joint_deltas[worst_joint]):.1f}° exceeds limit " + f"{self._config.max_joint_delta_deg}°" + ) + return False + + return True + + 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"CartesianIKTask {self._name} preempted by {by_task} on joints {joints}" + ) + + # ========================================================================= + # Task-specific methods + # ========================================================================= + + def set_target_pose(self, pose: Pose | PoseStamped, t_now: float) -> bool: + """Set target end-effector pose from delta. + + Treats incoming pose as a delta from the initial EE pose. The initial pose + is automatically captured on the first compute() call after activation. + + Call this from your teleop callback, visual servoing, or other input source. + + Args: + pose: Delta pose from teleop (position offset + orientation delta) + t_now: Current time (from coordinator or time.perf_counter()) + + Returns: + True if accepted + """ + delta_se3 = self._pose_to_se3(pose) + + with self._lock: + self._target_pose = delta_se3 # Store delta, will apply to initial in compute() + self._last_update_time = t_now + self._active = True + + return True + + def set_target_pose_dict( + self, + pose: dict[str, float], + t_now: float, + ) -> bool: + """Set target from pose dict with position and RPY orientation. + + Args: + pose: {x, y, z, roll, pitch, yaw} in meters/radians + t_now: Current time + + Returns: + True if accepted, False if missing required keys + """ + required_keys = {"x", "y", "z", "roll", "pitch", "yaw"} + if not required_keys.issubset(pose.keys()): + missing = required_keys - set(pose.keys()) + logger.warning(f"CartesianIKTask {self._name}: missing pose keys {missing}") + return False + + position = np.array([pose["x"], pose["y"], pose["z"]]) + rotation = pinocchio.rpy.rpyToMatrix(pose["roll"], pose["pitch"], pose["yaw"]) + target_se3 = pinocchio.SE3(rotation, position) + + with self._lock: + self._target_pose = target_se3 + self._last_update_time = t_now + self._active = True + + return True + + def _pose_to_se3(self, pose: Pose | PoseStamped) -> pinocchio.SE3: + """Convert a Pose message to pinocchio SE3. + + Uses quaternion directly to avoid Euler angle conversion issues. + """ + # Handle both Pose and PoseStamped + if hasattr(pose, "position"): + # Assume Pose or PoseStamped with position/orientation attributes + position = np.array([pose.x, pose.y, pose.z]) + quat = pose.orientation + rotation = pinocchio.Quaternion(quat.w, quat.x, quat.y, quat.z).toRotationMatrix() + else: + # Assume it has x, y, z directly + position = np.array([pose.x, pose.y, pose.z]) + quat = pose.orientation + rotation = pinocchio.Quaternion(quat.w, quat.x, quat.y, quat.z).toRotationMatrix() + + return pinocchio.SE3(rotation, position) + + def start(self) -> None: + """Activate the task (start accepting and outputting commands).""" + with self._lock: + 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 + logger.info(f"CartesianIKTask {self._name} stopped") + + def clear(self) -> None: + """Clear current target, initial pose, and deactivate.""" + with self._lock: + self._target_pose = None + self._initial_ee_pose = None + self._active = False + logger.info(f"CartesianIKTask {self._name} cleared") + + def capture_initial_pose(self, state: CoordinatorState) -> bool: + """Capture current EE pose as the initial/base pose for delta mode. + + Call this when teleop engages (e.g., X button pressed) to set the + reference pose that deltas will be applied to. + + Args: + state: Current coordinator state (for forward kinematics) + + Returns: + True if captured successfully, False if joint state unavailable + """ + q_current = self._get_current_joints(state) + if q_current is None: + logger.warning( + f"CartesianIKTask {self._name}: cannot capture initial pose, " + "joint state unavailable" + ) + return False + + pinocchio.forwardKinematics(self._model, self._data, q_current) + initial_pose = self._data.oMi[self._ee_joint_id].copy() + + with self._lock: + self._initial_ee_pose = initial_pose + + logger.info( + f"CartesianIKTask {self._name}: captured initial EE pose at " + f"[{initial_pose.translation[0]:.3f}, {initial_pose.translation[1]:.3f}, " + f"{initial_pose.translation[2]:.3f}]" + ) + return True + + def is_tracking(self) -> bool: + """Check if actively receiving and outputting commands.""" + with self._lock: + return self._active and self._target_pose is not None + + def get_current_ee_pose(self, state: CoordinatorState) -> pinocchio.SE3 | None: + """Get current end-effector pose via forward kinematics. + + Useful for getting initial pose before starting tracking. + + Args: + state: Current coordinator state + + Returns: + Current EE pose as SE3, or None if joint state unavailable + """ + q_current = self._get_current_joints(state) + if q_current is None: + return None + + pinocchio.forwardKinematics(self._model, self._data, q_current) + return self._data.oMi[self._ee_joint_id].copy() + + def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pinocchio.SE3: + """Compute end-effector pose from joint positions. + + Args: + joint_positions: Joint angles in radians + + Returns: + End-effector pose as SE3 + """ + pinocchio.forwardKinematics(self._model, self._data, joint_positions) + return self._data.oMi[self._ee_joint_id].copy() + + +__all__ = [ + "CartesianIKTask", + "CartesianIKTaskConfig", +] diff --git a/dimos/teleop/blueprints.py b/dimos/teleop/blueprints.py index bca504b1d8..176cf3a3a0 100644 --- a/dimos/teleop/blueprints.py +++ b/dimos/teleop/blueprints.py @@ -15,6 +15,7 @@ """Teleop blueprints for testing and deployment.""" +from dimos.control.blueprints import coordinator_cartesian_ik_xarm6 from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs import PoseStamped @@ -48,4 +49,24 @@ ) -__all__ = ["arm_teleop", "arm_teleop_visualizing"] +# ----------------------------------------------------------------------------- +# Teleop wired to Coordinator (right controller -> arm) +# ----------------------------------------------------------------------------- +# Combined blueprint: teleop + xarm6 CartesianIK coordinator +# Usage: dimos run arm-teleop-xarm6 + +arm_teleop_xarm6 = autoconnect( + arm_teleop_module(), + coordinator_cartesian_ik_xarm6 +).transports( + { + # Teleop outputs -> coordinator inputs (same channel for pub/sub) + ("right_controller_output", PoseStamped): LCMTransport( + "/coordinator/cartesian_command", PoseStamped + ), + ("buttons", QuestButtons): LCMTransport("/teleop/buttons", QuestButtons), + } +) + + +__all__ = ["arm_teleop", "arm_teleop_visualizing", "arm_teleop_xarm6"] From cf2021d02e788e8e405412bc32dec78209998510 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Mon, 9 Feb 2026 15:48:19 -0800 Subject: [PATCH 02/27] Feat: pinnochio_ik to manipulation/planning/kinematics --- .../planning/kinematics/__init__.py | 6 +- .../planning/kinematics/pinocchio_ik.py | 326 ++++++++++++++++++ 2 files changed, 331 insertions(+), 1 deletion(-) create mode 100644 dimos/manipulation/planning/kinematics/pinocchio_ik.py diff --git a/dimos/manipulation/planning/kinematics/__init__.py b/dimos/manipulation/planning/kinematics/__init__.py index 588fccbd72..dacd2007cb 100644 --- a/dimos/manipulation/planning/kinematics/__init__.py +++ b/dimos/manipulation/planning/kinematics/__init__.py @@ -43,5 +43,9 @@ DrakeOptimizationIK, ) from dimos.manipulation.planning.kinematics.jacobian_ik import JacobianIK +from dimos.manipulation.planning.kinematics.pinocchio_ik import ( + PinocchioIK, + PinocchioIKConfig, +) -__all__ = ["DrakeOptimizationIK", "JacobianIK"] +__all__ = ["DrakeOptimizationIK", "JacobianIK", "PinocchioIK", "PinocchioIKConfig"] diff --git a/dimos/manipulation/planning/kinematics/pinocchio_ik.py b/dimos/manipulation/planning/kinematics/pinocchio_ik.py new file mode 100644 index 0000000000..e689120dfc --- /dev/null +++ b/dimos/manipulation/planning/kinematics/pinocchio_ik.py @@ -0,0 +1,326 @@ +# Copyright 2025-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. + +"""Pinocchio-based inverse kinematics solver. + +Standalone IK solver using Pinocchio for forward kinematics and Jacobian +computation. Uses damped least-squares (Levenberg-Marquardt) for robust +convergence near singularities. + +Unlike JacobianIK (which uses the WorldSpec interface), this solver operates +directly on a Pinocchio model. This makes it suitable for lightweight, +real-time IK in control tasks where a full WorldSpec is not needed. + +Usage: + >>> from dimos.manipulation.planning.kinematics.pinocchio_ik import PinocchioIK + >>> ik = PinocchioIK.from_model_path("robot.urdf", ee_joint_id=6) + >>> q_solution, converged, error = ik.solve(target_se3, q_init) + >>> ee_pose = ik.forward_kinematics(q_solution) +""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import TYPE_CHECKING, Any + +import numpy as np +from numpy.linalg import norm, solve +import pinocchio # type: ignore[import-untyped] + +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from numpy.typing import NDArray + + from dimos.msgs.geometry_msgs import Pose, PoseStamped + +logger = setup_logger() + + +# ============================================================================= +# Configuration +# ============================================================================= + + +@dataclass +class PinocchioIKConfig: + """Configuration for the Pinocchio IK solver. + + Attributes: + max_iter: Maximum IK solver iterations + eps: Convergence threshold (SE3 log-error norm) + damp: Damping factor for singularity handling (higher = more stable) + dt: Integration step size + max_velocity: Max joint velocity per iteration (rad/s), clamps near singularities + """ + + max_iter: int = 100 + eps: float = 1e-4 + damp: float = 1e-2 + dt: float = 1.0 + max_velocity: float = 10.0 + + +# ============================================================================= +# PinocchioIK Solver +# ============================================================================= + + +class PinocchioIK: + """Pinocchio-based damped least-squares IK solver. + + Loads a URDF or MJCF model and provides: + - solve(): Damped least-squares IK from SE3 target + - forward_kinematics(): FK from joint angles to EE pose + - pose_to_se3(): Convert DimOS Pose/PoseStamped messages to pinocchio.SE3 + + Thread safety: NOT thread-safe. Each caller should use its own instance + or protect calls with an external lock. Control tasks typically hold a + lock around compute() which covers IK calls. + + Example: + >>> ik = PinocchioIK.from_model_path("robot.urdf", ee_joint_id=6) + >>> target = ik.pose_to_se3(pose_stamped) + >>> q, converged, err = ik.solve(target, q_current) + >>> if converged: + ... ee = ik.forward_kinematics(q) + """ + + def __init__( + self, + model: pinocchio.Model, + data: pinocchio.Data, + ee_joint_id: int, + config: PinocchioIKConfig | None = None, + ) -> None: + """Initialize solver with an existing Pinocchio model. + + Args: + model: Pinocchio model + data: Pinocchio data (created from model) + ee_joint_id: End-effector joint ID in the kinematic chain + config: Solver configuration (uses defaults if None) + """ + self._model = model + self._data = data + self._ee_joint_id = ee_joint_id + self._config = config or PinocchioIKConfig() + + @classmethod + def from_model_path( + cls, + model_path: str | Path, + ee_joint_id: int, + config: PinocchioIKConfig | None = None, + ) -> PinocchioIK: + """Create solver by loading a URDF or MJCF file. + + Args: + model_path: Path to URDF (.urdf) or MJCF (.xml) file + ee_joint_id: End-effector joint ID in the kinematic chain + config: Solver configuration (uses defaults if None) + + Returns: + Configured PinocchioIK instance + + Raises: + FileNotFoundError: If model file doesn't exist + """ + path = Path(model_path) + if not path.exists(): + raise FileNotFoundError(f"Model file not found: {path}") + + if path.suffix == ".xml": + model = pinocchio.buildModelFromMJCF(str(path)) + else: + model = pinocchio.buildModelFromUrdf(str(path)) + + data = model.createData() + return cls(model, data, ee_joint_id, config) + + @property + def model(self) -> pinocchio.Model: + """The Pinocchio model.""" + return self._model + + @property + def nq(self) -> int: + """Number of configuration variables (DOF).""" + return self._model.nq + + @property + def ee_joint_id(self) -> int: + """End-effector joint ID.""" + return self._ee_joint_id + + # ========================================================================= + # Core IK + # ========================================================================= + + def solve( + self, + target_pose: pinocchio.SE3, + q_init: NDArray[np.floating[Any]], + config: PinocchioIKConfig | None = None, + ) -> tuple[NDArray[np.floating[Any]], bool, float]: + """Solve IK using damped least-squares (Levenberg-Marquardt). + + Args: + target_pose: Target end-effector pose as SE3 + q_init: Initial joint configuration (warm-start) + config: Override solver config for this call (uses instance config if None) + + Returns: + Tuple of (joint_angles, converged, final_error) + """ + cfg = config or self._config + q = q_init.copy() + final_err = float("inf") + + for _ in range(cfg.max_iter): + pinocchio.forwardKinematics(self._model, self._data, q) + iMd = self._data.oMi[self._ee_joint_id].actInv(target_pose) + + err = pinocchio.log(iMd).vector + final_err = float(norm(err)) + if final_err < cfg.eps: + return q, True, final_err + + J = pinocchio.computeJointJacobian( + self._model, self._data, q, self._ee_joint_id + ) + J = -np.dot(pinocchio.Jlog6(iMd.inverse()), J) + v = -J.T.dot(solve(J.dot(J.T) + cfg.damp * np.eye(6), err)) + + # Clamp velocity to prevent explosion near singularities + v_norm = norm(v) + if v_norm > cfg.max_velocity: + v = v * (cfg.max_velocity / v_norm) + + q = pinocchio.integrate(self._model, q, v * cfg.dt) + + return q, False, final_err + + # ========================================================================= + # Forward Kinematics + # ========================================================================= + + def forward_kinematics( + self, joint_positions: NDArray[np.floating[Any]] + ) -> pinocchio.SE3: + """Compute end-effector pose from joint positions. + + Args: + joint_positions: Joint angles in radians + + Returns: + End-effector pose as SE3 + """ + pinocchio.forwardKinematics(self._model, self._data, joint_positions) + return self._data.oMi[self._ee_joint_id].copy() + + # ========================================================================= + # Pose Conversion Helpers + # ========================================================================= + + @staticmethod + def pose_to_se3(pose: Pose | PoseStamped) -> pinocchio.SE3: + """Convert a DimOS Pose or PoseStamped to pinocchio SE3. + + Uses quaternion directly to avoid Euler angle conversion issues. + + Args: + pose: Pose or PoseStamped message with position (x, y, z) + and orientation (quaternion) + + Returns: + pinocchio.SE3 representation + """ + position = np.array([pose.x, pose.y, pose.z]) + quat = pose.orientation + rotation = pinocchio.Quaternion( + quat.w, quat.x, quat.y, quat.z + ).toRotationMatrix() + return pinocchio.SE3(rotation, position) + + @staticmethod + def pose_dict_to_se3(pose: dict[str, float]) -> pinocchio.SE3: + """Convert a pose dict with RPY to pinocchio SE3. + + Args: + pose: Dict with keys {x, y, z, roll, pitch, yaw} in meters/radians + + Returns: + pinocchio.SE3 representation + + Raises: + KeyError: If required keys are missing + """ + position = np.array([pose["x"], pose["y"], pose["z"]]) + rotation = pinocchio.rpy.rpyToMatrix( + pose["roll"], pose["pitch"], pose["yaw"] + ) + return pinocchio.SE3(rotation, position) + + +# ============================================================================= +# Safety Utilities +# ============================================================================= + + +def check_joint_delta( + q_new: NDArray[np.floating[Any]], + q_current: NDArray[np.floating[Any]], + max_delta_deg: float, +) -> bool: + """Check if joint position change is within safety limits. + + Args: + q_new: Proposed joint positions (radians) + q_current: Current joint positions (radians) + max_delta_deg: Maximum allowed change per joint (degrees) + + Returns: + True if all joint deltas are within limits + """ + max_delta_rad = np.radians(max_delta_deg) + joint_deltas = np.abs(q_new - q_current) + return bool(np.all(joint_deltas <= max_delta_rad)) + + +def get_worst_joint_delta( + q_new: NDArray[np.floating[Any]], + q_current: NDArray[np.floating[Any]], +) -> tuple[int, float]: + """Find the joint with the largest position change. + + Args: + q_new: Proposed joint positions (radians) + q_current: Current joint positions (radians) + + Returns: + Tuple of (joint_index, delta_in_degrees) + """ + joint_deltas = np.abs(q_new - q_current) + worst_idx = int(np.argmax(joint_deltas)) + return worst_idx, float(np.degrees(joint_deltas[worst_idx])) + + +__all__ = [ + "PinocchioIK", + "PinocchioIKConfig", + "check_joint_delta", + "get_worst_joint_delta", +] From 018c5ee8cf8f9d9b3c8fa57d92df059fbe0d18ef Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 10 Feb 2026 14:38:31 -0800 Subject: [PATCH 03/27] using pinnochio class --- dimos/control/tasks/cartesian_ik_task.py | 157 ++++--------- dimos/control/tasks/teleop_task.py | 266 +++++++++-------------- 2 files changed, 140 insertions(+), 283 deletions(-) diff --git a/dimos/control/tasks/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task.py index 3f0ad70915..8acd496bad 100644 --- a/dimos/control/tasks/cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task.py @@ -29,7 +29,6 @@ from typing import TYPE_CHECKING, Any import numpy as np -from numpy.linalg import norm, solve import pinocchio # type: ignore[import-untyped] from dimos.control.task import ( @@ -39,6 +38,12 @@ JointCommandOutput, ResourceClaim, ) +from dimos.manipulation.planning.kinematics.pinocchio_ik import ( + PinocchioIK, + PinocchioIKConfig, + check_joint_delta, + get_worst_joint_delta, +) from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -128,23 +133,22 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: self._joint_names_list = list(config.joint_names) self._num_joints = len(config.joint_names) - # Load Pinocchio model - model_path = Path(config.model_path) - if not model_path.exists(): - raise FileNotFoundError(f"Model file not found: {model_path}") - - if model_path.suffix == ".xml": - self._model = pinocchio.buildModelFromMJCF(str(model_path)) - else: - self._model = pinocchio.buildModelFromUrdf(str(model_path)) - - self._data = self._model.createData() - self._ee_joint_id = config.ee_joint_id + # Create IK solver from model + ik_config = PinocchioIKConfig( + max_iter=config.ik_max_iter, + eps=config.ik_eps, + damp=config.ik_damp, + dt=config.ik_dt, + max_velocity=config.max_velocity, + ) + self._ik = PinocchioIK.from_model_path( + config.model_path, config.ee_joint_id, ik_config + ) # Validate DOF matches joint names - if self._model.nq != self._num_joints: + if self._ik.nq != self._num_joints: logger.warning( - f"CartesianIKTask {name}: model DOF ({self._model.nq}) != " + f"CartesianIKTask {name}: model DOF ({self._ik.nq}) != " f"joint_names count ({self._num_joints})" ) @@ -158,7 +162,7 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: self._last_q_solution: NDArray[np.floating[Any]] | None = None logger.info( - f"CartesianIKTask {name} initialized with model: {model_path}, " + f"CartesianIKTask {name} initialized with model: {config.model_path}, " f"ee_joint_id={config.ee_joint_id}, joints={config.joint_names}" ) @@ -213,27 +217,34 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: return None # Compute IK - q_solution, converged, final_error = self._solve_ik(target_pose, q_current) + q_solution, converged, final_error = self._ik.solve(target_pose, q_current) # Use the solution even if it didn't fully converge - the safety clamp # will handle any large jumps. This prevents the arm from "sticking" - # when near singularities or workspace boundaries. + # when near singularities or workspace boundary. if not converged: logger.debug( f"CartesianIKTask {self._name}: IK did not converge " f"(error={final_error:.4f}), using partial solution" ) - # Safety check: clamp large joint jumps for smooth motion - _is_exact, q_clamped = self._safety_check(q_solution, 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}°" + ) + return None # Cache solution for next warm-start with self._lock: - self._last_q_solution = q_clamped.copy() + self._last_q_solution = q_solution.copy() return JointCommandOutput( joint_names=self._joint_names_list, - positions=q_clamped.flatten().tolist(), + positions=q_solution.flatten().tolist(), mode=ControlMode.SERVO_POSITION, ) @@ -253,77 +264,6 @@ def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.floating[An positions.append(pos) return np.array(positions) - def _solve_ik( - self, - target_pose: pinocchio.SE3, - q_init: NDArray[np.floating[Any]], - ) -> tuple[NDArray[np.floating[Any]], bool, float]: - """Solve IK using damped least-squares (Levenberg-Marquardt). - - Args: - target_pose: Target end-effector pose as SE3 - q_init: Initial joint configuration (warm-start) - - Returns: - Tuple of (joint_angles, converged, final_error) - """ - q = q_init.copy() - final_err = float("inf") - - for _ in range(self._config.ik_max_iter): - pinocchio.forwardKinematics(self._model, self._data, q) - iMd = self._data.oMi[self._ee_joint_id].actInv(target_pose) - - err = pinocchio.log(iMd).vector - final_err = float(norm(err)) - if final_err < self._config.ik_eps: - return q, True, final_err - - J = pinocchio.computeJointJacobian(self._model, self._data, q, self._ee_joint_id) - J = -np.dot(pinocchio.Jlog6(iMd.inverse()), J) - v = -J.T.dot(solve(J.dot(J.T) + self._config.ik_damp * np.eye(6), err)) - - # Clamp velocity to prevent explosion near singularities - v_norm = norm(v) - if v_norm > self._config.max_velocity: - v = v * (self._config.max_velocity / v_norm) - - q = pinocchio.integrate(self._model, q, v * self._config.ik_dt) - - return q, False, final_err - - def _safety_check( - self, q_new: NDArray[np.floating[Any]], q_current: NDArray[np.floating[Any]] - ) -> tuple[bool, NDArray[np.floating[Any]]]: - """Check IK solution and clamp if needed. - - Args: - q_new: Proposed joint positions from IK - q_current: Current joint positions - - Returns: - Tuple of (is_safe, clamped_q) - if not safe, returns interpolated position - """ - max_delta_rad = np.radians(self._config.max_joint_delta_deg) - joint_deltas = q_new - q_current - - # Check if any joint exceeds the limit - if np.any(np.abs(joint_deltas) > max_delta_rad): - # Clamp the delta to the max allowed - clamped_delta = np.clip(joint_deltas, -max_delta_rad, max_delta_rad) - q_clamped = q_current + clamped_delta - - worst_joint = int(np.argmax(np.abs(joint_deltas))) - logger.debug( - f"CartesianIKTask {self._name}: clamping joint motion - " - f"joint {self._joint_names_list[worst_joint]} delta " - f"{np.degrees(joint_deltas[worst_joint]):.1f}° -> " - f"{np.degrees(clamped_delta[worst_joint]):.1f}°" - ) - return False, q_clamped - - return True, q_new - def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: """Handle preemption by higher-priority task. @@ -352,7 +292,7 @@ def set_target_pose(self, pose: Pose | PoseStamped, t_now: float) -> bool: Returns: True if accepted """ - target_se3 = self._pose_to_se3(pose) + target_se3 = PinocchioIK.pose_to_se3(pose) with self._lock: self._target_pose = target_se3 @@ -381,9 +321,7 @@ def set_target_pose_dict( logger.warning(f"CartesianIKTask {self._name}: missing pose keys {missing}") return False - position = np.array([pose["x"], pose["y"], pose["z"]]) - rotation = pinocchio.rpy.rpyToMatrix(pose["roll"], pose["pitch"], pose["yaw"]) - target_se3 = pinocchio.SE3(rotation, position) + target_se3 = PinocchioIK.pose_dict_to_se3(pose) with self._lock: self._target_pose = target_se3 @@ -392,25 +330,6 @@ def set_target_pose_dict( return True - def _pose_to_se3(self, pose: Pose | PoseStamped) -> pinocchio.SE3: - """Convert a Pose message to pinocchio SE3. - - Uses quaternion directly to avoid Euler angle conversion issues. - """ - # Handle both Pose and PoseStamped - if hasattr(pose, "position"): - # Assume Pose or PoseStamped with position/orientation attributes - position = np.array([pose.x, pose.y, pose.z]) - quat = pose.orientation - rotation = pinocchio.Quaternion(quat.w, quat.x, quat.y, quat.z).toRotationMatrix() - else: - # Assume it has x, y, z directly - position = np.array([pose.x, pose.y, pose.z]) - quat = pose.orientation - rotation = pinocchio.Quaternion(quat.w, quat.x, quat.y, quat.z).toRotationMatrix() - - return pinocchio.SE3(rotation, position) - def start(self) -> None: """Activate the task (start accepting and outputting commands).""" with self._lock: @@ -450,8 +369,7 @@ def get_current_ee_pose(self, state: CoordinatorState) -> pinocchio.SE3 | None: if q_current is None: return None - pinocchio.forwardKinematics(self._model, self._data, q_current) - return self._data.oMi[self._ee_joint_id].copy() + return self._ik.forward_kinematics(q_current) def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pinocchio.SE3: """Compute end-effector pose from joint positions. @@ -462,8 +380,7 @@ def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pino Returns: End-effector pose as SE3 """ - pinocchio.forwardKinematics(self._model, self._data, joint_positions) - return self._data.oMi[self._ee_joint_id].copy() + return self._ik.forward_kinematics(joint_positions) __all__ = [ diff --git a/dimos/control/tasks/teleop_task.py b/dimos/control/tasks/teleop_task.py index f73b0890e4..ffe36618e7 100644 --- a/dimos/control/tasks/teleop_task.py +++ b/dimos/control/tasks/teleop_task.py @@ -12,10 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Cartesian control task with internal Pinocchio IK solver. +"""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. -Accepts streaming cartesian poses (e.g., from teleoperation, visual servoing) -and computes inverse kinematics internally to output joint commands. Participates in joint-level arbitration. CRITICAL: Uses t_now from CoordinatorState, never calls time.time() @@ -29,7 +31,6 @@ from typing import TYPE_CHECKING, Any import numpy as np -from numpy.linalg import norm, solve import pinocchio # type: ignore[import-untyped] from dimos.control.task import ( @@ -39,6 +40,13 @@ JointCommandOutput, ResourceClaim, ) +from dimos.teleop.quest.quest_types import QuestButtons +from dimos.manipulation.planning.kinematics.pinocchio_ik import ( + PinocchioIK, + PinocchioIKConfig, + check_joint_delta, + get_worst_joint_delta, +) from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -50,8 +58,8 @@ @dataclass -class CartesianIKTaskConfig: - """Configuration for cartesian IK task. +class TeleopIKTaskConfig: + """Configuration for teleop IK task. Attributes: joint_names: List of joint names this task controls (must match model DOF) @@ -76,26 +84,27 @@ class CartesianIKTaskConfig: ik_eps: float = 1e-4 ik_damp: float = 1e-2 ik_dt: float = 1.0 + max_joint_delta_deg: float = 20.0 # ~500°/s at 100Hz max_velocity: float = 10.0 -class CartesianIKTask(ControlTask): - """Cartesian control task with internal Pinocchio IK solver. +class TeleopIKTask(ControlTask): + """Teleop cartesian control task with internal Pinocchio IK solver. - Accepts streaming cartesian poses via set_target_pose() and computes IK - internally to output joint commands. Uses current joint state from - CoordinatorState as IK warm-start for fast convergence. + Accepts streaming cartesian delta poses via set_target_pose() and computes IK + internally to output joint commands. Deltas are applied relative to the EE pose + captured at engage time (first compute or explicit capture_initial_pose call). - Unlike CartesianServoTask (which bypasses joint arbitration), this task - outputs JointCommandOutput and participates in joint-level arbitration. + 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 = CartesianIKTask( - ... name="cartesian_arm", - ... config=CartesianIKTaskConfig( + >>> 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, @@ -106,21 +115,21 @@ class CartesianIKTask(ControlTask): >>> coordinator.add_task(task) >>> task.start() >>> - >>> # From teleop callback or other source: - >>> task.set_target_pose(pose_stamped, t_now=time.perf_counter()) + >>> # From teleop callback: + >>> task.set_target_pose(delta_pose, t_now=time.perf_counter()) """ - def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: - """Initialize cartesian IK task. + 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"CartesianIKTask '{name}' requires at least one joint") + raise ValueError(f"TeleopIKTask '{name}' requires at least one joint") if not config.model_path: - raise ValueError(f"CartesianIKTask '{name}' requires model_path for IK solver") + raise ValueError(f"TeleopIKTask '{name}' requires model_path for IK solver") self._name = name self._config = config @@ -128,23 +137,22 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: self._joint_names_list = list(config.joint_names) self._num_joints = len(config.joint_names) - # Load Pinocchio model - model_path = Path(config.model_path) - if not model_path.exists(): - raise FileNotFoundError(f"Model file not found: {model_path}") - - if model_path.suffix == ".xml": - self._model = pinocchio.buildModelFromMJCF(str(model_path)) - else: - self._model = pinocchio.buildModelFromUrdf(str(model_path)) - - self._data = self._model.createData() - self._ee_joint_id = config.ee_joint_id + # Create IK solver from model + ik_config = PinocchioIKConfig( + max_iter=config.ik_max_iter, + eps=config.ik_eps, + damp=config.ik_damp, + dt=config.ik_dt, + max_velocity=config.max_velocity, + ) + self._ik = PinocchioIK.from_model_path( + config.model_path, config.ee_joint_id, ik_config + ) # Validate DOF matches joint names - if self._model.nq != self._num_joints: + if self._ik.nq != self._num_joints: logger.warning( - f"CartesianIKTask {name}: model DOF ({self._model.nq}) != " + f"TeleopIKTask {name}: model DOF ({self._ik.nq}) != " f"joint_names count ({self._num_joints})" ) @@ -160,8 +168,11 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: # Cache last successful IK solution for warm-starting self._last_q_solution: NDArray[np.floating[Any]] | None = None + # Button edge detection + self._prev_engage = False + logger.info( - f"CartesianIKTask {name} initialized with model: {model_path}, " + f"TeleopIKTask {name} initialized with model: {config.model_path}, " f"ee_joint_id={config.ee_joint_id}, joints={config.joint_names}" ) @@ -201,7 +212,7 @@ 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"TeleopIKTask {self._name} timed out " f"(no update for {time_since_update:.3f}s)" ) self._active = False @@ -213,17 +224,17 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: if self._initial_ee_pose is None: q_current = self._get_current_joints(state) if q_current is None: - logger.debug(f"CartesianIKTask {self._name}: cannot capture initial pose") + logger.debug(f"TeleopIKTask {self._name}: cannot capture initial pose") return None - pinocchio.forwardKinematics(self._model, self._data, q_current) + initial_pose = self._ik.forward_kinematics(q_current) with self._lock: - self._initial_ee_pose = self._data.oMi[self._ee_joint_id].copy() + self._initial_ee_pose = initial_pose logger.info( - f"CartesianIKTask {self._name}: captured initial EE pose at " - f"[{self._initial_ee_pose.translation[0]:.3f}, " - f"{self._initial_ee_pose.translation[1]:.3f}, " - f"{self._initial_ee_pose.translation[2]:.3f}]" + f"TeleopIKTask {self._name}: captured initial EE pose at " + f"[{initial_pose.translation[0]:.3f}, " + f"{initial_pose.translation[1]:.3f}, " + f"{initial_pose.translation[2]:.3f}]" ) # Apply delta to initial pose: target = initial + delta @@ -236,37 +247,35 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: # Get current joint positions for IK warm-start 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") + logger.debug(f"TeleopIKTask {self._name}: missing joint state for IK warm-start") return None # Compute IK - q_solution, converged, final_error = self._solve_ik(target_pose, q_current) + q_solution, converged, final_error = self._ik.solve(target_pose, q_current) # Use the solution even if it didn't fully converge - the safety clamp # will handle any large jumps. This prevents the arm from "sticking" # when near singularities or workspace boundaries. if not converged: logger.debug( - f"CartesianIKTask {self._name}: IK did not converge " + f"TeleopIKTask {self._name}: IK did not converge " f"(error={final_error:.4f}), using partial solution" ) - # Safety check: reject if joint delta too large - if not self._safety_check(q_solution, 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"TeleopIKTask {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}°" + ) return None # Cache solution for next warm-start with self._lock: self._last_q_solution = q_solution.copy() - # DEBUG: Print FK'd EE pose instead of sending commands to arm - ee_pose = self.forward_kinematics(q_solution) - logger.info( - f"CartesianIKTask {self._name}: EE pose " - f"[{ee_pose.translation[0]:.4f}, {ee_pose.translation[1]:.4f}, {ee_pose.translation[2]:.4f}]" - ) - # return None # Don't send commands to hardware - return JointCommandOutput( joint_names=self._joint_names_list, positions=q_solution.flatten().tolist(), @@ -289,72 +298,6 @@ def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.floating[An positions.append(pos) return np.array(positions) - def _solve_ik( - self, - target_pose: pinocchio.SE3, - q_init: NDArray[np.floating[Any]], - ) -> tuple[NDArray[np.floating[Any]], bool, float]: - """Solve IK using damped least-squares (Levenberg-Marquardt). - - Args: - target_pose: Target end-effector pose as SE3 - q_init: Initial joint configuration (warm-start) - - Returns: - Tuple of (joint_angles, converged, final_error) - """ - q = q_init.copy() - final_err = float("inf") - - for _ in range(self._config.ik_max_iter): - pinocchio.forwardKinematics(self._model, self._data, q) - iMd = self._data.oMi[self._ee_joint_id].actInv(target_pose) - - err = pinocchio.log(iMd).vector - final_err = float(norm(err)) - if final_err < self._config.ik_eps: - return q, True, final_err - - J = pinocchio.computeJointJacobian(self._model, self._data, q, self._ee_joint_id) - J = -np.dot(pinocchio.Jlog6(iMd.inverse()), J) - v = -J.T.dot(solve(J.dot(J.T) + self._config.ik_damp * np.eye(6), err)) - - # Clamp velocity to prevent explosion near singularities - v_norm = norm(v) - if v_norm > self._config.max_velocity: - v = v * (self._config.max_velocity / v_norm) - - q = pinocchio.integrate(self._model, q, v * self._config.ik_dt) - - return q, False, final_err - - def _safety_check( - self, q_new: NDArray[np.floating[Any]], q_current: NDArray[np.floating[Any]] - ) -> bool: - """Check if IK solution is safe to execute. - - Args: - q_new: Proposed joint positions from IK - q_current: Current joint positions - - Returns: - True if safe, False if motion should be rejected - """ - max_delta_rad = np.radians(self._config.max_joint_delta_deg) - joint_deltas = q_new - q_current - - if np.any(np.abs(joint_deltas) > max_delta_rad): - worst_joint = int(np.argmax(np.abs(joint_deltas))) - logger.warning( - f"CartesianIKTask {self._name}: rejecting motion - " - f"joint {self._joint_names_list[worst_joint]} delta " - f"{np.degrees(joint_deltas[worst_joint]):.1f}° exceeds limit " - f"{self._config.max_joint_delta_deg}°" - ) - return False - - return True - def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: """Handle preemption by higher-priority task. @@ -364,20 +307,41 @@ def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: """ if joints & self._joint_names: logger.warning( - f"CartesianIKTask {self._name} preempted by {by_task} on joints {joints}" + f"TeleopIKTask {self._name} preempted by {by_task} on joints {joints}" ) # ========================================================================= # Task-specific methods # ========================================================================= + def on_buttons(self, msg: QuestButtons) -> None: + """Handle engage/disengage from button state. + + Uses left X button with rising/falling edge detection: + - Rising edge: reset initial pose so next compute() recaptures + - Falling edge: clear task (stop tracking) + """ + engage = msg.left_x + + if engage and not self._prev_engage: + # Rising edge: prepare for new tracking session + logger.info(f"TeleopIKTask {self._name}: engage") + with self._lock: + self._initial_ee_pose = None + elif not engage and self._prev_engage: + # Falling edge: stop tracking + logger.info(f"TeleopIKTask {self._name}: disengage") + self.clear() + + self._prev_engage = engage + def set_target_pose(self, pose: Pose | PoseStamped, t_now: float) -> bool: """Set target end-effector pose from delta. Treats incoming pose as a delta from the initial EE pose. The initial pose is automatically captured on the first compute() call after activation. - Call this from your teleop callback, visual servoing, or other input source. + Call this from your teleop callback. Args: pose: Delta pose from teleop (position offset + orientation delta) @@ -386,7 +350,7 @@ def set_target_pose(self, pose: Pose | PoseStamped, t_now: float) -> bool: Returns: True if accepted """ - delta_se3 = self._pose_to_se3(pose) + delta_se3 = PinocchioIK.pose_to_se3(pose) with self._lock: self._target_pose = delta_se3 # Store delta, will apply to initial in compute() @@ -412,12 +376,10 @@ def set_target_pose_dict( required_keys = {"x", "y", "z", "roll", "pitch", "yaw"} if not required_keys.issubset(pose.keys()): missing = required_keys - set(pose.keys()) - logger.warning(f"CartesianIKTask {self._name}: missing pose keys {missing}") + logger.warning(f"TeleopIKTask {self._name}: missing pose keys {missing}") return False - position = np.array([pose["x"], pose["y"], pose["z"]]) - rotation = pinocchio.rpy.rpyToMatrix(pose["roll"], pose["pitch"], pose["yaw"]) - target_se3 = pinocchio.SE3(rotation, position) + target_se3 = PinocchioIK.pose_dict_to_se3(pose) with self._lock: self._target_pose = target_se3 @@ -426,36 +388,17 @@ def set_target_pose_dict( return True - def _pose_to_se3(self, pose: Pose | PoseStamped) -> pinocchio.SE3: - """Convert a Pose message to pinocchio SE3. - - Uses quaternion directly to avoid Euler angle conversion issues. - """ - # Handle both Pose and PoseStamped - if hasattr(pose, "position"): - # Assume Pose or PoseStamped with position/orientation attributes - position = np.array([pose.x, pose.y, pose.z]) - quat = pose.orientation - rotation = pinocchio.Quaternion(quat.w, quat.x, quat.y, quat.z).toRotationMatrix() - else: - # Assume it has x, y, z directly - position = np.array([pose.x, pose.y, pose.z]) - quat = pose.orientation - rotation = pinocchio.Quaternion(quat.w, quat.x, quat.y, quat.z).toRotationMatrix() - - return pinocchio.SE3(rotation, position) - def start(self) -> None: """Activate the task (start accepting and outputting commands).""" with self._lock: self._active = True - logger.info(f"CartesianIKTask {self._name} started") + logger.info(f"TeleopIKTask {self._name} started") def stop(self) -> None: """Deactivate the task (stop outputting commands).""" with self._lock: self._active = False - logger.info(f"CartesianIKTask {self._name} stopped") + logger.info(f"TeleopIKTask {self._name} stopped") def clear(self) -> None: """Clear current target, initial pose, and deactivate.""" @@ -463,7 +406,7 @@ def clear(self) -> None: self._target_pose = None self._initial_ee_pose = None self._active = False - logger.info(f"CartesianIKTask {self._name} cleared") + logger.info(f"TeleopIKTask {self._name} cleared") def capture_initial_pose(self, state: CoordinatorState) -> bool: """Capture current EE pose as the initial/base pose for delta mode. @@ -480,19 +423,18 @@ def capture_initial_pose(self, state: CoordinatorState) -> bool: q_current = self._get_current_joints(state) if q_current is None: logger.warning( - f"CartesianIKTask {self._name}: cannot capture initial pose, " + f"TeleopIKTask {self._name}: cannot capture initial pose, " "joint state unavailable" ) return False - pinocchio.forwardKinematics(self._model, self._data, q_current) - initial_pose = self._data.oMi[self._ee_joint_id].copy() + initial_pose = self._ik.forward_kinematics(q_current) with self._lock: self._initial_ee_pose = initial_pose logger.info( - f"CartesianIKTask {self._name}: captured initial EE pose at " + f"TeleopIKTask {self._name}: captured initial EE pose at " f"[{initial_pose.translation[0]:.3f}, {initial_pose.translation[1]:.3f}, " f"{initial_pose.translation[2]:.3f}]" ) @@ -518,8 +460,7 @@ def get_current_ee_pose(self, state: CoordinatorState) -> pinocchio.SE3 | None: if q_current is None: return None - pinocchio.forwardKinematics(self._model, self._data, q_current) - return self._data.oMi[self._ee_joint_id].copy() + return self._ik.forward_kinematics(q_current) def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pinocchio.SE3: """Compute end-effector pose from joint positions. @@ -530,11 +471,10 @@ def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pino Returns: End-effector pose as SE3 """ - pinocchio.forwardKinematics(self._model, self._data, joint_positions) - return self._data.oMi[self._ee_joint_id].copy() + return self._ik.forward_kinematics(joint_positions) __all__ = [ - "CartesianIKTask", - "CartesianIKTaskConfig", + "TeleopIKTask", + "TeleopIKTaskConfig", ] From 4a3258ee248f7c986abf76743d17422a6d5d65ba Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 10 Feb 2026 14:39:01 -0800 Subject: [PATCH 04/27] Feat: taking in QuestButtons stream --- dimos/control/coordinator.py | 31 ++++++++++++++++++++++++++----- 1 file changed, 26 insertions(+), 5 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index ae05d1de59..879e7677dc 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -44,6 +44,7 @@ from dimos.msgs.sensor_msgs import ( JointState, # noqa: TC001 - needed at runtime for Out[JointState] ) +from dimos.teleop.quest.quest_types import QuestButtons # noqa: TC001 - needed for teleop buttons from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -145,6 +146,9 @@ class ControlCoordinator(Module[ControlCoordinatorConfig]): # Uses frame_id as task name for routing cartesian_command: In[PoseStamped] + # Input: Teleop buttons for engage/disengage signaling + buttons: In[QuestButtons] + config: ControlCoordinatorConfig default_config = ControlCoordinatorConfig @@ -168,6 +172,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # Subscription handles for streaming commands self._joint_command_unsub: Callable[[], None] | None = None self._cartesian_command_unsub: Callable[[], None] | None = None + self._buttons_unsub: Callable[[], None] | None = None logger.info(f"ControlCoordinator initialized at {self.config.tick_rate}Hz") @@ -464,11 +469,14 @@ def _on_cartesian_command(self, msg: PoseStamped) -> None: logger.warning(f"Cartesian command for unknown task: {task_name}") return - # Route to CartesianIKTask - if hasattr(task, "set_target_pose"): - task.set_target_pose(msg, t_now) # type: ignore[attr-defined] - else: - logger.warning(f"Task {task_name} does not support set_target_pose") + task.set_target_pose(msg, t_now) # type: ignore[attr-defined] + + def _on_buttons(self, msg: QuestButtons) -> None: + """Forward button state to all tasks that accept it.""" + with self._task_lock: + for task in self._tasks.values(): + if hasattr(task, "on_buttons"): + task.on_buttons(msg) # type: ignore[attr-defined] @rpc def task_invoke( @@ -559,6 +567,16 @@ def start(self) -> None: except Exception as e: logger.warning(f"Could not subscribe to cartesian_command: {e}") + # Also subscribe to buttons for engage/disengage + try: + if self.buttons.transport: + self._buttons_unsub = self.buttons.subscribe( + self._on_buttons + ) + logger.info("Subscribed to buttons for engage/disengage") + except Exception as e: + logger.debug(f"Could not subscribe to buttons: {e}") + logger.info(f"ControlCoordinator started at {self.config.tick_rate}Hz") @rpc @@ -573,6 +591,9 @@ def stop(self) -> None: if self._cartesian_command_unsub: self._cartesian_command_unsub() self._cartesian_command_unsub = None + if self._buttons_unsub: + self._buttons_unsub() + self._buttons_unsub = None if self._tick_loop: self._tick_loop.stop() From a70f34389a8a0079ce782d87455090fa4d25db3e Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 10 Feb 2026 14:40:51 -0800 Subject: [PATCH 05/27] Fix: pre-commit fixes --- dimos/control/coordinator.py | 4 +--- dimos/control/tasks/cartesian_ik_task.py | 9 ++++----- dimos/control/tasks/teleop_task.py | 18 +++++++----------- .../planning/kinematics/pinocchio_ik.py | 16 ++++------------ dimos/teleop/blueprints.py | 5 +---- 5 files changed, 17 insertions(+), 35 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 879e7677dc..2ec47fafb0 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -570,9 +570,7 @@ def start(self) -> None: # Also subscribe to buttons for engage/disengage try: if self.buttons.transport: - self._buttons_unsub = self.buttons.subscribe( - self._on_buttons - ) + self._buttons_unsub = self.buttons.subscribe(self._on_buttons) logger.info("Subscribed to buttons for engage/disengage") except Exception as e: logger.debug(f"Could not subscribe to buttons: {e}") diff --git a/dimos/control/tasks/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task.py index 8acd496bad..f6f9f180b7 100644 --- a/dimos/control/tasks/cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task.py @@ -24,12 +24,10 @@ from __future__ import annotations from dataclasses import dataclass -from pathlib import Path import threading from typing import TYPE_CHECKING, Any import numpy as np -import pinocchio # type: ignore[import-untyped] from dimos.control.task import ( ControlMode, @@ -47,7 +45,10 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: + from pathlib import Path + from numpy.typing import NDArray + import pinocchio from dimos.msgs.geometry_msgs import Pose, PoseStamped @@ -141,9 +142,7 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: dt=config.ik_dt, max_velocity=config.max_velocity, ) - self._ik = PinocchioIK.from_model_path( - config.model_path, config.ee_joint_id, ik_config - ) + self._ik = PinocchioIK.from_model_path(config.model_path, config.ee_joint_id, ik_config) # Validate DOF matches joint names if self._ik.nq != self._num_joints: diff --git a/dimos/control/tasks/teleop_task.py b/dimos/control/tasks/teleop_task.py index ffe36618e7..2272895d25 100644 --- a/dimos/control/tasks/teleop_task.py +++ b/dimos/control/tasks/teleop_task.py @@ -26,7 +26,6 @@ from __future__ import annotations from dataclasses import dataclass -from pathlib import Path import threading from typing import TYPE_CHECKING, Any @@ -40,7 +39,6 @@ JointCommandOutput, ResourceClaim, ) -from dimos.teleop.quest.quest_types import QuestButtons from dimos.manipulation.planning.kinematics.pinocchio_ik import ( PinocchioIK, PinocchioIKConfig, @@ -50,9 +48,12 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: + from pathlib import Path + from numpy.typing import NDArray from dimos.msgs.geometry_msgs import Pose, PoseStamped + from dimos.teleop.quest.quest_types import QuestButtons logger = setup_logger() @@ -84,7 +85,7 @@ class TeleopIKTaskConfig: ik_eps: float = 1e-4 ik_damp: float = 1e-2 ik_dt: float = 1.0 - + max_joint_delta_deg: float = 20.0 # ~500°/s at 100Hz max_velocity: float = 10.0 @@ -145,9 +146,7 @@ def __init__(self, name: str, config: TeleopIKTaskConfig) -> None: dt=config.ik_dt, max_velocity=config.max_velocity, ) - self._ik = PinocchioIK.from_model_path( - config.model_path, config.ee_joint_id, ik_config - ) + self._ik = PinocchioIK.from_model_path(config.model_path, config.ee_joint_id, ik_config) # Validate DOF matches joint names if self._ik.nq != self._num_joints: @@ -306,9 +305,7 @@ def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: joints: Joints that were preempted """ if joints & self._joint_names: - logger.warning( - f"TeleopIKTask {self._name} preempted by {by_task} on joints {joints}" - ) + logger.warning(f"TeleopIKTask {self._name} preempted by {by_task} on joints {joints}") # ========================================================================= # Task-specific methods @@ -423,8 +420,7 @@ def capture_initial_pose(self, state: CoordinatorState) -> bool: q_current = self._get_current_joints(state) if q_current is None: logger.warning( - f"TeleopIKTask {self._name}: cannot capture initial pose, " - "joint state unavailable" + f"TeleopIKTask {self._name}: cannot capture initial pose, joint state unavailable" ) return False diff --git a/dimos/manipulation/planning/kinematics/pinocchio_ik.py b/dimos/manipulation/planning/kinematics/pinocchio_ik.py index e689120dfc..3db800801e 100644 --- a/dimos/manipulation/planning/kinematics/pinocchio_ik.py +++ b/dimos/manipulation/planning/kinematics/pinocchio_ik.py @@ -198,9 +198,7 @@ def solve( if final_err < cfg.eps: return q, True, final_err - J = pinocchio.computeJointJacobian( - self._model, self._data, q, self._ee_joint_id - ) + J = pinocchio.computeJointJacobian(self._model, self._data, q, self._ee_joint_id) J = -np.dot(pinocchio.Jlog6(iMd.inverse()), J) v = -J.T.dot(solve(J.dot(J.T) + cfg.damp * np.eye(6), err)) @@ -217,9 +215,7 @@ def solve( # Forward Kinematics # ========================================================================= - def forward_kinematics( - self, joint_positions: NDArray[np.floating[Any]] - ) -> pinocchio.SE3: + def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pinocchio.SE3: """Compute end-effector pose from joint positions. Args: @@ -250,9 +246,7 @@ def pose_to_se3(pose: Pose | PoseStamped) -> pinocchio.SE3: """ position = np.array([pose.x, pose.y, pose.z]) quat = pose.orientation - rotation = pinocchio.Quaternion( - quat.w, quat.x, quat.y, quat.z - ).toRotationMatrix() + rotation = pinocchio.Quaternion(quat.w, quat.x, quat.y, quat.z).toRotationMatrix() return pinocchio.SE3(rotation, position) @staticmethod @@ -269,9 +263,7 @@ def pose_dict_to_se3(pose: dict[str, float]) -> pinocchio.SE3: KeyError: If required keys are missing """ position = np.array([pose["x"], pose["y"], pose["z"]]) - rotation = pinocchio.rpy.rpyToMatrix( - pose["roll"], pose["pitch"], pose["yaw"] - ) + rotation = pinocchio.rpy.rpyToMatrix(pose["roll"], pose["pitch"], pose["yaw"]) return pinocchio.SE3(rotation, position) diff --git a/dimos/teleop/blueprints.py b/dimos/teleop/blueprints.py index 176cf3a3a0..6843791e6e 100644 --- a/dimos/teleop/blueprints.py +++ b/dimos/teleop/blueprints.py @@ -55,10 +55,7 @@ # Combined blueprint: teleop + xarm6 CartesianIK coordinator # Usage: dimos run arm-teleop-xarm6 -arm_teleop_xarm6 = autoconnect( - arm_teleop_module(), - coordinator_cartesian_ik_xarm6 -).transports( +arm_teleop_xarm6 = autoconnect(arm_teleop_module(), coordinator_cartesian_ik_xarm6).transports( { # Teleop outputs -> coordinator inputs (same channel for pub/sub) ("right_controller_output", PoseStamped): LCMTransport( From 299cb63472edd417d70ad4fcb3c1c30e24c38711 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 11 Feb 2026 14:42:51 -0800 Subject: [PATCH 06/27] LFS: Xarm6 URDF created from xacro --- data/.lfs/xarm_description.tar.gz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/data/.lfs/xarm_description.tar.gz b/data/.lfs/xarm_description.tar.gz index ed2ddb780f..18b9fdc01a 100644 --- a/data/.lfs/xarm_description.tar.gz +++ b/data/.lfs/xarm_description.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:82634d767b1f96474b9273500d3f8123f0df9dd1f7e1d84f369e65b3d257f00f -size 12703173 +oid sha256:7dad8ee820f79c80a5c1799990db83a4b84bb70de209f0ae54344e44eee8f9b3 +size 12706951 From 69a77bdd4f00f2df98bd4499f801df5bac739662 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 11 Feb 2026 14:48:48 -0800 Subject: [PATCH 07/27] Feat: blueprints added for teleop xarm, piper, dual tasks --- dimos/control/blueprints.py | 144 ++++++++++++++++++++++++++++++++++ dimos/robot/all_blueprints.py | 5 ++ dimos/teleop/blueprints.py | 46 ++++++++++- 3 files changed, 191 insertions(+), 4 deletions(-) diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py index 323c850046..735896412c 100644 --- a/dimos/control/blueprints.py +++ b/dimos/control/blueprints.py @@ -35,6 +35,8 @@ from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs import PoseStamped from dimos.msgs.sensor_msgs import JointState +from dimos.teleop.quest.quest_types import QuestButtons + # ============================================================================= # Single Arm Blueprints @@ -399,6 +401,14 @@ def _get_piper_model_path() -> str: return str(piper_path / "mujoco_model" / "piper_no_gripper_description.xml") +def _get_xarm6_model_path() -> str: + """Get path to xarm6 URDF model for IK solver.""" + from dimos.utils.data import get_data + + xarm_path = get_data("xarm_description") + return str(xarm_path / "urdf" / "xarm6.urdf") + + # Mock 6-DOF arm with CartesianIK coordinator_cartesian_ik_mock = control_coordinator( tick_rate=100.0, @@ -466,6 +476,136 @@ def _get_piper_model_path() -> str: ) +# ============================================================================= +# Teleop IK Blueprints (VR teleoperation with internal Pinocchio IK) +# ============================================================================= + +# Single XArm6 with TeleopIK +coordinator_teleop_xarm6 = control_coordinator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[ + HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 6), + adapter_type="xarm", + address="192.168.1.210", + auto_enable=True, + ), + ], + tasks=[ + TaskConfig( + name="teleop_xarm", + type="teleop_ik", + joint_names=[f"arm_joint{i + 1}" for i in range(6)], + priority=10, + model_path=_get_xarm6_model_path(), + ee_joint_id=6, + hand="right", + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("cartesian_command", PoseStamped): LCMTransport( + "/coordinator/cartesian_command", PoseStamped + ), + ("buttons", QuestButtons): LCMTransport("/teleop/buttons", QuestButtons), + } +) + +# Single Piper with TeleopIK +coordinator_teleop_piper = control_coordinator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[ + HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 6), + adapter_type="piper", + address="can0", + auto_enable=True, + ), + ], + tasks=[ + TaskConfig( + name="teleop_piper", + type="teleop_ik", + joint_names=[f"arm_joint{i + 1}" for i in range(6)], + priority=10, + model_path=_get_piper_model_path(), + ee_joint_id=6, + hand="left", + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("cartesian_command", PoseStamped): LCMTransport( + "/coordinator/cartesian_command", PoseStamped + ), + ("buttons", QuestButtons): LCMTransport("/teleop/buttons", QuestButtons), + } +) + +# Dual arm teleop: XArm6 + Piper with TeleopIK +coordinator_teleop_dual = control_coordinator( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[ + HardwareComponent( + hardware_id="xarm_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("xarm_arm", 6), + adapter_type="xarm", + address="192.168.1.210", + auto_enable=True, + ), + HardwareComponent( + hardware_id="piper_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("piper_arm", 6), + adapter_type="piper", + address="can0", + auto_enable=True, + ), + ], + tasks=[ + TaskConfig( + name="teleop_xarm", + type="teleop_ik", + joint_names=[f"xarm_arm_joint{i + 1}" for i in range(6)], + priority=10, + model_path=_get_xarm6_model_path(), + ee_joint_id=6, + hand="left", + ), + TaskConfig( + name="teleop_piper", + type="teleop_ik", + joint_names=[f"piper_arm_joint{i + 1}" for i in range(6)], + priority=10, + model_path=_get_piper_model_path(), + ee_joint_id=6, + hand="right", + ), + ], +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + ("cartesian_command", PoseStamped): LCMTransport( + "/coordinator/cartesian_command", PoseStamped + ), + ("buttons", QuestButtons): LCMTransport("/teleop/buttons", QuestButtons), + } +) + + # ============================================================================= # Raw Blueprints (for programmatic setup) # ============================================================================= @@ -493,6 +633,10 @@ def _get_piper_model_path() -> str: "coordinator_cartesian_ik_piper", # Streaming control "coordinator_combined_xarm6", + # Teleop IK + "coordinator_teleop_dual", + "coordinator_teleop_piper", + "coordinator_teleop_xarm6", # Dual arm "coordinator_dual_mock", "coordinator_dual_xarm", diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 70cc455d34..914439b60c 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -17,7 +17,10 @@ all_blueprints = { "arm-teleop": "dimos.teleop.blueprints:arm_teleop", + "arm-teleop-dual": "dimos.teleop.blueprints:arm_teleop_dual", + "arm-teleop-piper": "dimos.teleop.blueprints:arm_teleop_piper", "arm-teleop-visualizing": "dimos.teleop.blueprints:arm_teleop_visualizing", + "arm-teleop-xarm6": "dimos.teleop.blueprints:arm_teleop_xarm6", "coordinator-basic": "dimos.control.blueprints:coordinator_basic", "coordinator-cartesian-ik-mock": "dimos.control.blueprints:coordinator_cartesian_ik_mock", "coordinator-cartesian-ik-piper": "dimos.control.blueprints:coordinator_cartesian_ik_piper", @@ -27,6 +30,8 @@ "coordinator-mock": "dimos.control.blueprints:coordinator_mock", "coordinator-piper": "dimos.control.blueprints:coordinator_piper", "coordinator-piper-xarm": "dimos.control.blueprints:coordinator_piper_xarm", + "coordinator-teleop-dual": "dimos.control.blueprints:coordinator_teleop_dual", + "coordinator-teleop-piper": "dimos.control.blueprints:coordinator_teleop_piper", "coordinator-teleop-xarm6": "dimos.control.blueprints:coordinator_teleop_xarm6", "coordinator-velocity-xarm6": "dimos.control.blueprints:coordinator_velocity_xarm6", "coordinator-xarm6": "dimos.control.blueprints:coordinator_xarm6", diff --git a/dimos/teleop/blueprints.py b/dimos/teleop/blueprints.py index 6843791e6e..bd0896737d 100644 --- a/dimos/teleop/blueprints.py +++ b/dimos/teleop/blueprints.py @@ -15,7 +15,11 @@ """Teleop blueprints for testing and deployment.""" -from dimos.control.blueprints import coordinator_cartesian_ik_xarm6 +from dimos.control.blueprints import ( + coordinator_teleop_dual, + coordinator_teleop_piper, + coordinator_teleop_xarm6, +) from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs import PoseStamped @@ -55,9 +59,11 @@ # Combined blueprint: teleop + xarm6 CartesianIK coordinator # Usage: dimos run arm-teleop-xarm6 -arm_teleop_xarm6 = autoconnect(arm_teleop_module(), coordinator_cartesian_ik_xarm6).transports( +arm_teleop_xarm6 = autoconnect( + arm_teleop_module(task_names={"right": "teleop_xarm"}), + coordinator_teleop_xarm6, +).transports( { - # Teleop outputs -> coordinator inputs (same channel for pub/sub) ("right_controller_output", PoseStamped): LCMTransport( "/coordinator/cartesian_command", PoseStamped ), @@ -66,4 +72,36 @@ ) -__all__ = ["arm_teleop", "arm_teleop_visualizing", "arm_teleop_xarm6"] +# Single Piper teleop: left controller -> piper arm +# Usage: dimos run arm-teleop-piper +arm_teleop_piper = autoconnect( + arm_teleop_module(task_names={"left": "teleop_piper"}), + coordinator_teleop_piper, +).transports( + { + ("left_controller_output", PoseStamped): LCMTransport( + "/coordinator/cartesian_command", PoseStamped + ), + ("buttons", QuestButtons): LCMTransport("/teleop/buttons", QuestButtons), + } +) + + +# Dual arm teleop: right -> piper, left -> xarm6 (TeleopIK) +arm_teleop_dual = autoconnect( + arm_teleop_module(task_names={"right": "teleop_piper", "left": "teleop_xarm"}), + coordinator_teleop_dual, +).transports( + { + ("right_controller_output", PoseStamped): LCMTransport( + "/coordinator/cartesian_command", PoseStamped + ), + ("left_controller_output", PoseStamped): LCMTransport( + "/coordinator/cartesian_command", PoseStamped + ), + ("buttons", QuestButtons): LCMTransport("/teleop/buttons", QuestButtons), + } +) + + +__all__ = ["arm_teleop", "arm_teleop_dual", "arm_teleop_piper", "arm_teleop_visualizing", "arm_teleop_xarm6"] From 62b7dbea808b4f788f8df987aac38f74c32c8238 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 11 Feb 2026 14:49:14 -0800 Subject: [PATCH 08/27] Feat: teleop_ik task upgrades --- dimos/control/coordinator.py | 28 ++++++-- dimos/control/tasks/teleop_task.py | 105 +++++++++++++---------------- 2 files changed, 68 insertions(+), 65 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 003ddb00bf..f29f3ed969 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -66,20 +66,21 @@ class TaskConfig: Attributes: name: Task name (e.g., "traj_arm") - type: Task type ("trajectory", "servo", "velocity", "cartesian_ik") + type: Task type ("trajectory", "servo", "velocity", "cartesian_ik", "teleop_ik") joint_names: List of joint names this task controls priority: Task priority (higher wins arbitration) - model_path: Path to URDF/MJCF for IK solver (cartesian_ik only) - ee_joint_id: End-effector joint ID in model (cartesian_ik only) + model_path: Path to URDF/MJCF for IK solver (cartesian_ik/teleop_ik only) + ee_joint_id: End-effector joint ID in model (cartesian_ik/teleop_ik only) """ name: str type: str = "trajectory" joint_names: list[str] = field(default_factory=lambda: []) priority: int = 10 - # Cartesian IK specific + # Cartesian IK / Teleop IK specific model_path: str | None = None ee_joint_id: int = 6 + hand: str = "" # teleop_ik only: "left" or "right" controller @dataclass @@ -281,6 +282,23 @@ def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask: ), ) + elif task_type == "teleop_ik": + from dimos.control.tasks.teleop_task import TeleopIKTask, TeleopIKTaskConfig + + if cfg.model_path is None: + raise ValueError(f"TeleopIKTask '{cfg.name}' requires model_path in TaskConfig") + + return TeleopIKTask( + cfg.name, + TeleopIKTaskConfig( + joint_names=cfg.joint_names, + model_path=cfg.model_path, + ee_joint_id=cfg.ee_joint_id, + priority=cfg.priority, + hand=cfg.hand, + ), + ) + else: raise ValueError(f"Unknown task type: {task_type}") @@ -583,7 +601,7 @@ def start(self) -> None: logger.warning(f"Could not subscribe to joint_command: {e}") # Subscribe to cartesian commands if any cartesian_ik tasks configured - has_cartesian_ik = any(t.type == "cartesian_ik" for t in self.config.tasks) + has_cartesian_ik = any(t.type in ("cartesian_ik", "teleop_ik") for t in self.config.tasks) if has_cartesian_ik: try: if self.cartesian_command.transport: diff --git a/dimos/control/tasks/teleop_task.py b/dimos/control/tasks/teleop_task.py index 2272895d25..e7a8813038 100644 --- a/dimos/control/tasks/teleop_task.py +++ b/dimos/control/tasks/teleop_task.py @@ -42,8 +42,6 @@ from dimos.manipulation.planning.kinematics.pinocchio_ik import ( PinocchioIK, PinocchioIKConfig, - check_joint_delta, - get_worst_joint_delta, ) from dimos.utils.logging_config import setup_logger @@ -86,8 +84,9 @@ class TeleopIKTaskConfig: ik_damp: float = 1e-2 ik_dt: float = 1.0 - max_joint_delta_deg: float = 20.0 # ~500°/s at 100Hz - max_velocity: float = 10.0 + max_joint_delta_deg: float = 5.0 # ~500°/s at 100Hz + max_velocity: float = 5.0 + hand: str = "" # "left" or "right" — which controller's primary button to listen to class TeleopIKTask(ControlTask): @@ -160,6 +159,7 @@ def __init__(self, name: str, config: TeleopIKTaskConfig) -> None: self._target_pose: pinocchio.SE3 | None = None self._last_update_time: float = 0.0 self._active = False + self._engaged = False # Controlled by on_buttons toggle; gates set_target_pose # Initial EE pose (captured when tracking starts, used for delta mode) self._initial_ee_pose: pinocchio.SE3 | None = None @@ -167,8 +167,8 @@ def __init__(self, name: str, config: TeleopIKTaskConfig) -> None: # Cache last successful IK solution for warm-starting self._last_q_solution: NDArray[np.floating[Any]] | None = None - # Button edge detection - self._prev_engage = False + # Rising edge detection for primary button (engage/disengage toggle) + self._prev_primary: bool = False logger.info( f"TeleopIKTask {name} initialized with model: {config.model_path}, " @@ -261,23 +261,18 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: f"(error={final_error:.4f}), using partial solution" ) - # 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"TeleopIKTask {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}°" - ) - return None - # Cache solution for next warm-start with self._lock: self._last_q_solution = q_solution.copy() + positions = q_solution.flatten().tolist() + # degrees = [f"{np.degrees(p):.1f}" for p in positions] + # logger.info(f"TeleopIKTask {self._name}: {dict(zip(self._joint_names_list, degrees))}") + # return None + # TODO: uncomment when joint values look good return JointCommandOutput( joint_names=self._joint_names_list, - positions=q_solution.flatten().tolist(), + positions=positions, mode=ControlMode.SERVO_POSITION, ) @@ -311,26 +306,42 @@ def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: # Task-specific methods # ========================================================================= - def on_buttons(self, msg: QuestButtons) -> None: - """Handle engage/disengage from button state. + def engage(self) -> None: + """Prepare for new tracking session. - Uses left X button with rising/falling edge detection: - - Rising edge: reset initial pose so next compute() recaptures - - Falling edge: clear task (stop tracking) + Resets initial EE pose so next compute() recaptures from current position. + Called via on_buttons when primary button toggles to engaged. """ - engage = msg.left_x + logger.info(f"TeleopIKTask {self._name}: engage") + with self._lock: + self._engaged = True + self._initial_ee_pose = None - if engage and not self._prev_engage: - # Rising edge: prepare for new tracking session - logger.info(f"TeleopIKTask {self._name}: engage") - with self._lock: - self._initial_ee_pose = None - elif not engage and self._prev_engage: - # Falling edge: stop tracking - logger.info(f"TeleopIKTask {self._name}: disengage") - self.clear() + def disengage(self) -> None: + """Stop tracking. Called via on_buttons when primary button toggles to disengaged.""" + logger.info(f"TeleopIKTask {self._name}: disengage") + self.clear() + + def on_buttons(self, msg: QuestButtons) -> None: + """Toggle engage/disengage on primary button rising edge. - self._prev_engage = engage + Checks only the button matching self._config.hand (left_x or right_a). + If hand is not set, listens to both. + """ + hand = self._config.hand + if hand == "left": + primary = msg.left_x + elif hand == "right": + primary = msg.right_a + else: + primary = msg.left_x or msg.right_a + + if primary and not self._prev_primary: + if self._engaged: + self.disengage() + else: + self.engage() + self._prev_primary = primary def set_target_pose(self, pose: Pose | PoseStamped, t_now: float) -> bool: """Set target end-effector pose from delta. @@ -403,6 +414,7 @@ def clear(self) -> None: self._target_pose = None self._initial_ee_pose = None self._active = False + self._engaged = False logger.info(f"TeleopIKTask {self._name} cleared") def capture_initial_pose(self, state: CoordinatorState) -> bool: @@ -436,38 +448,11 @@ def capture_initial_pose(self, state: CoordinatorState) -> bool: ) return True - def is_tracking(self) -> bool: - """Check if actively receiving and outputting commands.""" - with self._lock: - return self._active and self._target_pose is not None - def get_current_ee_pose(self, state: CoordinatorState) -> pinocchio.SE3 | None: - """Get current end-effector pose via forward kinematics. - Useful for getting initial pose before starting tracking. - Args: - state: Current coordinator state - - Returns: - Current EE pose as SE3, or None if joint state unavailable - """ - q_current = self._get_current_joints(state) - if q_current is None: - return None - - return self._ik.forward_kinematics(q_current) - - def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pinocchio.SE3: - """Compute end-effector pose from joint positions. - Args: - joint_positions: Joint angles in radians - Returns: - End-effector pose as SE3 - """ - return self._ik.forward_kinematics(joint_positions) __all__ = [ From e9f20caeea0bc829e9bcf0f2d0d969c395087dbf Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 11 Feb 2026 14:55:03 -0800 Subject: [PATCH 09/27] Feat: arm teleop support --- dimos/teleop/quest/quest_extensions.py | 44 ++++++++++++++++++++++++-- 1 file changed, 41 insertions(+), 3 deletions(-) diff --git a/dimos/teleop/quest/quest_extensions.py b/dimos/teleop/quest/quest_extensions.py index 1131ab0dec..99bc26239c 100644 --- a/dimos/teleop/quest/quest_extensions.py +++ b/dimos/teleop/quest/quest_extensions.py @@ -16,12 +16,14 @@ """Quest teleop module extensions and subclasses. Available subclasses: - - ArmTeleopModule: Per-hand toggle engage (X/A press to toggle) + - ArmTeleopModule: Per-hand toggle engage (X/A press to toggle), task name routing - TwistTeleopModule: Outputs Twist instead of PoseStamped - VisualizingTeleopModule: Adds Rerun visualization (uses toggle engage) """ -from dataclasses import dataclass +from __future__ import annotations + +from dataclasses import dataclass, field from typing import Any from dimos.core import Out @@ -75,21 +77,56 @@ def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: self.right_twist.publish(twist) +@dataclass +class ArmTeleopConfig(QuestTeleopConfig): + """Configuration for ArmTeleopModule. + + Attributes: + task_names: Mapping of Hand -> coordinator task name. Used to set + frame_id on output PoseStamped so the coordinator routes each + hand's commands to the correct TeleopIKTask. + """ + + task_names: dict[str, str] = field(default_factory=dict) + + class ArmTeleopModule(QuestTeleopModule): - """Quest teleop with per-hand toggle engage. + """Quest teleop with per-hand toggle engage and task name routing. Each controller's primary button (X for left, A for right) toggles that hand's engage state independently. + When task_names is configured, output PoseStamped messages have their + frame_id set to the task name, enabling the coordinator to route + each hand's commands to the correct TeleopIKTask. + Outputs: - left_controller_output: PoseStamped (inherited) - right_controller_output: PoseStamped (inherited) - buttons: QuestButtons (inherited) """ + default_config = ArmTeleopConfig + def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) self._prev_primary: dict[Hand, bool] = {Hand.LEFT: False, Hand.RIGHT: False} + cfg: ArmTeleopConfig = self.config # type: ignore[assignment] + self._task_names: dict[Hand, str] = { + Hand[k.upper()]: v for k, v in cfg.task_names.items() + } + + def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: + """Stamp frame_id with task name and publish.""" + task_name = self._task_names.get(hand) + if task_name: + output_msg = PoseStamped( + position=output_msg.position, + orientation=output_msg.orientation, + ts=output_msg.ts, + frame_id=task_name, + ) + super()._publish_msg(hand, output_msg) def _handle_engage(self) -> None: """Toggle per-hand engage on primary button rising edge.""" @@ -147,6 +184,7 @@ def _get_output_pose(self, hand: Hand) -> PoseStamped | None: visualizing_teleop_module = VisualizingTeleopModule.blueprint __all__ = [ + "ArmTeleopConfig", "ArmTeleopModule", "TwistTeleopModule", "VisualizingTeleopModule", From 7f0133ee67c244014a44f0832b0b627fea155094 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 11 Feb 2026 14:55:30 -0800 Subject: [PATCH 10/27] Fix: pre-commit errors --- dimos/control/blueprints.py | 9 ++++----- dimos/control/tasks/teleop_task.py | 6 ------ dimos/teleop/blueprints.py | 8 +++++++- dimos/teleop/quest/quest_extensions.py | 10 +++++----- 4 files changed, 16 insertions(+), 17 deletions(-) diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py index 735896412c..52e2d67e80 100644 --- a/dimos/control/blueprints.py +++ b/dimos/control/blueprints.py @@ -37,7 +37,6 @@ from dimos.msgs.sensor_msgs import JointState from dimos.teleop.quest.quest_types import QuestButtons - # ============================================================================= # Single Arm Blueprints # ============================================================================= @@ -633,10 +632,6 @@ def _get_xarm6_model_path() -> str: "coordinator_cartesian_ik_piper", # Streaming control "coordinator_combined_xarm6", - # Teleop IK - "coordinator_teleop_dual", - "coordinator_teleop_piper", - "coordinator_teleop_xarm6", # Dual arm "coordinator_dual_mock", "coordinator_dual_xarm", @@ -644,6 +639,10 @@ def _get_xarm6_model_path() -> str: "coordinator_mock", "coordinator_piper", "coordinator_piper_xarm", + # Teleop IK + "coordinator_teleop_dual", + "coordinator_teleop_piper", + "coordinator_teleop_xarm6", "coordinator_teleop_xarm6", "coordinator_velocity_xarm6", "coordinator_xarm6", diff --git a/dimos/control/tasks/teleop_task.py b/dimos/control/tasks/teleop_task.py index e7a8813038..36763da698 100644 --- a/dimos/control/tasks/teleop_task.py +++ b/dimos/control/tasks/teleop_task.py @@ -449,12 +449,6 @@ def capture_initial_pose(self, state: CoordinatorState) -> bool: return True - - - - - - __all__ = [ "TeleopIKTask", "TeleopIKTaskConfig", diff --git a/dimos/teleop/blueprints.py b/dimos/teleop/blueprints.py index bd0896737d..6f3ed7cada 100644 --- a/dimos/teleop/blueprints.py +++ b/dimos/teleop/blueprints.py @@ -104,4 +104,10 @@ ) -__all__ = ["arm_teleop", "arm_teleop_dual", "arm_teleop_piper", "arm_teleop_visualizing", "arm_teleop_xarm6"] +__all__ = [ + "arm_teleop", + "arm_teleop_dual", + "arm_teleop_piper", + "arm_teleop_visualizing", + "arm_teleop_xarm6", +] diff --git a/dimos/teleop/quest/quest_extensions.py b/dimos/teleop/quest/quest_extensions.py index 99bc26239c..a419e9904b 100644 --- a/dimos/teleop/quest/quest_extensions.py +++ b/dimos/teleop/quest/quest_extensions.py @@ -24,9 +24,8 @@ from __future__ import annotations from dataclasses import dataclass, field -from typing import Any +from typing import TYPE_CHECKING, Any -from dimos.core import Out from dimos.msgs.geometry_msgs import PoseStamped, TwistStamped from dimos.teleop.quest.quest_teleop_module import Hand, QuestTeleopConfig, QuestTeleopModule from dimos.teleop.utils.teleop_visualization import ( @@ -34,6 +33,9 @@ visualize_pose, ) +if TYPE_CHECKING: + from dimos.core import Out + @dataclass class TwistTeleopConfig(QuestTeleopConfig): @@ -112,9 +114,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) self._prev_primary: dict[Hand, bool] = {Hand.LEFT: False, Hand.RIGHT: False} cfg: ArmTeleopConfig = self.config # type: ignore[assignment] - self._task_names: dict[Hand, str] = { - Hand[k.upper()]: v for k, v in cfg.task_names.items() - } + self._task_names: dict[Hand, str] = {Hand[k.upper()]: v for k, v in cfg.task_names.items()} def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: """Stamp frame_id with task name and publish.""" From 848311fb7eb1bf6a6988958961e400add197f184 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 11 Feb 2026 15:05:48 -0800 Subject: [PATCH 11/27] Fix: changed xarm6 urdf path in lfs --- data/.lfs/xarm_description.tar.gz | 4 ++-- dimos/control/blueprints.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/data/.lfs/xarm_description.tar.gz b/data/.lfs/xarm_description.tar.gz index 18b9fdc01a..e7c473e44f 100644 --- a/data/.lfs/xarm_description.tar.gz +++ b/data/.lfs/xarm_description.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:7dad8ee820f79c80a5c1799990db83a4b84bb70de209f0ae54344e44eee8f9b3 -size 12706951 +oid sha256:0b247baea9b52f583824b0bc90e1e38640e6090aca7f4ae1a1a72a24e5a66fee +size 12706830 diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py index 52e2d67e80..60e703e57c 100644 --- a/dimos/control/blueprints.py +++ b/dimos/control/blueprints.py @@ -405,7 +405,7 @@ def _get_xarm6_model_path() -> str: from dimos.utils.data import get_data xarm_path = get_data("xarm_description") - return str(xarm_path / "urdf" / "xarm6.urdf") + return str(xarm_path / "urdf" / "xarm6" / "xarm6.urdf") # Mock 6-DOF arm with CartesianIK From a4a3217d31f7b4955b50318b7b46e64c77cf8358 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 12 Feb 2026 11:34:55 -0800 Subject: [PATCH 12/27] cleanup --- dimos/control/blueprints.py | 1 - dimos/control/coordinator.py | 6 +- dimos/control/tasks/__init__.py | 6 + dimos/control/tasks/cartesian_ik_task.py | 14 +- dimos/control/tasks/teleop_task.py | 146 +++++------------- .../planning/kinematics/pinocchio_ik.py | 2 +- dimos/teleop/blueprints.py | 7 +- dimos/teleop/quest/quest_extensions.py | 20 +-- 8 files changed, 65 insertions(+), 137 deletions(-) diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py index 60e703e57c..2c4bd5c647 100644 --- a/dimos/control/blueprints.py +++ b/dimos/control/blueprints.py @@ -643,7 +643,6 @@ def _get_xarm6_model_path() -> str: "coordinator_teleop_dual", "coordinator_teleop_piper", "coordinator_teleop_xarm6", - "coordinator_teleop_xarm6", "coordinator_velocity_xarm6", "coordinator_xarm6", "coordinator_xarm7", diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index f29f3ed969..bad14eeaea 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -487,7 +487,7 @@ def _on_cartesian_command(self, msg: PoseStamped) -> None: logger.warning(f"Cartesian command for unknown task: {task_name}") return - task.set_target_pose(msg, t_now) # type: ignore[attr-defined] + task.on_cartesian_command(msg, t_now) # type: ignore[attr-defined] def _on_buttons(self, msg: QuestButtons) -> None: """Forward button state to all tasks that accept it.""" @@ -608,10 +608,10 @@ def start(self) -> None: self._cartesian_command_unsub = self.cartesian_command.subscribe( self._on_cartesian_command ) - logger.info("Subscribed to cartesian_command for CartesianIK tasks") + logger.info("Subscribed to cartesian_command for CartesianIK/TeleopIK tasks") else: logger.warning( - "CartesianIK tasks configured but no transport set for cartesian_command. " + "CartesianIK/TeleopIK tasks configured but no transport set for cartesian_command. " "Use task_invoke RPC or set transport via blueprint." ) except Exception as e: diff --git a/dimos/control/tasks/__init__.py b/dimos/control/tasks/__init__.py index bbb77dc129..5b869b01f9 100644 --- a/dimos/control/tasks/__init__.py +++ b/dimos/control/tasks/__init__.py @@ -22,6 +22,10 @@ JointServoTask, JointServoTaskConfig, ) +from dimos.control.tasks.teleop_task import ( + TeleopIKTask, + TeleopIKTaskConfig, +) from dimos.control.tasks.trajectory_task import ( JointTrajectoryTask, JointTrajectoryTaskConfig, @@ -40,4 +44,6 @@ "JointTrajectoryTaskConfig", "JointVelocityTask", "JointVelocityTaskConfig", + "TeleopIKTask", + "TeleopIKTaskConfig", ] diff --git a/dimos/control/tasks/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task.py index f6f9f180b7..a8e4fc455c 100644 --- a/dimos/control/tasks/cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task.py @@ -48,7 +48,7 @@ from pathlib import Path from numpy.typing import NDArray - import pinocchio + import pinocchio # type: ignore[import-untyped] from dimos.msgs.geometry_msgs import Pose, PoseStamped @@ -89,7 +89,7 @@ class CartesianIKTaskConfig: class CartesianIKTask(ControlTask): """Cartesian control task with internal Pinocchio IK solver. - Accepts streaming cartesian poses via set_target_pose() and computes IK + Accepts streaming cartesian poses via on_cartesian_command() and computes IK internally to output joint commands. Uses current joint state from CoordinatorState as IK warm-start for fast convergence. @@ -113,7 +113,7 @@ class CartesianIKTask(ControlTask): >>> task.start() >>> >>> # From teleop callback or other source: - >>> task.set_target_pose(pose_stamped, t_now=time.perf_counter()) + >>> task.on_cartesian_command(pose_stamped, t_now=time.perf_counter()) """ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: @@ -279,10 +279,8 @@ def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: # Task-specific methods # ========================================================================= - def set_target_pose(self, pose: Pose | PoseStamped, t_now: float) -> bool: - """Set target end-effector pose. - - Call this from your teleop callback, visual servoing, or other input source. + def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool: + """Handle incoming cartesian command (target EE pose). Args: pose: Target end-effector pose (Pose or PoseStamped) @@ -300,7 +298,7 @@ def set_target_pose(self, pose: Pose | PoseStamped, t_now: float) -> bool: return True - def set_target_pose_dict( + def on_cartesian_command_dict( self, pose: dict[str, float], t_now: float, diff --git a/dimos/control/tasks/teleop_task.py b/dimos/control/tasks/teleop_task.py index 36763da698..705479d679 100644 --- a/dimos/control/tasks/teleop_task.py +++ b/dimos/control/tasks/teleop_task.py @@ -42,6 +42,7 @@ from dimos.manipulation.planning.kinematics.pinocchio_ik import ( PinocchioIK, PinocchioIKConfig, + check_joint_delta, ) from dimos.utils.logging_config import setup_logger @@ -92,7 +93,7 @@ class TeleopIKTaskConfig: class TeleopIKTask(ControlTask): """Teleop cartesian control task with internal Pinocchio IK solver. - Accepts streaming cartesian delta poses via set_target_pose() and computes IK + 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 or explicit capture_initial_pose call). @@ -116,7 +117,7 @@ class TeleopIKTask(ControlTask): >>> task.start() >>> >>> # From teleop callback: - >>> task.set_target_pose(delta_pose, t_now=time.perf_counter()) + >>> task.on_cartesian_command(delta_pose, t_now=time.perf_counter()) """ def __init__(self, name: str, config: TeleopIKTaskConfig) -> None: @@ -159,15 +160,11 @@ def __init__(self, name: str, config: TeleopIKTaskConfig) -> None: self._target_pose: pinocchio.SE3 | None = None self._last_update_time: float = 0.0 self._active = False - self._engaged = False # Controlled by on_buttons toggle; gates set_target_pose - # Initial EE pose (captured when tracking starts, used for delta mode) + # Initial EE pose for delta application, cached last solution for IK warm-start self._initial_ee_pose: pinocchio.SE3 | None = None - - # Cache last successful IK solution for warm-starting self._last_q_solution: NDArray[np.floating[Any]] | None = None - - # Rising edge detection for primary button (engage/disengage toggle) + self._buttons: QuestButtons | None = None self._prev_primary: bool = False logger.info( @@ -206,7 +203,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: if not self._active or self._target_pose is None: return None - # Check timeout (safety stop if teleop crashes) + # 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: @@ -214,6 +211,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: f"TeleopIKTask {self._name} timed out " f"(no update for {time_since_update:.3f}s)" ) + self._target_pose = None self._active = False return None @@ -223,18 +221,13 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: if self._initial_ee_pose is None: q_current = self._get_current_joints(state) if q_current is None: - logger.debug(f"TeleopIKTask {self._name}: cannot capture initial pose") + logger.debug( + f"TeleopIKTask {self._name}: cannot capture initial pose, joint state unavailable" + ) return None - initial_pose = self._ik.forward_kinematics(q_current) with self._lock: self._initial_ee_pose = initial_pose - logger.info( - f"TeleopIKTask {self._name}: captured initial EE pose at " - f"[{initial_pose.translation[0]:.3f}, " - f"{initial_pose.translation[1]:.3f}, " - f"{initial_pose.translation[2]:.3f}]" - ) # Apply delta to initial pose: target = initial + delta with self._lock: @@ -251,25 +244,25 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | 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 - the safety clamp - # will handle any large jumps. This prevents the arm from "sticking" - # when near singularities or workspace boundaries. + # 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 # Cache solution for next warm-start with self._lock: self._last_q_solution = q_solution.copy() positions = q_solution.flatten().tolist() - # degrees = [f"{np.degrees(p):.1f}" for p in positions] - # logger.info(f"TeleopIKTask {self._name}: {dict(zip(self._joint_names_list, degrees))}") - # return None - # TODO: uncomment when joint values look good return JointCommandOutput( joint_names=self._joint_names_list, positions=positions, @@ -306,28 +299,15 @@ def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: # Task-specific methods # ========================================================================= - def engage(self) -> None: - """Prepare for new tracking session. - - Resets initial EE pose so next compute() recaptures from current position. - Called via on_buttons when primary button toggles to engaged. - """ - logger.info(f"TeleopIKTask {self._name}: engage") - with self._lock: - self._engaged = True - self._initial_ee_pose = None - - def disengage(self) -> None: - """Stop tracking. Called via on_buttons when primary button toggles to disengaged.""" - logger.info(f"TeleopIKTask {self._name}: disengage") - self.clear() - def on_buttons(self, msg: QuestButtons) -> None: - """Toggle engage/disengage on primary button rising edge. + """Press-and-hold engage: hold primary button to track, release to stop. Checks only the button matching self._config.hand (left_x or right_a). If hand is not set, listens to both. + Also stores button state for future gripper control. """ + self._buttons = msg + hand = self._config.hand if hand == "left": primary = msg.left_x @@ -337,27 +317,20 @@ def on_buttons(self, msg: QuestButtons) -> None: primary = msg.left_x or msg.right_a if primary and not self._prev_primary: - if self._engaged: - self.disengage() - else: - self.engage() + # Rising edge: reset initial pose so compute() recaptures + logger.info(f"TeleopIKTask {self._name}: engage") + with self._lock: + self._initial_ee_pose = None + elif not primary and self._prev_primary: + # Falling edge: stop tracking + logger.info(f"TeleopIKTask {self._name}: disengage") + with self._lock: + self._target_pose = None + self._initial_ee_pose = None self._prev_primary = primary - def set_target_pose(self, pose: Pose | PoseStamped, t_now: float) -> bool: - """Set target end-effector pose from delta. - - Treats incoming pose as a delta from the initial EE pose. The initial pose - is automatically captured on the first compute() call after activation. - - Call this from your teleop callback. - - Args: - pose: Delta pose from teleop (position offset + orientation delta) - t_now: Current time (from coordinator or time.perf_counter()) - - Returns: - True if accepted - """ + def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool: + """Handle incoming cartesian command (delta pose from teleop)""" delta_se3 = PinocchioIK.pose_to_se3(pose) with self._lock: @@ -367,35 +340,6 @@ def set_target_pose(self, pose: Pose | PoseStamped, t_now: float) -> bool: return True - def set_target_pose_dict( - self, - pose: dict[str, float], - t_now: float, - ) -> bool: - """Set target from pose dict with position and RPY orientation. - - Args: - pose: {x, y, z, roll, pitch, yaw} in meters/radians - t_now: Current time - - Returns: - True if accepted, False if missing required keys - """ - required_keys = {"x", "y", "z", "roll", "pitch", "yaw"} - if not required_keys.issubset(pose.keys()): - missing = required_keys - set(pose.keys()) - logger.warning(f"TeleopIKTask {self._name}: missing pose keys {missing}") - return False - - target_se3 = PinocchioIK.pose_dict_to_se3(pose) - - with self._lock: - self._target_pose = target_se3 - self._last_update_time = t_now - self._active = True - - return True - def start(self) -> None: """Activate the task (start accepting and outputting commands).""" with self._lock: @@ -408,27 +352,9 @@ def stop(self) -> None: self._active = False logger.info(f"TeleopIKTask {self._name} stopped") - def clear(self) -> None: - """Clear current target, initial pose, and deactivate.""" - with self._lock: - self._target_pose = None - self._initial_ee_pose = None - self._active = False - self._engaged = False - logger.info(f"TeleopIKTask {self._name} cleared") - def capture_initial_pose(self, state: CoordinatorState) -> bool: - """Capture current EE pose as the initial/base pose for delta mode. - - Call this when teleop engages (e.g., X button pressed) to set the - reference pose that deltas will be applied to. + """Capture current EE pose as the initial/base pose for delta mode""" - Args: - state: Current coordinator state (for forward kinematics) - - Returns: - True if captured successfully, False if joint state unavailable - """ q_current = self._get_current_joints(state) if q_current is None: logger.warning( @@ -437,10 +363,8 @@ def capture_initial_pose(self, state: CoordinatorState) -> bool: return False initial_pose = self._ik.forward_kinematics(q_current) - with self._lock: self._initial_ee_pose = initial_pose - logger.info( f"TeleopIKTask {self._name}: captured initial EE pose at " f"[{initial_pose.translation[0]:.3f}, {initial_pose.translation[1]:.3f}, " diff --git a/dimos/manipulation/planning/kinematics/pinocchio_ik.py b/dimos/manipulation/planning/kinematics/pinocchio_ik.py index 3db800801e..d4cc841e85 100644 --- a/dimos/manipulation/planning/kinematics/pinocchio_ik.py +++ b/dimos/manipulation/planning/kinematics/pinocchio_ik.py @@ -158,7 +158,7 @@ def model(self) -> pinocchio.Model: @property def nq(self) -> int: """Number of configuration variables (DOF).""" - return self._model.nq + return int(self._model.nq) @property def ee_joint_id(self) -> int: diff --git a/dimos/teleop/blueprints.py b/dimos/teleop/blueprints.py index 6f3ed7cada..605c471243 100644 --- a/dimos/teleop/blueprints.py +++ b/dimos/teleop/blueprints.py @@ -30,7 +30,7 @@ # Quest Teleop Blueprints # ----------------------------------------------------------------------------- -# Arm teleop with toggle-based engage +# Arm teleop with press-and-hold engage arm_teleop = autoconnect( arm_teleop_module(), ).transports( @@ -54,9 +54,10 @@ # ----------------------------------------------------------------------------- -# Teleop wired to Coordinator (right controller -> arm) +# Teleop wired to Coordinator (TeleopIK) # ----------------------------------------------------------------------------- -# Combined blueprint: teleop + xarm6 CartesianIK coordinator + +# Single XArm6 teleop: right controller -> xarm6 # Usage: dimos run arm-teleop-xarm6 arm_teleop_xarm6 = autoconnect( diff --git a/dimos/teleop/quest/quest_extensions.py b/dimos/teleop/quest/quest_extensions.py index a419e9904b..14a4b33c9f 100644 --- a/dimos/teleop/quest/quest_extensions.py +++ b/dimos/teleop/quest/quest_extensions.py @@ -16,9 +16,9 @@ """Quest teleop module extensions and subclasses. Available subclasses: - - ArmTeleopModule: Per-hand toggle engage (X/A press to toggle), task name routing + - ArmTeleopModule: Per-hand press-and-hold engage (X/A hold to track), task name routing - TwistTeleopModule: Outputs Twist instead of PoseStamped - - VisualizingTeleopModule: Adds Rerun visualization (uses toggle engage) + - VisualizingTeleopModule: Adds Rerun visualization (inherits press-and-hold engage) """ from __future__ import annotations @@ -93,10 +93,10 @@ class ArmTeleopConfig(QuestTeleopConfig): class ArmTeleopModule(QuestTeleopModule): - """Quest teleop with per-hand toggle engage and task name routing. + """Quest teleop with per-hand press-and-hold engage and task name routing. Each controller's primary button (X for left, A for right) - toggles that hand's engage state independently. + engages that hand while held, disengages on release. When task_names is configured, output PoseStamped messages have their frame_id set to the task name, enabling the coordinator to route @@ -112,8 +112,9 @@ class ArmTeleopModule(QuestTeleopModule): def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) - self._prev_primary: dict[Hand, bool] = {Hand.LEFT: False, Hand.RIGHT: False} cfg: ArmTeleopConfig = self.config # type: ignore[assignment] + + self._prev_primary: dict[Hand, bool] = {Hand.LEFT: False, Hand.RIGHT: False} self._task_names: dict[Hand, str] = {Hand[k.upper()]: v for k, v in cfg.task_names.items()} def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: @@ -129,7 +130,7 @@ def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: super()._publish_msg(hand, output_msg) def _handle_engage(self) -> None: - """Toggle per-hand engage on primary button rising edge.""" + """Press-and-hold per-hand engage: hold primary to track, release to stop.""" for hand in Hand: controller = self._controllers.get(hand) if controller is None: @@ -137,10 +138,9 @@ def _handle_engage(self) -> None: pressed = controller.primary if pressed and not self._prev_primary[hand]: - if self._is_engaged[hand]: - self._disengage(hand) - else: - self._engage(hand) + self._engage(hand) + elif not pressed and self._prev_primary[hand]: + self._disengage(hand) self._prev_primary[hand] = pressed From 1c08c7639918ed3c5a787e5d513bd4dd62dda31d Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 12 Feb 2026 12:36:24 -0800 Subject: [PATCH 13/27] Fix: fix redundant code --- dimos/control/blueprints.py | 34 +++++++------------ dimos/control/tasks/cartesian_ik_task.py | 20 +---------- dimos/control/tasks/teleop_task.py | 24 ++----------- .../planning/kinematics/pinocchio_ik.py | 26 ++------------ dimos/teleop/quest/quest_extensions.py | 15 -------- 5 files changed, 19 insertions(+), 100 deletions(-) diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py index 2c4bd5c647..b8d50b88a6 100644 --- a/dimos/control/blueprints.py +++ b/dimos/control/blueprints.py @@ -36,6 +36,12 @@ from dimos.msgs.geometry_msgs import PoseStamped from dimos.msgs.sensor_msgs import JointState from dimos.teleop.quest.quest_types import QuestButtons +from dimos.utils.data import get_data + +_PIPER_MODEL_PATH = str( + get_data("piper_description") / "mujoco_model" / "piper_no_gripper_description.xml" +) +_XARM6_MODEL_PATH = str(get_data("xarm_description") / "urdf" / "xarm6" / "xarm6.urdf") # ============================================================================= # Single Arm Blueprints @@ -392,22 +398,6 @@ # ============================================================================= -def _get_piper_model_path() -> str: - """Get path to Piper MJCF model for IK solver.""" - from dimos.utils.data import get_data - - piper_path = get_data("piper_description") - return str(piper_path / "mujoco_model" / "piper_no_gripper_description.xml") - - -def _get_xarm6_model_path() -> str: - """Get path to xarm6 URDF model for IK solver.""" - from dimos.utils.data import get_data - - xarm_path = get_data("xarm_description") - return str(xarm_path / "urdf" / "xarm6" / "xarm6.urdf") - - # Mock 6-DOF arm with CartesianIK coordinator_cartesian_ik_mock = control_coordinator( tick_rate=100.0, @@ -427,7 +417,7 @@ def _get_xarm6_model_path() -> str: type="cartesian_ik", joint_names=[f"arm_joint{i + 1}" for i in range(6)], priority=10, - model_path=_get_piper_model_path(), + model_path=_PIPER_MODEL_PATH, ee_joint_id=6, ), ], @@ -461,7 +451,7 @@ def _get_xarm6_model_path() -> str: type="cartesian_ik", joint_names=[f"arm_joint{i + 1}" for i in range(6)], priority=10, - model_path=_get_piper_model_path(), + model_path=_PIPER_MODEL_PATH, ee_joint_id=6, ), ], @@ -500,7 +490,7 @@ def _get_xarm6_model_path() -> str: type="teleop_ik", joint_names=[f"arm_joint{i + 1}" for i in range(6)], priority=10, - model_path=_get_xarm6_model_path(), + model_path=_XARM6_MODEL_PATH, ee_joint_id=6, hand="right", ), @@ -536,7 +526,7 @@ def _get_xarm6_model_path() -> str: type="teleop_ik", joint_names=[f"arm_joint{i + 1}" for i in range(6)], priority=10, - model_path=_get_piper_model_path(), + model_path=_PIPER_MODEL_PATH, ee_joint_id=6, hand="left", ), @@ -580,7 +570,7 @@ def _get_xarm6_model_path() -> str: type="teleop_ik", joint_names=[f"xarm_arm_joint{i + 1}" for i in range(6)], priority=10, - model_path=_get_xarm6_model_path(), + model_path=_XARM6_MODEL_PATH, ee_joint_id=6, hand="left", ), @@ -589,7 +579,7 @@ def _get_xarm6_model_path() -> str: type="teleop_ik", joint_names=[f"piper_arm_joint{i + 1}" for i in range(6)], priority=10, - model_path=_get_piper_model_path(), + model_path=_PIPER_MODEL_PATH, ee_joint_id=6, hand="right", ), diff --git a/dimos/control/tasks/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task.py index e342285f4a..63c28bcaa8 100644 --- a/dimos/control/tasks/cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task.py @@ -38,7 +38,6 @@ ) from dimos.manipulation.planning.kinematics.pinocchio_ik import ( PinocchioIK, - PinocchioIKConfig, check_joint_delta, get_worst_joint_delta, ) @@ -65,12 +64,7 @@ class CartesianIKTaskConfig: 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) - ik_max_iter: Maximum IK solver iterations - ik_eps: IK convergence threshold (error norm in meters) - ik_damp: IK damping factor for singularity handling (higher = more stable) - ik_dt: IK integration step size max_joint_delta_deg: Maximum allowed joint change per tick (safety limit) - max_velocity: Max joint velocity per IK iteration (rad/s) """ joint_names: list[str] @@ -78,12 +72,7 @@ class CartesianIKTaskConfig: ee_joint_id: int priority: int = 10 timeout: float = 0.5 - ik_max_iter: int = 100 - ik_eps: float = 1e-4 - ik_damp: float = 1e-2 - ik_dt: float = 1.0 max_joint_delta_deg: float = 15.0 # ~1500°/s at 100Hz - max_velocity: float = 2.0 class CartesianIKTask(ControlTask): @@ -135,14 +124,7 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: self._num_joints = len(config.joint_names) # Create IK solver from model - ik_config = PinocchioIKConfig( - max_iter=config.ik_max_iter, - eps=config.ik_eps, - damp=config.ik_damp, - dt=config.ik_dt, - max_velocity=config.max_velocity, - ) - self._ik = PinocchioIK.from_model_path(config.model_path, config.ee_joint_id, ik_config) + 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: diff --git a/dimos/control/tasks/teleop_task.py b/dimos/control/tasks/teleop_task.py index 705479d679..db62a80fe0 100644 --- a/dimos/control/tasks/teleop_task.py +++ b/dimos/control/tasks/teleop_task.py @@ -41,7 +41,6 @@ ) from dimos.manipulation.planning.kinematics.pinocchio_ik import ( PinocchioIK, - PinocchioIKConfig, check_joint_delta, ) from dimos.utils.logging_config import setup_logger @@ -67,12 +66,8 @@ class TeleopIKTaskConfig: 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) - ik_max_iter: Maximum IK solver iterations - ik_eps: IK convergence threshold (error norm in meters) - ik_damp: IK damping factor for singularity handling (higher = more stable) - ik_dt: IK integration step size max_joint_delta_deg: Maximum allowed joint change per tick (safety limit) - max_velocity: Max joint velocity per IK iteration (rad/s) + hand: "left" or "right" — which controller's primary button to listen to """ joint_names: list[str] @@ -80,14 +75,8 @@ class TeleopIKTaskConfig: ee_joint_id: int priority: int = 10 timeout: float = 0.5 - ik_max_iter: int = 100 - ik_eps: float = 1e-4 - ik_damp: float = 1e-2 - ik_dt: float = 1.0 - max_joint_delta_deg: float = 5.0 # ~500°/s at 100Hz - max_velocity: float = 5.0 - hand: str = "" # "left" or "right" — which controller's primary button to listen to + hand: str = "" class TeleopIKTask(ControlTask): @@ -139,14 +128,7 @@ def __init__(self, name: str, config: TeleopIKTaskConfig) -> None: self._num_joints = len(config.joint_names) # Create IK solver from model - ik_config = PinocchioIKConfig( - max_iter=config.ik_max_iter, - eps=config.ik_eps, - damp=config.ik_damp, - dt=config.ik_dt, - max_velocity=config.max_velocity, - ) - self._ik = PinocchioIK.from_model_path(config.model_path, config.ee_joint_id, ik_config) + 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: diff --git a/dimos/manipulation/planning/kinematics/pinocchio_ik.py b/dimos/manipulation/planning/kinematics/pinocchio_ik.py index d4cc841e85..4d90132676 100644 --- a/dimos/manipulation/planning/kinematics/pinocchio_ik.py +++ b/dimos/manipulation/planning/kinematics/pinocchio_ik.py @@ -123,14 +123,12 @@ def from_model_path( cls, model_path: str | Path, ee_joint_id: int, - config: PinocchioIKConfig | None = None, ) -> PinocchioIK: """Create solver by loading a URDF or MJCF file. Args: model_path: Path to URDF (.urdf) or MJCF (.xml) file ee_joint_id: End-effector joint ID in the kinematic chain - config: Solver configuration (uses defaults if None) Returns: Configured PinocchioIK instance @@ -148,7 +146,7 @@ def from_model_path( model = pinocchio.buildModelFromUrdf(str(path)) data = model.createData() - return cls(model, data, ee_joint_id, config) + return cls(model, data, ee_joint_id) @property def model(self) -> pinocchio.Model: @@ -233,17 +231,8 @@ def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pino @staticmethod def pose_to_se3(pose: Pose | PoseStamped) -> pinocchio.SE3: - """Convert a DimOS Pose or PoseStamped to pinocchio SE3. + """Convert Pose or PoseStamped to pinocchio SE3""" - Uses quaternion directly to avoid Euler angle conversion issues. - - Args: - pose: Pose or PoseStamped message with position (x, y, z) - and orientation (quaternion) - - Returns: - pinocchio.SE3 representation - """ position = np.array([pose.x, pose.y, pose.z]) quat = pose.orientation rotation = pinocchio.Quaternion(quat.w, quat.x, quat.y, quat.z).toRotationMatrix() @@ -251,17 +240,8 @@ def pose_to_se3(pose: Pose | PoseStamped) -> pinocchio.SE3: @staticmethod def pose_dict_to_se3(pose: dict[str, float]) -> pinocchio.SE3: - """Convert a pose dict with RPY to pinocchio SE3. + """Convert a pose dict with RPY to pinocchio SE3""" - Args: - pose: Dict with keys {x, y, z, roll, pitch, yaw} in meters/radians - - Returns: - pinocchio.SE3 representation - - Raises: - KeyError: If required keys are missing - """ position = np.array([pose["x"], pose["y"], pose["z"]]) rotation = pinocchio.rpy.rpyToMatrix(pose["roll"], pose["pitch"], pose["yaw"]) return pinocchio.SE3(rotation, position) diff --git a/dimos/teleop/quest/quest_extensions.py b/dimos/teleop/quest/quest_extensions.py index 14a4b33c9f..94875e248d 100644 --- a/dimos/teleop/quest/quest_extensions.py +++ b/dimos/teleop/quest/quest_extensions.py @@ -114,7 +114,6 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) cfg: ArmTeleopConfig = self.config # type: ignore[assignment] - self._prev_primary: dict[Hand, bool] = {Hand.LEFT: False, Hand.RIGHT: False} self._task_names: dict[Hand, str] = {Hand[k.upper()]: v for k, v in cfg.task_names.items()} def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: @@ -129,20 +128,6 @@ def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: ) super()._publish_msg(hand, output_msg) - def _handle_engage(self) -> None: - """Press-and-hold per-hand engage: hold primary to track, release to stop.""" - for hand in Hand: - controller = self._controllers.get(hand) - if controller is None: - continue - - pressed = controller.primary - if pressed and not self._prev_primary[hand]: - self._engage(hand) - elif not pressed and self._prev_primary[hand]: - self._disengage(hand) - self._prev_primary[hand] = pressed - class VisualizingTeleopModule(ArmTeleopModule): """Quest teleop with Rerun visualization. From a84177d792891842e14668efd5ef0c70e41703a2 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 12 Feb 2026 13:42:24 -0800 Subject: [PATCH 14/27] Fix: _hasattr check added --- dimos/control/coordinator.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index bad14eeaea..19aee139d3 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -487,7 +487,10 @@ def _on_cartesian_command(self, msg: PoseStamped) -> None: logger.warning(f"Cartesian command for unknown task: {task_name}") return - task.on_cartesian_command(msg, t_now) # type: ignore[attr-defined] + if hasattr(task, "on_cartesian_command"): + task.on_cartesian_command(msg, t_now) # type: ignore[attr-defined] + else: + logger.warning(f"Task {task_name} does not support on_cartesian_command") def _on_buttons(self, msg: QuestButtons) -> None: """Forward button state to all tasks that accept it.""" From 1216332215c2b71990ce365b61c024a04db52438 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 12 Feb 2026 14:54:37 -0800 Subject: [PATCH 15/27] Fix: improper lock errors + remove fallback --- dimos/control/tasks/teleop_task.py | 45 +++++------------------------- 1 file changed, 7 insertions(+), 38 deletions(-) diff --git a/dimos/control/tasks/teleop_task.py b/dimos/control/tasks/teleop_task.py index db62a80fe0..83f4fdb428 100644 --- a/dimos/control/tasks/teleop_task.py +++ b/dimos/control/tasks/teleop_task.py @@ -84,7 +84,7 @@ class TeleopIKTask(ControlTask): 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 or explicit capture_initial_pose call). + 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. @@ -143,10 +143,8 @@ def __init__(self, name: str, config: TeleopIKTaskConfig) -> None: self._last_update_time: float = 0.0 self._active = False - # Initial EE pose for delta application, cached last solution for IK warm-start + # Initial EE pose for delta application self._initial_ee_pose: pinocchio.SE3 | None = None - self._last_q_solution: NDArray[np.floating[Any]] | None = None - self._buttons: QuestButtons | None = None self._prev_primary: bool = False logger.info( @@ -200,7 +198,10 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: delta_pose = self._target_pose # Capture initial EE pose if not set (first command after engage) - if self._initial_ee_pose is None: + 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( @@ -240,10 +241,6 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: ) return None - # Cache solution for next warm-start - with self._lock: - self._last_q_solution = q_solution.copy() - positions = q_solution.flatten().tolist() return JointCommandOutput( joint_names=self._joint_names_list, @@ -252,17 +249,11 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: ) def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.floating[Any]] | None: - """Get current joint positions from coordinator state. - - Falls back to last IK solution if joint state unavailable. - """ + """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: - # Fallback to last solution - if self._last_q_solution is not None: - return self._last_q_solution.copy() return None positions.append(pos) return np.array(positions) @@ -288,8 +279,6 @@ def on_buttons(self, msg: QuestButtons) -> None: If hand is not set, listens to both. Also stores button state for future gripper control. """ - self._buttons = msg - hand = self._config.hand if hand == "left": primary = msg.left_x @@ -334,26 +323,6 @@ def stop(self) -> None: self._active = False logger.info(f"TeleopIKTask {self._name} stopped") - def capture_initial_pose(self, state: CoordinatorState) -> bool: - """Capture current EE pose as the initial/base pose for delta mode""" - - q_current = self._get_current_joints(state) - if q_current is None: - logger.warning( - f"TeleopIKTask {self._name}: cannot capture initial pose, joint state unavailable" - ) - return False - - initial_pose = self._ik.forward_kinematics(q_current) - with self._lock: - self._initial_ee_pose = initial_pose - logger.info( - f"TeleopIKTask {self._name}: captured initial EE pose at " - f"[{initial_pose.translation[0]:.3f}, {initial_pose.translation[1]:.3f}, " - f"{initial_pose.translation[2]:.3f}]" - ) - return True - __all__ = [ "TeleopIKTask", From e874e4d41d2817b9b19a346352b735928df892a1 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Thu, 12 Feb 2026 15:07:54 -0800 Subject: [PATCH 16/27] Fix: mypy issues - initial_pos check --- dimos/control/tasks/teleop_task.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dimos/control/tasks/teleop_task.py b/dimos/control/tasks/teleop_task.py index 83f4fdb428..d9db424142 100644 --- a/dimos/control/tasks/teleop_task.py +++ b/dimos/control/tasks/teleop_task.py @@ -214,6 +214,8 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: # 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_pose.rotation @ self._initial_ee_pose.rotation, self._initial_ee_pose.translation + delta_pose.translation, @@ -277,7 +279,6 @@ def on_buttons(self, msg: QuestButtons) -> None: Checks only the button matching self._config.hand (left_x or right_a). If hand is not set, listens to both. - Also stores button state for future gripper control. """ hand = self._config.hand if hand == "left": From 1c43535b0253fa4eecd9a37455b0b4060d602cc8 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 13 Feb 2026 01:05:39 -0800 Subject: [PATCH 17/27] add LfsPath helper --- dimos/control/blueprints.py | 8 +- dimos/control/coordinator.py | 3 +- dimos/utils/data.py | 100 +++++++++++++++++ dimos/utils/test_data.py | 212 +++++++++++++++++++++++++++++++++++ 4 files changed, 317 insertions(+), 6 deletions(-) diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py index b8d50b88a6..ceb3a74fbe 100644 --- a/dimos/control/blueprints.py +++ b/dimos/control/blueprints.py @@ -36,12 +36,10 @@ from dimos.msgs.geometry_msgs import PoseStamped from dimos.msgs.sensor_msgs import JointState from dimos.teleop.quest.quest_types import QuestButtons -from dimos.utils.data import get_data +from dimos.utils.data import LfsPath -_PIPER_MODEL_PATH = str( - get_data("piper_description") / "mujoco_model" / "piper_no_gripper_description.xml" -) -_XARM6_MODEL_PATH = str(get_data("xarm_description") / "urdf" / "xarm6" / "xarm6.urdf") +_PIPER_MODEL_PATH = LfsPath("piper_description/mujoco_model/piper_no_gripper_description.xml") +_XARM6_MODEL_PATH = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf") # ============================================================================= # Single Arm Blueprints diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 19aee139d3..ec2c67c957 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -49,6 +49,7 @@ if TYPE_CHECKING: from collections.abc import Callable + from pathlib import Path from dimos.hardware.manipulators.spec import ManipulatorAdapter @@ -78,7 +79,7 @@ class TaskConfig: joint_names: list[str] = field(default_factory=lambda: []) priority: int = 10 # Cartesian IK / Teleop IK specific - model_path: str | None = None + model_path: str | Path | None = None ee_joint_id: int = 6 hand: str = "" # teleop_ik only: "left" or "right" controller diff --git a/dimos/utils/data.py b/dimos/utils/data.py index 7666343dbb..aba2e50e5a 100644 --- a/dimos/utils/data.py +++ b/dimos/utils/data.py @@ -248,3 +248,103 @@ def get_data(filename: str | Path) -> Path: return file_path return _decompress_archive(_pull_lfs_archive(filename)) + + +class LfsPath(type(Path())): # type: ignore[misc] + """ + A Path subclass that lazily downloads LFS data when accessed. + + This is useful for both lazy loading and differentiating between LFS paths and regular paths. + + This class wraps pathlib.Path and ensures that get_data() is called + before any meaningful filesystem operation, making LFS data lazy-loaded. + + Usage: + path = LfsPath("sample_data") + # No download yet + + with path.open('rb') as f: # Downloads now if needed + data = f.read() + + # Or use any Path operation: + if path.exists(): # Downloads now if needed + files = list(path.iterdir()) + """ + + # Attributes that should NOT trigger download + _LFS_SAFE_ATTRIBUTES = { + # LfsPath internal attributes + "_lfs_filename", + "_lfs_resolved_cache", + "_ensure_downloaded", + # Python magic methods + "__class__", + "__dict__", + "__init__", + "__new__", + "__getattribute__", + "__setattr__", + "__delattr__", + # Path internal attributes (needed for Path operations) + "_drv", + "_flavour", + "_format_parsed_parts", + "_from_parsed_parts", + "_hash", + "_load_parts", + "_make_child_relpath", + "_parse_path", + "_parts_normcase", + "_parts_normcase_cached", + "_raw_paths", + "_root", + "_scandir", + "_str", + "_str_normcase", + "_str_normcase_cached", + "_tail", + "_tail_cached", + } + + def __new__(cls, filename: str | Path) -> "LfsPath": + # Create instance with a placeholder path to satisfy Path.__new__ + # We use "." as a dummy path that always exists + instance: LfsPath = super().__new__(cls, ".") # type: ignore[call-arg] + # Store the actual filename as an instance attribute + object.__setattr__(instance, "_lfs_filename", filename) + object.__setattr__(instance, "_lfs_resolved_cache", None) + return instance + + def _ensure_downloaded(self) -> Path: + """Ensure the LFS data is downloaded and return the resolved path.""" + cache: Path | None = object.__getattribute__(self, "_lfs_resolved_cache") + if cache is None: + filename = object.__getattribute__(self, "_lfs_filename") + cache = get_data(filename) + object.__setattr__(self, "_lfs_resolved_cache", cache) + return cache + + def __getattribute__(self, name: str) -> object: + # Allow access to safe attributes without triggering download + if name in LfsPath._LFS_SAFE_ATTRIBUTES: + return object.__getattribute__(self, name) + + # For all other attributes, ensure download first then delegate to resolved path + resolved = object.__getattribute__(self, "_ensure_downloaded")() + return getattr(resolved, name) + + def __str__(self) -> str: + """String representation returns resolved path.""" + return str(self._ensure_downloaded()) + + def __fspath__(self) -> str: + """Return filesystem path, downloading from LFS if needed.""" + return str(self._ensure_downloaded()) + + def __truediv__(self, other: object) -> Path: + """Path division operator - returns resolved path.""" + return self._ensure_downloaded() / other # type: ignore[operator, return-value] + + def __rtruediv__(self, other: object) -> Path: + """Reverse path division operator.""" + return other / self._ensure_downloaded() # type: ignore[operator, return-value] diff --git a/dimos/utils/test_data.py b/dimos/utils/test_data.py index 01f145f60c..d60e7cc355 100644 --- a/dimos/utils/test_data.py +++ b/dimos/utils/test_data.py @@ -14,11 +14,13 @@ import hashlib import os +from pathlib import Path import subprocess import pytest from dimos.utils import data +from dimos.utils.data import LfsPath @pytest.mark.heavy @@ -128,3 +130,213 @@ def test_pull_dir() -> None: with file.open("rb") as f: sha256 = hashlib.sha256(f.read()).hexdigest() assert sha256 == expected_hash + + +# ============================================================================ +# LfsPath Tests +# ============================================================================ + + +def test_lfs_path_lazy_creation() -> None: + """Test that creating LfsPath doesn't trigger download.""" + lfs_path = LfsPath("test_data_file") + + # Check that the object is created + assert isinstance(lfs_path, LfsPath) + + # Check that cache is None (not downloaded yet) + cache = object.__getattribute__(lfs_path, "_lfs_resolved_cache") + assert cache is None + + # Check that filename is stored + filename = object.__getattribute__(lfs_path, "_lfs_filename") + assert filename == "test_data_file" + + +def test_lfs_path_safe_attributes() -> None: + """Test that safe attributes don't trigger download.""" + lfs_path = LfsPath("test_data_file") + + # Access safe attributes directly + filename = object.__getattribute__(lfs_path, "_lfs_filename") + cache = object.__getattribute__(lfs_path, "_lfs_resolved_cache") + ensure_fn = object.__getattribute__(lfs_path, "_ensure_downloaded") + + # Verify they exist and cache is still None + assert filename == "test_data_file" + assert cache is None + assert callable(ensure_fn) + + +def test_lfs_path_conversion_to_path() -> None: + """Test that LfsPath can be converted to regular Path.""" + # Use a file that doesn't exist to test conversion without download + lfs_path = LfsPath("nonexistent_file") + + # Path conversion accesses internal attributes but shouldn't trigger download + # because Path internal attributes are in the safe list + converted = Path(lfs_path) + assert isinstance(converted, Path) + assert type(converted).__name__ in ("PosixPath", "WindowsPath") + + +@pytest.mark.heavy +def test_lfs_path_with_real_file() -> None: + """Test LfsPath with a real small LFS file.""" + # Use a small existing LFS file + filename = "three_paths.png" + lfs_path = LfsPath(filename) + + # Initially, cache should be None + cache = object.__getattribute__(lfs_path, "_lfs_resolved_cache") + assert cache is None + + # Access a Path method - this should trigger download + exists = lfs_path.exists() + + # Now cache should be populated + cache = object.__getattribute__(lfs_path, "_lfs_resolved_cache") + assert cache is not None + assert isinstance(cache, Path) + + # File should exist after download + assert exists is True + + # Should be able to get file stats + stat_result = lfs_path.stat() + assert stat_result.st_size > 0 + + # Should be able to read the file + content = lfs_path.read_bytes() + assert len(content) > 0 + + # Verify it's a PNG file + assert content.startswith(b"\x89PNG") + + +@pytest.mark.heavy +def test_lfs_path_unload_and_reload() -> None: + """Test unloading and reloading an LFS file.""" + filename = "three_paths.png" + data_dir = data._get_data_dir() + file_path = data_dir / filename + + # Clean up if file already exists + if file_path.exists(): + file_path.unlink() + + # Create LfsPath + lfs_path = LfsPath(filename) + + # Verify file doesn't exist yet + assert not file_path.exists() + + # Access the file - this triggers download + content_first = lfs_path.read_bytes() + assert file_path.exists() + + # Get hash of first download + hash_first = hashlib.sha256(content_first).hexdigest() + + # Now unload (delete the file) + file_path.unlink() + assert not file_path.exists() + + # Create a new LfsPath instance for the same file + lfs_path_2 = LfsPath(filename) + + # Access the file again - should re-download + content_second = lfs_path_2.read_bytes() + assert file_path.exists() + + # Get hash of second download + hash_second = hashlib.sha256(content_second).hexdigest() + + # Hashes should match (same file downloaded) + assert hash_first == hash_second + + # Content should be identical + assert content_first == content_second + + +@pytest.mark.heavy +def test_lfs_path_operations() -> None: + """Test various Path operations with LfsPath.""" + filename = "three_paths.png" + lfs_path = LfsPath(filename) + + # Test is_file + assert lfs_path.is_file() is True + assert lfs_path.is_dir() is False + + # Test absolute path + abs_path = lfs_path.absolute() + assert abs_path.is_absolute() + + # Test resolve + resolved = lfs_path.resolve() + assert resolved.is_absolute() + + # Test string conversion + path_str = str(lfs_path) + assert isinstance(path_str, str) + assert filename in path_str + + # Test __fspath__ + fspath_result = os.fspath(lfs_path) + assert isinstance(fspath_result, str) + assert filename in fspath_result + + +@pytest.mark.heavy +def test_lfs_path_division_operator() -> None: + """Test path division operator with LfsPath.""" + # Use a directory for testing + lfs_path = LfsPath("three_paths.png") + + # Test truediv - this should trigger download and return resolved path + result = lfs_path / "subpath" + assert isinstance(result, Path) + + # The result should be the resolved path with subpath appended + assert "three_paths.png" in str(result) + + +@pytest.mark.heavy +def test_lfs_path_multiple_instances() -> None: + """Test that multiple LfsPath instances for same file work correctly.""" + filename = "three_paths.png" + + # Create two separate instances + lfs_path_1 = LfsPath(filename) + lfs_path_2 = LfsPath(filename) + + # Both should start with None cache + cache_1 = object.__getattribute__(lfs_path_1, "_lfs_resolved_cache") + cache_2 = object.__getattribute__(lfs_path_2, "_lfs_resolved_cache") + assert cache_1 is None + assert cache_2 is None + + # Access file through first instance + content_1 = lfs_path_1.read_bytes() + + # First instance should have cache + cache_1 = object.__getattribute__(lfs_path_1, "_lfs_resolved_cache") + assert cache_1 is not None + + # Second instance cache should still be None (separate instance) + cache_2 = object.__getattribute__(lfs_path_2, "_lfs_resolved_cache") + assert cache_2 is None + + # Access through second instance + content_2 = lfs_path_2.read_bytes() + + # Now second instance should also have cache + cache_2 = object.__getattribute__(lfs_path_2, "_lfs_resolved_cache") + assert cache_2 is not None + + # Content should be the same + assert content_1 == content_2 + + # Both caches should point to the same file + assert cache_1 == cache_2 From 69e225c8367f89e4519596a185f661da1328f0e6 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Fri, 13 Feb 2026 11:15:42 -0800 Subject: [PATCH 18/27] Fix: removing ignore + config fix --- dimos/teleop/quest/quest_extensions.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/dimos/teleop/quest/quest_extensions.py b/dimos/teleop/quest/quest_extensions.py index 94875e248d..76d040191c 100644 --- a/dimos/teleop/quest/quest_extensions.py +++ b/dimos/teleop/quest/quest_extensions.py @@ -60,18 +60,18 @@ class TwistTeleopModule(QuestTeleopModule): """ default_config = TwistTeleopConfig + config: TwistTeleopConfig left_twist: Out[TwistStamped] right_twist: Out[TwistStamped] def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: """Convert PoseStamped to TwistStamped, apply scaling, and publish.""" - cfg: TwistTeleopConfig = self.config # type: ignore[assignment] twist = TwistStamped( ts=output_msg.ts, frame_id=output_msg.frame_id, - linear=output_msg.position * cfg.linear_scale, - angular=output_msg.orientation.to_euler() * cfg.angular_scale, + linear=output_msg.position * self.config.linear_scale, + angular=output_msg.orientation.to_euler() * self.config.angular_scale, ) if hand == Hand.LEFT: self.left_twist.publish(twist) @@ -109,12 +109,12 @@ class ArmTeleopModule(QuestTeleopModule): """ default_config = ArmTeleopConfig + config: ArmTeleopConfig def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) - cfg: ArmTeleopConfig = self.config # type: ignore[assignment] - self._task_names: dict[Hand, str] = {Hand[k.upper()]: v for k, v in cfg.task_names.items()} + self._task_names: dict[Hand, str] = {Hand[k.upper()]: v for k, v in self.config.task_names.items()} def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: """Stamp frame_id with task name and publish.""" From b4e1a2790b7bcf2981af05c10975a2c7808351cc Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Fri, 13 Feb 2026 11:17:02 -0800 Subject: [PATCH 19/27] Fix: pose_to_se3 standalone method + remove redundant dict_pose conversion --- .../planning/kinematics/pinocchio_ik.py | 30 ++++++++----------- 1 file changed, 12 insertions(+), 18 deletions(-) diff --git a/dimos/manipulation/planning/kinematics/pinocchio_ik.py b/dimos/manipulation/planning/kinematics/pinocchio_ik.py index 4d90132676..4841e76cc6 100644 --- a/dimos/manipulation/planning/kinematics/pinocchio_ik.py +++ b/dimos/manipulation/planning/kinematics/pinocchio_ik.py @@ -84,7 +84,6 @@ class PinocchioIK: Loads a URDF or MJCF model and provides: - solve(): Damped least-squares IK from SE3 target - forward_kinematics(): FK from joint angles to EE pose - - pose_to_se3(): Convert DimOS Pose/PoseStamped messages to pinocchio.SE3 Thread safety: NOT thread-safe. Each caller should use its own instance or protect calls with an external lock. Control tasks typically hold a @@ -92,7 +91,7 @@ class PinocchioIK: Example: >>> ik = PinocchioIK.from_model_path("robot.urdf", ee_joint_id=6) - >>> target = ik.pose_to_se3(pose_stamped) + >>> target = pose_to_se3(pose_stamped) >>> q, converged, err = ik.solve(target, q_current) >>> if converged: ... ee = ik.forward_kinematics(q) @@ -225,26 +224,20 @@ def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pino pinocchio.forwardKinematics(self._model, self._data, joint_positions) return self._data.oMi[self._ee_joint_id].copy() - # ========================================================================= - # Pose Conversion Helpers - # ========================================================================= - @staticmethod - def pose_to_se3(pose: Pose | PoseStamped) -> pinocchio.SE3: - """Convert Pose or PoseStamped to pinocchio SE3""" - position = np.array([pose.x, pose.y, pose.z]) - quat = pose.orientation - rotation = pinocchio.Quaternion(quat.w, quat.x, quat.y, quat.z).toRotationMatrix() - return pinocchio.SE3(rotation, position) +# ============================================================================= +# Pose Conversion Helpers +# ============================================================================= + - @staticmethod - def pose_dict_to_se3(pose: dict[str, float]) -> pinocchio.SE3: - """Convert a pose dict with RPY to pinocchio SE3""" +def pose_to_se3(pose: Pose | PoseStamped) -> pinocchio.SE3: + """Convert Pose or PoseStamped to pinocchio SE3""" - position = np.array([pose["x"], pose["y"], pose["z"]]) - rotation = pinocchio.rpy.rpyToMatrix(pose["roll"], pose["pitch"], pose["yaw"]) - return pinocchio.SE3(rotation, position) + position = np.array([pose.x, pose.y, pose.z]) + quat = pose.orientation + rotation = pinocchio.Quaternion(quat.w, quat.x, quat.y, quat.z).toRotationMatrix() + return pinocchio.SE3(rotation, position) # ============================================================================= @@ -295,4 +288,5 @@ def get_worst_joint_delta( "PinocchioIKConfig", "check_joint_delta", "get_worst_joint_delta", + "pose_to_se3", ] From 0df2ea0b0902ef57065d414eaf5455266f1cc7c9 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Fri, 13 Feb 2026 11:36:47 -0800 Subject: [PATCH 20/27] method reorder + add warning --- dimos/control/coordinator.py | 2 +- dimos/control/tasks/cartesian_ik_task.py | 41 ++++-------------------- dimos/control/tasks/teleop_task.py | 16 +++++---- 3 files changed, 17 insertions(+), 42 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index ec2c67c957..e4201adbe8 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -627,7 +627,7 @@ def start(self) -> None: self._buttons_unsub = self.buttons.subscribe(self._on_buttons) logger.info("Subscribed to buttons for engage/disengage") except Exception as e: - logger.debug(f"Could not subscribe to buttons: {e}") + logger.warning(f"Could not subscribe to buttons: {e}") logger.info(f"ControlCoordinator started at {self.config.tick_rate}Hz") diff --git a/dimos/control/tasks/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task.py index 63c28bcaa8..e6d5273a30 100644 --- a/dimos/control/tasks/cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task.py @@ -40,6 +40,7 @@ PinocchioIK, check_joint_delta, get_worst_joint_delta, + pose_to_se3, ) from dimos.utils.logging_config import setup_logger @@ -135,7 +136,7 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None: # Thread-safe target state self._lock = threading.Lock() - self._target_pose: pinocchio.SE3 | None = None + self._target_pose: Pose | PoseStamped | None = None self._last_update_time: float = 0.0 self._active = False @@ -189,7 +190,10 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: self._active = False return None - target_pose = self._target_pose + raw_pose = self._target_pose + + # Convert to SE3 right before use + target_pose = pose_to_se3(raw_pose) # Get current joint positions for IK warm-start q_current = self._get_current_joints(state) @@ -272,39 +276,8 @@ def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool: Returns: True if accepted """ - target_se3 = PinocchioIK.pose_to_se3(pose) - - with self._lock: - self._target_pose = target_se3 - self._last_update_time = t_now - self._active = True - - return True - - def on_cartesian_command_dict( - self, - pose: dict[str, float], - t_now: float, - ) -> bool: - """Set target from pose dict with position and RPY orientation. - - Args: - pose: {x, y, z, roll, pitch, yaw} in meters/radians - t_now: Current time - - Returns: - True if accepted, False if missing required keys - """ - required_keys = {"x", "y", "z", "roll", "pitch", "yaw"} - if not required_keys.issubset(pose.keys()): - missing = required_keys - set(pose.keys()) - logger.warning(f"CartesianIKTask {self._name}: missing pose keys {missing}") - return False - - target_se3 = PinocchioIK.pose_dict_to_se3(pose) - with self._lock: - self._target_pose = target_se3 + self._target_pose = pose # Store raw, convert to SE3 in compute() self._last_update_time = t_now self._active = True diff --git a/dimos/control/tasks/teleop_task.py b/dimos/control/tasks/teleop_task.py index d9db424142..ef7787f240 100644 --- a/dimos/control/tasks/teleop_task.py +++ b/dimos/control/tasks/teleop_task.py @@ -42,6 +42,7 @@ from dimos.manipulation.planning.kinematics.pinocchio_ik import ( PinocchioIK, check_joint_delta, + pose_to_se3, ) from dimos.utils.logging_config import setup_logger @@ -139,7 +140,7 @@ def __init__(self, name: str, config: TeleopIKTaskConfig) -> None: # Thread-safe target state self._lock = threading.Lock() - self._target_pose: pinocchio.SE3 | None = None + self._target_pose: Pose | PoseStamped | None = None self._last_update_time: float = 0.0 self._active = False @@ -195,7 +196,10 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: self._active = False return None - delta_pose = self._target_pose + 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: @@ -217,8 +221,8 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: if self._initial_ee_pose is None: return None target_pose = pinocchio.SE3( - delta_pose.rotation @ self._initial_ee_pose.rotation, - self._initial_ee_pose.translation + delta_pose.translation, + delta_se3.rotation @ self._initial_ee_pose.rotation, + self._initial_ee_pose.translation + delta_se3.translation, ) # Get current joint positions for IK warm-start @@ -303,10 +307,8 @@ def on_buttons(self, msg: QuestButtons) -> None: def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool: """Handle incoming cartesian command (delta pose from teleop)""" - delta_se3 = PinocchioIK.pose_to_se3(pose) - with self._lock: - self._target_pose = delta_se3 # Store delta, will apply to initial in compute() + self._target_pose = pose # Store raw, convert to SE3 in compute() self._last_update_time = t_now self._active = True From 0e7872cceda987383fffbef6f032bc79cb5ed092 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Fri, 13 Feb 2026 12:38:19 -0800 Subject: [PATCH 21/27] Feat: Added BaseControlTask + removal of hasattr for methodchecking --- dimos/control/coordinator.py | 18 ++++----- dimos/control/task.py | 49 +++++++++++++++++++++++- dimos/control/tasks/cartesian_ik_task.py | 4 +- dimos/control/tasks/servo_task.py | 4 +- dimos/control/tasks/teleop_task.py | 7 ++-- dimos/control/tasks/trajectory_task.py | 4 +- dimos/control/tasks/velocity_task.py | 4 +- 7 files changed, 66 insertions(+), 24 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index e4201adbe8..81985829f0 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -461,14 +461,14 @@ def _on_joint_command(self, msg: JointState) -> None: continue # Route to servo tasks (position control) - if hasattr(task, "set_target_by_name") and msg.position: + if msg.position: positions_by_name = dict(zip(msg.name, msg.position, strict=False)) - task.set_target_by_name(positions_by_name, t_now) # type: ignore[attr-defined] + task.set_target_by_name(positions_by_name, t_now) # Route to velocity tasks (velocity control) - elif hasattr(task, "set_velocities_by_name") and msg.velocity: + elif msg.velocity: velocities_by_name = dict(zip(msg.name, msg.velocity, strict=False)) - task.set_velocities_by_name(velocities_by_name, t_now) # type: ignore[attr-defined] + task.set_velocities_by_name(velocities_by_name, t_now) def _on_cartesian_command(self, msg: PoseStamped) -> None: """Route incoming PoseStamped to CartesianIKTask by task name. @@ -488,17 +488,13 @@ def _on_cartesian_command(self, msg: PoseStamped) -> None: logger.warning(f"Cartesian command for unknown task: {task_name}") return - if hasattr(task, "on_cartesian_command"): - task.on_cartesian_command(msg, t_now) # type: ignore[attr-defined] - else: - logger.warning(f"Task {task_name} does not support on_cartesian_command") + task.on_cartesian_command(msg, t_now) def _on_buttons(self, msg: QuestButtons) -> None: - """Forward button state to all tasks that accept it.""" + """Forward button state to all tasks.""" with self._task_lock: for task in self._tasks.values(): - if hasattr(task, "on_buttons"): - task.on_buttons(msg) # type: ignore[attr-defined] + task.on_buttons(msg) @rpc def task_invoke( diff --git a/dimos/control/task.py b/dimos/control/task.py index e9d9c4a584..4ff789d1bb 100644 --- a/dimos/control/task.py +++ b/dimos/control/task.py @@ -28,11 +28,15 @@ from __future__ import annotations from dataclasses import dataclass, field -from typing import Protocol, runtime_checkable +from typing import TYPE_CHECKING, Protocol, runtime_checkable from dimos.control.components import JointName from dimos.hardware.manipulators.spec import ControlMode +if TYPE_CHECKING: + from dimos.msgs.geometry_msgs import Pose, PoseStamped + from dimos.teleop.quest.quest_types import QuestButtons + # ============================================================================= # Data Types # ============================================================================= @@ -287,11 +291,52 @@ def on_preempted(self, by_task: str, joints: frozenset[JointName]) -> None: """ ... + def on_buttons(self, msg: QuestButtons) -> bool: + """Handle button state from teleop controllers.""" + ... + + def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool: + """Handle incoming cartesian command (target or delta pose).""" + ... + + def set_target_by_name(self, positions: dict[str, float], t_now: float) -> bool: + """Handle servo position commands by joint name.""" + ... + + def set_velocities_by_name(self, velocities: dict[str, float], t_now: float) -> bool: + """Handle velocity commands by joint name.""" + ... + + +class BaseControlTask(ControlTask): + """Base class with no-op defaults for optional listener methods. + + Inherit from this to avoid implementing empty methods + in tasks that don't need them. Only override what your task uses. + """ + + def on_buttons(self, msg: QuestButtons) -> bool: + """No-op default.""" + return False + + def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool: + """No-op default.""" + return False + + def set_target_by_name(self, positions: dict[str, float], t_now: float) -> bool: + """No-op default.""" + return False + + def set_velocities_by_name(self, velocities: dict[str, float], t_now: float) -> bool: + """No-op default.""" + return False + __all__ = [ # Types "ControlMode", - # Protocol + # Protocol + Base + "BaseControlTask", "ControlTask", "CoordinatorState", "JointCommandOutput", diff --git a/dimos/control/tasks/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task.py index e6d5273a30..48340727c3 100644 --- a/dimos/control/tasks/cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task.py @@ -30,8 +30,8 @@ import numpy as np from dimos.control.task import ( + BaseControlTask, ControlMode, - ControlTask, CoordinatorState, JointCommandOutput, ResourceClaim, @@ -76,7 +76,7 @@ class CartesianIKTaskConfig: max_joint_delta_deg: float = 15.0 # ~1500°/s at 100Hz -class CartesianIKTask(ControlTask): +class CartesianIKTask(BaseControlTask): """Cartesian control task with internal Pinocchio IK solver. Accepts streaming cartesian poses via on_cartesian_command() and computes IK diff --git a/dimos/control/tasks/servo_task.py b/dimos/control/tasks/servo_task.py index 9c5eb1f446..b69b4dd099 100644 --- a/dimos/control/tasks/servo_task.py +++ b/dimos/control/tasks/servo_task.py @@ -27,8 +27,8 @@ import threading from dimos.control.task import ( + BaseControlTask, ControlMode, - ControlTask, CoordinatorState, JointCommandOutput, ResourceClaim, @@ -53,7 +53,7 @@ class JointServoTaskConfig: timeout: float = 0.5 # 500ms default timeout -class JointServoTask(ControlTask): +class JointServoTask(BaseControlTask): """Streaming joint position control for teleoperation/visual servoing. Accepts target positions via set_target() or set_target_by_name() and diff --git a/dimos/control/tasks/teleop_task.py b/dimos/control/tasks/teleop_task.py index ef7787f240..bff02539f3 100644 --- a/dimos/control/tasks/teleop_task.py +++ b/dimos/control/tasks/teleop_task.py @@ -33,8 +33,8 @@ import pinocchio # type: ignore[import-untyped] from dimos.control.task import ( + BaseControlTask, ControlMode, - ControlTask, CoordinatorState, JointCommandOutput, ResourceClaim, @@ -80,7 +80,7 @@ class TeleopIKTaskConfig: hand: str = "" -class TeleopIKTask(ControlTask): +class TeleopIKTask(BaseControlTask): """Teleop cartesian control task with internal Pinocchio IK solver. Accepts streaming cartesian delta poses via on_cartesian_command() and computes IK @@ -278,7 +278,7 @@ def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: # Task-specific methods # ========================================================================= - def on_buttons(self, msg: QuestButtons) -> None: + def on_buttons(self, msg: QuestButtons) -> bool: """Press-and-hold engage: hold primary button to track, release to stop. Checks only the button matching self._config.hand (left_x or right_a). @@ -304,6 +304,7 @@ def on_buttons(self, msg: QuestButtons) -> None: self._target_pose = None self._initial_ee_pose = None self._prev_primary = primary + return True def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool: """Handle incoming cartesian command (delta pose from teleop)""" diff --git a/dimos/control/tasks/trajectory_task.py b/dimos/control/tasks/trajectory_task.py index 676f1dac70..4d2eaa188b 100644 --- a/dimos/control/tasks/trajectory_task.py +++ b/dimos/control/tasks/trajectory_task.py @@ -26,8 +26,8 @@ from dataclasses import dataclass from dimos.control.task import ( + BaseControlTask, ControlMode, - ControlTask, CoordinatorState, JointCommandOutput, ResourceClaim, @@ -51,7 +51,7 @@ class JointTrajectoryTaskConfig: priority: int = 10 -class JointTrajectoryTask(ControlTask): +class JointTrajectoryTask(BaseControlTask): """Passive trajectory execution task. Unlike JointTrajectoryController which owns a thread, this task diff --git a/dimos/control/tasks/velocity_task.py b/dimos/control/tasks/velocity_task.py index 23c56b0511..163bc09827 100644 --- a/dimos/control/tasks/velocity_task.py +++ b/dimos/control/tasks/velocity_task.py @@ -29,8 +29,8 @@ import threading from dimos.control.task import ( + BaseControlTask, ControlMode, - ControlTask, CoordinatorState, JointCommandOutput, ResourceClaim, @@ -57,7 +57,7 @@ class JointVelocityTaskConfig: zero_on_timeout: bool = True # Send zeros to stop motion -class JointVelocityTask(ControlTask): +class JointVelocityTask(BaseControlTask): """Streaming joint velocity control for joystick/force feedback. Accepts target velocities via set_velocities() or set_velocities_by_name() From 1842857ee8a94e4eaf9bc270c662988a4ffb3a46 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Fri, 13 Feb 2026 12:38:47 -0800 Subject: [PATCH 22/27] Fix: pre-commit fixes --- dimos/control/task.py | 4 ++-- dimos/manipulation/planning/kinematics/pinocchio_ik.py | 1 - dimos/teleop/quest/quest_extensions.py | 4 +++- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/dimos/control/task.py b/dimos/control/task.py index 4ff789d1bb..5782393880 100644 --- a/dimos/control/task.py +++ b/dimos/control/task.py @@ -333,10 +333,10 @@ def set_velocities_by_name(self, velocities: dict[str, float], t_now: float) -> __all__ = [ - # Types - "ControlMode", # Protocol + Base "BaseControlTask", + # Types + "ControlMode", "ControlTask", "CoordinatorState", "JointCommandOutput", diff --git a/dimos/manipulation/planning/kinematics/pinocchio_ik.py b/dimos/manipulation/planning/kinematics/pinocchio_ik.py index 4841e76cc6..07c81ab0a2 100644 --- a/dimos/manipulation/planning/kinematics/pinocchio_ik.py +++ b/dimos/manipulation/planning/kinematics/pinocchio_ik.py @@ -225,7 +225,6 @@ def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pino return self._data.oMi[self._ee_joint_id].copy() - # ============================================================================= # Pose Conversion Helpers # ============================================================================= diff --git a/dimos/teleop/quest/quest_extensions.py b/dimos/teleop/quest/quest_extensions.py index 76d040191c..b121e431f6 100644 --- a/dimos/teleop/quest/quest_extensions.py +++ b/dimos/teleop/quest/quest_extensions.py @@ -114,7 +114,9 @@ class ArmTeleopModule(QuestTeleopModule): def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) - self._task_names: dict[Hand, str] = {Hand[k.upper()]: v for k, v in self.config.task_names.items()} + self._task_names: dict[Hand, str] = { + Hand[k.upper()]: v for k, v in self.config.task_names.items() + } def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: """Stamp frame_id with task name and publish.""" From 84a2efb329be7fdcfa3052c37e64723b77a9469a Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Fri, 13 Feb 2026 14:45:18 -0800 Subject: [PATCH 23/27] Fix: remove .transport checks in coordinator subscriptions --- dimos/control/coordinator.py | 58 +++++++++++++++++------------------- 1 file changed, 27 insertions(+), 31 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 81985829f0..8f58a25e43 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -587,43 +587,39 @@ def start(self) -> None: streaming_types = ("servo", "velocity") has_streaming = any(t.type in streaming_types for t in self.config.tasks) if has_streaming: - # Only subscribe if transport is configured try: - if self.joint_command.transport: - self._joint_command_unsub = self.joint_command.subscribe(self._on_joint_command) - logger.info("Subscribed to joint_command for streaming tasks") - else: - logger.warning( - "Streaming tasks configured but no transport set for joint_command. " - "Use task_invoke RPC or set transport via blueprint." - ) - except Exception as e: - logger.warning(f"Could not subscribe to joint_command: {e}") + self._joint_command_unsub = self.joint_command.subscribe(self._on_joint_command) + logger.info("Subscribed to joint_command for streaming tasks") + except Exception: + logger.warning( + "Streaming tasks configured but could not subscribe to joint_command. " + "Use task_invoke RPC or set transport via blueprint." + ) # Subscribe to cartesian commands if any cartesian_ik tasks configured has_cartesian_ik = any(t.type in ("cartesian_ik", "teleop_ik") for t in self.config.tasks) if has_cartesian_ik: try: - if self.cartesian_command.transport: - self._cartesian_command_unsub = self.cartesian_command.subscribe( - self._on_cartesian_command - ) - logger.info("Subscribed to cartesian_command for CartesianIK/TeleopIK tasks") - else: - logger.warning( - "CartesianIK/TeleopIK tasks configured but no transport set for cartesian_command. " - "Use task_invoke RPC or set transport via blueprint." - ) - except Exception as e: - logger.warning(f"Could not subscribe to cartesian_command: {e}") - - # Also subscribe to buttons for engage/disengage - try: - if self.buttons.transport: - self._buttons_unsub = self.buttons.subscribe(self._on_buttons) - logger.info("Subscribed to buttons for engage/disengage") - except Exception as e: - logger.warning(f"Could not subscribe to buttons: {e}") + self._cartesian_command_unsub = self.cartesian_command.subscribe( + self._on_cartesian_command + ) + logger.info("Subscribed to cartesian_command for CartesianIK/TeleopIK tasks") + except Exception: + logger.warning( + "CartesianIK/TeleopIK tasks configured but could not subscribe to cartesian_command. " + "Use task_invoke RPC or set transport via blueprint." + ) + + # Subscribe to buttons if any teleop_ik tasks configured (engage/disengage) + has_teleop_ik = any(t.type == "teleop_ik" for t in self.config.tasks) + if has_teleop_ik: + if not self.buttons.transport: + raise ValueError( + "TeleopIK tasks require buttons transport for engage/disengage. " + "Wire buttons via blueprint." + ) + self._buttons_unsub = self.buttons.subscribe(self._on_buttons) + logger.info("Subscribed to buttons for engage/disengage") logger.info(f"ControlCoordinator started at {self.config.tick_rate}Hz") From 77cb5e97d67a400738245de9566f72a9dd50def7 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Fri, 13 Feb 2026 17:27:56 -0800 Subject: [PATCH 24/27] Comments restructure --- dimos/control/coordinator.py | 5 ----- dimos/control/tasks/cartesian_ik_task.py | 9 +-------- dimos/control/tasks/teleop_task.py | 2 -- 3 files changed, 1 insertion(+), 15 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 8f58a25e43..f0db048db6 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -613,11 +613,6 @@ def start(self) -> None: # Subscribe to buttons if any teleop_ik tasks configured (engage/disengage) has_teleop_ik = any(t.type == "teleop_ik" for t in self.config.tasks) if has_teleop_ik: - if not self.buttons.transport: - raise ValueError( - "TeleopIK tasks require buttons transport for engage/disengage. " - "Wire buttons via blueprint." - ) self._buttons_unsub = self.buttons.subscribe(self._on_buttons) logger.info("Subscribed to buttons for engage/disengage") diff --git a/dimos/control/tasks/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task.py index 48340727c3..6ea5ddc55b 100644 --- a/dimos/control/tasks/cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task.py @@ -178,7 +178,6 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: with self._lock: if not self._active or self._target_pose is None: return None - # Check timeout if self._config.timeout > 0: time_since_update = state.t_now - self._last_update_time @@ -189,12 +188,10 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: ) self._active = False return None - raw_pose = self._target_pose # Convert to SE3 right before use target_pose = pose_to_se3(raw_pose) - # Get current joint positions for IK warm-start q_current = self._get_current_joints(state) if q_current is None: @@ -203,10 +200,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | 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 - the safety clamp - # will handle any large jumps. This prevents the arm from "sticking" - # when near singularities or workspace boundary. + # Use the solution even if it didn't fully converge if not converged: logger.debug( f"CartesianIKTask {self._name}: IK did not converge " @@ -226,7 +220,6 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: # Cache solution for next warm-start with self._lock: self._last_q_solution = q_solution.copy() - return JointCommandOutput( joint_names=self._joint_names_list, positions=q_solution.flatten().tolist(), diff --git a/dimos/control/tasks/teleop_task.py b/dimos/control/tasks/teleop_task.py index bff02539f3..0399f52c23 100644 --- a/dimos/control/tasks/teleop_task.py +++ b/dimos/control/tasks/teleop_task.py @@ -195,12 +195,10 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: 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 From 2f9b0c1b21a33134fec892466215f51f50e74979 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Sat, 14 Feb 2026 12:13:15 -0800 Subject: [PATCH 25/27] fix: LfsPath resolution --- dimos/manipulation/planning/kinematics/pinocchio_ik.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/manipulation/planning/kinematics/pinocchio_ik.py b/dimos/manipulation/planning/kinematics/pinocchio_ik.py index 07c81ab0a2..4224dda556 100644 --- a/dimos/manipulation/planning/kinematics/pinocchio_ik.py +++ b/dimos/manipulation/planning/kinematics/pinocchio_ik.py @@ -135,7 +135,7 @@ def from_model_path( Raises: FileNotFoundError: If model file doesn't exist """ - path = Path(model_path) + path = Path(str(model_path)) if not path.exists(): raise FileNotFoundError(f"Model file not found: {path}") From fee4c6a351c90a37715ee5b0912768f097cf3baf Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Sat, 14 Feb 2026 12:14:47 -0800 Subject: [PATCH 26/27] fix: replace python-version-specific safe attributes with try/except + replace in tests --- dimos/utils/data.py | 46 ++++++++-------------------------------- dimos/utils/test_data.py | 23 +++++++++++++------- 2 files changed, 24 insertions(+), 45 deletions(-) diff --git a/dimos/utils/data.py b/dimos/utils/data.py index aba2e50e5a..48e73f9105 100644 --- a/dimos/utils/data.py +++ b/dimos/utils/data.py @@ -271,41 +271,6 @@ class LfsPath(type(Path())): # type: ignore[misc] files = list(path.iterdir()) """ - # Attributes that should NOT trigger download - _LFS_SAFE_ATTRIBUTES = { - # LfsPath internal attributes - "_lfs_filename", - "_lfs_resolved_cache", - "_ensure_downloaded", - # Python magic methods - "__class__", - "__dict__", - "__init__", - "__new__", - "__getattribute__", - "__setattr__", - "__delattr__", - # Path internal attributes (needed for Path operations) - "_drv", - "_flavour", - "_format_parsed_parts", - "_from_parsed_parts", - "_hash", - "_load_parts", - "_make_child_relpath", - "_parse_path", - "_parts_normcase", - "_parts_normcase_cached", - "_raw_paths", - "_root", - "_scandir", - "_str", - "_str_normcase", - "_str_normcase_cached", - "_tail", - "_tail_cached", - } - def __new__(cls, filename: str | Path) -> "LfsPath": # Create instance with a placeholder path to satisfy Path.__new__ # We use "." as a dummy path that always exists @@ -325,8 +290,15 @@ def _ensure_downloaded(self) -> Path: return cache def __getattribute__(self, name: str) -> object: - # Allow access to safe attributes without triggering download - if name in LfsPath._LFS_SAFE_ATTRIBUTES: + # During Path.__new__(), _lfs_filename hasn't been set yet. + # Fall through to normal Path behavior until construction is complete. + try: + object.__getattribute__(self, "_lfs_filename") + except AttributeError: + return object.__getattribute__(self, name) + + # After construction, allow access to our internal attributes directly + if name in ("_lfs_filename", "_lfs_resolved_cache", "_ensure_downloaded"): return object.__getattribute__(self, name) # For all other attributes, ensure download first then delegate to resolved path diff --git a/dimos/utils/test_data.py b/dimos/utils/test_data.py index d60e7cc355..265530abcc 100644 --- a/dimos/utils/test_data.py +++ b/dimos/utils/test_data.py @@ -168,16 +168,23 @@ def test_lfs_path_safe_attributes() -> None: assert callable(ensure_fn) -def test_lfs_path_conversion_to_path() -> None: - """Test that LfsPath can be converted to regular Path.""" - # Use a file that doesn't exist to test conversion without download +def test_lfs_path_no_download_on_creation() -> None: + """Test that LfsPath construction doesn't trigger download. + + Path(lfs_path) extracts internal _raw_paths (\".\") and does NOT + call __fspath__, so it won't trigger download. The correct way to + convert is Path(str(lfs_path)), which triggers __str__ -> download. + """ lfs_path = LfsPath("nonexistent_file") - # Path conversion accesses internal attributes but shouldn't trigger download - # because Path internal attributes are in the safe list - converted = Path(lfs_path) - assert isinstance(converted, Path) - assert type(converted).__name__ in ("PosixPath", "WindowsPath") + # Construction should not trigger download + cache = object.__getattribute__(lfs_path, "_lfs_resolved_cache") + assert cache is None + + # Accessing internal LfsPath attributes should not trigger download + filename = object.__getattribute__(lfs_path, "_lfs_filename") + assert filename == "nonexistent_file" + assert cache is None @pytest.mark.heavy From d1c8d0ecef4ddece7b9e6ff4426ad9b1c2b1c26d Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Sat, 14 Feb 2026 12:31:43 -0800 Subject: [PATCH 27/27] Fix: merge conflicts --- dimos/utils/data.py | 25 ++++++++++++------------- dimos/utils/test_data.py | 2 +- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/dimos/utils/data.py b/dimos/utils/data.py index 960c538c2f..d14ac04730 100644 --- a/dimos/utils/data.py +++ b/dimos/utils/data.py @@ -250,7 +250,18 @@ def get_data(name: str | Path) -> Path: if file_path.exists(): return file_path - return _decompress_archive(_pull_lfs_archive(filename)) + # extract archive root (first path component) and nested path + path_parts = Path(name).parts + archive_name = path_parts[0] + nested_path = Path(*path_parts[1:]) if len(path_parts) > 1 else None + + # download and decompress the archive root + archive_path = _decompress_archive(_pull_lfs_archive(archive_name)) + + # return full path including nested components + if nested_path: + return archive_path / nested_path + return archive_path class LfsPath(type(Path())): # type: ignore[misc] @@ -323,15 +334,3 @@ def __truediv__(self, other: object) -> Path: def __rtruediv__(self, other: object) -> Path: """Reverse path division operator.""" return other / self._ensure_downloaded() # type: ignore[operator, return-value] - # extract archive root (first path component) and nested path - path_parts = Path(name).parts - archive_name = path_parts[0] - nested_path = Path(*path_parts[1:]) if len(path_parts) > 1 else None - - # download and decompress the archive root - archive_path = _decompress_archive(_pull_lfs_archive(archive_name)) - - # return full path including nested components - if nested_path: - return archive_path / nested_path - return archive_path diff --git a/dimos/utils/test_data.py b/dimos/utils/test_data.py index 358aa64349..e5be4307c7 100644 --- a/dimos/utils/test_data.py +++ b/dimos/utils/test_data.py @@ -225,7 +225,7 @@ def test_lfs_path_with_real_file() -> None: def test_lfs_path_unload_and_reload() -> None: """Test unloading and reloading an LFS file.""" filename = "three_paths.png" - data_dir = data._get_data_dir() + data_dir = data.get_data_dir() file_path = data_dir / filename # Clean up if file already exists