diff --git a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py
index 7f9bb5145b..fbaa8e7360 100644
--- a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py
+++ b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py
@@ -12,7 +12,7 @@
# See the License for the specific language governing permissions and
# limitations under the License.
-"""Cartesian control task with internal Pinocchio IK solver.
+"""Cartesian control task with Pink differential IK by default.
Accepts streaming cartesian poses (e.g., from teleoperation, visual servoing)
and computes inverse kinematics internally to output joint commands.
@@ -22,12 +22,14 @@
from __future__ import annotations
from dataclasses import dataclass
-from pathlib import Path
import threading
-from typing import TYPE_CHECKING, Any
+from typing import TYPE_CHECKING
import numpy as np
+import pinocchio
+from pydantic import FiniteFloat
+from dimos.control.coordinator import TaskConfig
from dimos.control.task import (
BaseControlTask,
ControlMode,
@@ -35,18 +37,20 @@
JointCommandOutput,
ResourceClaim,
)
+from dimos.control.tasks.cartesian_ik_task.pink_control_ik import (
+ PinkControlIK,
+ PinkControlIKConfig,
+ create_pink_control_ik,
+)
from dimos.manipulation.planning.kinematics.pinocchio_ik import (
- PinocchioIK,
check_joint_delta,
get_worst_joint_delta,
- pose_to_se3,
)
from dimos.protocol.service.spec import BaseConfig
from dimos.utils.logging_config import setup_logger
if TYPE_CHECKING:
from numpy.typing import NDArray
- import pinocchio
from dimos.msgs.geometry_msgs.Pose import Pose
from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
@@ -60,40 +64,50 @@ class CartesianIKTaskConfig:
Attributes:
joint_names: List of joint names this task controls (must match model DOF)
- model_path: Path to URDF or MJCF file for IK solver
- ee_joint_id: End-effector joint ID in the kinematic chain
priority: Priority for arbitration (higher wins)
timeout: If no command received for this many seconds, go inactive (0 = never)
max_joint_delta_deg: Maximum allowed joint change per tick (safety limit)
"""
joint_names: list[str]
- model_path: str | Path
- ee_joint_id: int
+ control_ik: PinkControlIKConfig
priority: int = 10
timeout: float = 0.5
max_joint_delta_deg: float = 15.0 # ~1500°/s at 100Hz
+ min_dt: FiniteFloat = 1e-4
+ max_dt: FiniteFloat = 0.05
+
+ def __post_init__(self) -> None:
+ if (
+ not np.isfinite(self.min_dt)
+ or not np.isfinite(self.max_dt)
+ or self.min_dt <= 0.0
+ or self.max_dt <= 0.0
+ ):
+ raise ValueError("CartesianIKTask dt bounds must be finite and positive")
+ if self.max_dt < self.min_dt:
+ raise ValueError("CartesianIKTask dt bounds must be ordered")
class CartesianIKTask(BaseControlTask):
- """Cartesian control task with internal Pinocchio IK solver.
+ """Cartesian control task with Pink differential 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.
+ internally to output joint commands. Pink re-anchors each solve to the
+ current joint state from CoordinatorState.
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")
+ >>> from dimos.robot.manipulators.piper.config import make_piper_model_config
>>> 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,
+ ... control_ik=PinkControlIKConfig(
+ ... robot_model=make_piper_model_config(),
+ ... ),
... priority=10,
... timeout=0.5,
... ),
@@ -112,23 +126,30 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None:
name: Unique task name
config: Task configuration
"""
- if not config.joint_names:
+ if not config.joint_names or len(set(config.joint_names)) != len(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")
+ if not np.isfinite(config.timeout) or config.timeout < 0.0:
+ raise ValueError("CartesianIKTask timeout must be finite and non-negative")
+ if not np.isfinite(config.max_joint_delta_deg) or config.max_joint_delta_deg <= 0.0:
+ raise ValueError("CartesianIKTask max_joint_delta_deg must be positive and finite")
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)
+ expected_joints = config.control_ik.robot_model.get_coordinator_joint_names()
+ if config.joint_names != expected_joints:
+ raise ValueError(
+ f"CartesianIKTask {name}: task joints must match RobotModelConfig coordinator joints"
+ )
# Create IK solver from model
- self._ik = PinocchioIK.from_model_path(config.model_path, config.ee_joint_id)
+ self._ik: PinkControlIK = create_pink_control_ik(config.control_ik)
# Validate DOF matches joint names
if self._ik.nq != self._num_joints:
- logger.warning(
+ raise ValueError(
f"CartesianIKTask {name}: model DOF ({self._ik.nq}) != "
f"joint_names count ({self._num_joints})"
)
@@ -139,12 +160,10 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None:
self._last_update_time: float = 0.0
self._active = False
- # 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: {config.model_path}, "
- f"ee_joint_id={config.ee_joint_id}, joints={config.joint_names}"
+ f"CartesianIKTask {name} initialized with model: "
+ f"{config.control_ik.robot_model.model_path}, "
+ f"joints={config.joint_names}"
)
def claim(self) -> ResourceClaim:
@@ -164,13 +183,14 @@ 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)
+ state: Current coordinator state (contains measured joint positions)
Returns:
- JointCommandOutput with positions, or None if inactive/timed out/IK failed
+ JointCommandOutput with positions or a measured-state hold after an
+ expected runtime failure; None if inactive or timed out.
"""
with self._lock:
- if not self._active or self._target_pose is None:
+ if not self._active or (self._target_pose is None and not self._uses_prepared_target()):
return None
# Check timeout
if self._config.timeout > 0:
@@ -181,25 +201,39 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
f"(no update for {time_since_update:.3f}s)"
)
self._active = False
+ self._target_pose = None
+ self._on_timeout()
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:
logger.debug(f"CartesianIKTask {self._name}: missing joint state for IK warm-start")
return None
+ if not np.all(np.isfinite(q_current)):
+ logger.error("CartesianIKTask %s: measured joint state is non-finite", self._name)
+ return None
+ raw_dt = state.dt
+ if not np.isfinite(raw_dt) or raw_dt <= 0.0:
+ return self._hold(q_current)
+ dt = min(max(raw_dt, self._config.min_dt), self._config.max_dt)
+ try:
+ target_pose = self._prepare_target(state, q_current, dt)
+ except (FloatingPointError, RuntimeError, ValueError) as exc:
+ logger.warning("CartesianIKTask %s: target preparation failed: %s", self._name, exc)
+ return self._hold(q_current)
+ if target_pose is None:
+ return self._hold(q_current)
# Compute IK
- q_solution, converged, final_error = self._ik.solve(target_pose, q_current)
- # Use the solution even if it didn't fully converge
- if not converged:
- logger.debug(
- f"CartesianIKTask {self._name}: IK did not converge "
- f"(error={final_error:.4f}), using partial solution"
- )
+ try:
+ result = self._ik.solve(target_pose, q_current, dt)
+ except (FloatingPointError, RuntimeError, ValueError) as exc:
+ logger.warning("CartesianIKTask %s: IK solve failed: %s", self._name, exc)
+ return self._hold(q_current)
+ q_solution = np.asarray(result.positions, dtype=np.float64).reshape(-1)
+ if not np.all(np.isfinite(q_solution)) or q_solution.shape != q_current.shape:
+ logger.warning("CartesianIKTask %s: rejecting invalid IK output", self._name)
+ return self._hold(q_current)
# Safety check: reject if any joint delta exceeds limit
if not check_joint_delta(q_solution, q_current, self._config.max_joint_delta_deg):
@@ -209,34 +243,68 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
f"joint {self._joint_names_list[worst_idx]} delta "
f"{worst_deg:.1f}° exceeds limit {self._config.max_joint_delta_deg}°"
)
- return None
+ return self._hold(q_current)
- # 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(),
mode=ControlMode.SERVO_POSITION,
)
- def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.floating[Any]] | None:
- """Get current joint positions from coordinator state.
+ def _hold(self, q_current: NDArray[np.float64]) -> JointCommandOutput:
+ """Keep the measured configuration under the task's servo contract."""
+ return JointCommandOutput(
+ joint_names=self._joint_names_list,
+ positions=q_current.tolist(),
+ mode=ControlMode.SERVO_POSITION,
+ )
- Falls back to last IK solution if joint state unavailable.
- """
+ def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.float64] | None:
+ """Get the measured coordinator joint snapshot (never a command cache)."""
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:
- result: NDArray[np.floating[Any]] = self._last_q_solution.copy()
- return result
return None
positions.append(pos)
return np.array(positions, dtype=np.float64)
+ def _prepare_target(
+ self,
+ state: CoordinatorState,
+ q_current: NDArray[np.float64],
+ dt: float,
+ ) -> pinocchio.SE3 | None:
+ """Prepare one normalized target for the measured-state solve."""
+ with self._lock:
+ pose = self._target_pose
+ if pose is None:
+ return None
+ quaternion = np.array(
+ [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w],
+ dtype=np.float64,
+ )
+ quaternion_norm = float(np.linalg.norm(quaternion))
+ if not np.isfinite(quaternion_norm) or quaternion_norm <= 1e-12:
+ return None
+ normalized = quaternion / quaternion_norm
+ target = pinocchio.SE3(
+ pinocchio.Quaternion(
+ normalized[3], normalized[0], normalized[1], normalized[2]
+ ).toRotationMatrix(),
+ np.array([pose.x, pose.y, pose.z], dtype=np.float64),
+ )
+ values = np.concatenate((target.translation, target.rotation.reshape(-1)))
+ if not np.all(np.isfinite(values)):
+ return None
+ return target
+
+ def _on_timeout(self) -> None:
+ """Hook for target sources with state outside the Cartesian pose cache."""
+
+ def _uses_prepared_target(self) -> bool:
+ return False
+
def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:
"""Handle preemption by higher-priority task.
@@ -276,6 +344,7 @@ def stop(self) -> None:
"""Deactivate the task (stop outputting commands)."""
with self._lock:
self._active = False
+ self._target_pose = None
logger.info(f"CartesianIKTask {self._name} stopped")
def clear(self) -> None:
@@ -307,7 +376,7 @@ def get_current_ee_pose(self, state: CoordinatorState) -> pinocchio.SE3 | None:
return self._ik.forward_kinematics(q_current)
- def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pinocchio.SE3:
+ def forward_kinematics(self, joint_positions: NDArray[np.float64]) -> pinocchio.SE3:
"""Compute end-effector pose from joint positions.
Args:
@@ -320,18 +389,20 @@ def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pino
class CartesianIKTaskParams(BaseConfig):
- model_path: str | Path
- ee_joint_id: int = 6
+ control_ik: PinkControlIKConfig
+ min_dt: FiniteFloat = 1e-4
+ max_dt: FiniteFloat = 0.05
-def create_task(cfg: Any, hardware: Any) -> CartesianIKTask:
+def create_task(cfg: TaskConfig, hardware: object) -> CartesianIKTask:
params = CartesianIKTaskParams.model_validate(cfg.params)
return CartesianIKTask(
cfg.name,
CartesianIKTaskConfig(
joint_names=cfg.joint_names,
- model_path=params.model_path,
- ee_joint_id=params.ee_joint_id,
priority=cfg.priority,
+ min_dt=params.min_dt,
+ max_dt=params.max_dt,
+ control_ik=params.control_ik,
),
)
diff --git a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py
new file mode 100644
index 0000000000..4cd9cdb2a7
--- /dev/null
+++ b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py
@@ -0,0 +1,434 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Pink differential IK for coordinator Cartesian control."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+
+import numpy as np
+from numpy.typing import NDArray
+import pinocchio
+from pydantic import Field, FiniteFloat
+
+_PINK_INSTALL_ERROR = "Pink control tasks require the 'pink' dependency. Install it with `uv sync`."
+
+try:
+ from pink import Configuration, solve_ik
+ from pink.limits import ConfigurationLimit, VelocityLimit
+ from pink.tasks import FrameTask, PostureTask
+except ModuleNotFoundError as exc:
+ raise ModuleNotFoundError(
+ f"{_PINK_INSTALL_ERROR} Missing module: {exc.name}",
+ name=exc.name,
+ ) from exc
+
+from dimos.manipulation.planning.spec.config import RobotModelConfig
+from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake
+from dimos.protocol.service.spec import BaseConfig
+
+# Pink's integration/QP boundary tolerance is small but larger than machine epsilon.
+_POSITION_LIMIT_EPSILON_RAD = 1e-5
+
+
+class PinkControlIKConfig(BaseConfig):
+ """Typed configuration for the control IK backend."""
+
+ robot_model: RobotModelConfig
+ solver: str = "proxqp"
+ max_velocity: FiniteFloat = Field(10.0, gt=0.0)
+ lm_damping: FiniteFloat = Field(1e-4, gt=0.0)
+ task_gain: FiniteFloat = Field(1.0, gt=0.0)
+ position_cost: FiniteFloat = Field(1.0, ge=0.0)
+ orientation_cost: FiniteFloat = Field(1.0, ge=0.0)
+ posture_cost: FiniteFloat = Field(1e-3, ge=0.0)
+ reference_q: list[float] | None = None
+ qpsolver_options: dict[str, FiniteFloat] = Field(default_factory=dict)
+
+
+@dataclass(frozen=True)
+class ControlIKResult:
+ positions: NDArray[np.float64]
+ velocity: NDArray[np.float64]
+
+
+class IKControlRuntimeError(RuntimeError):
+ """A runtime solver/model failure that should produce a bounded hold."""
+
+
+@dataclass(frozen=True)
+class _CoordinateMapping:
+ joint_names: tuple[str, ...]
+ q_indices: tuple[int, ...]
+ v_indices: tuple[int, ...]
+ q_widths: tuple[int, ...]
+ joint_ids: frozenset[int]
+
+
+@dataclass(frozen=True)
+class _PinkRuntime:
+ config: PinkControlIKConfig
+ model: pinocchio.Model
+ data: pinocchio.Data
+ mapping: _CoordinateMapping
+ ee_frame_id: int
+ reference_q: NDArray[np.float64]
+ configuration: Configuration
+ frame_task: FrameTask
+ posture_task: PostureTask | None
+ tasks: list[object]
+ limits: list[object]
+
+
+class _PinkControlIKBuilder:
+ """Assemble model and Pink state before creating the runtime solver."""
+
+ def __init__(self, config: PinkControlIKConfig) -> None:
+ self._config = config
+
+ def build(self) -> _PinkRuntime:
+ config = self._config
+ robot = config.robot_model
+ prepared_path = Path(
+ prepare_urdf_for_drake(
+ robot.model_path,
+ package_paths=robot.package_paths,
+ xacro_args=robot.xacro_args,
+ convert_meshes=False,
+ )
+ )
+ if not prepared_path.exists():
+ raise FileNotFoundError(f"prepared Pink control URDF not found: {prepared_path}")
+
+ model = pinocchio.buildModelFromUrdf(str(prepared_path))
+ mapping = self._build_mapping(model, robot)
+ ee_frame_id = self._validate_frame(model, robot.end_effector_link)
+ limits = self._apply_limits(model, mapping, robot)
+ full_reference_q = self._build_reference_q(model, config.reference_q)
+ locked_joint_ids = [
+ joint_id
+ for joint_id in range(1, len(model.joints))
+ if joint_id not in mapping.joint_ids
+ ]
+ if locked_joint_ids:
+ if config.reference_q is None and self._uncontrolled_ee_chain(
+ model, ee_frame_id, mapping.joint_ids
+ ):
+ raise ValueError(
+ "Pink requires reference_q for an uncontrolled joint on the end-effector chain"
+ )
+ model = pinocchio.buildReducedModel(model, locked_joint_ids, full_reference_q)
+ mapping = self._build_mapping(model, robot)
+ ee_frame_id = self._validate_frame(model, robot.end_effector_link)
+ limits = self._apply_limits(model, mapping, robot)
+
+ data = model.createData()
+ reference_q = self._build_reference_q(model, None)
+ configuration = Configuration(
+ model,
+ data,
+ reference_q.copy(),
+ )
+ frame_task = FrameTask(
+ robot.end_effector_link,
+ position_cost=config.position_cost,
+ orientation_cost=config.orientation_cost,
+ lm_damping=config.lm_damping,
+ gain=config.task_gain,
+ )
+ posture_task = PostureTask(cost=config.posture_cost) if config.posture_cost > 0.0 else None
+ tasks: list[object] = [frame_task]
+ if posture_task is not None:
+ tasks.append(posture_task)
+
+ return _PinkRuntime(
+ config=config,
+ model=model,
+ data=data,
+ mapping=mapping,
+ ee_frame_id=ee_frame_id,
+ reference_q=reference_q,
+ configuration=configuration,
+ frame_task=frame_task,
+ posture_task=posture_task,
+ tasks=tasks,
+ limits=limits,
+ )
+
+ @staticmethod
+ def _build_mapping(
+ model: pinocchio.Model,
+ robot: RobotModelConfig,
+ ) -> _CoordinateMapping:
+ joint_names = tuple(robot.get_coordinator_joint_names())
+ if not joint_names or len(set(joint_names)) != len(joint_names):
+ raise ValueError("control task joints must be unique and non-empty")
+
+ q_indices: list[int] = []
+ v_indices: list[int] = []
+ q_widths: list[int] = []
+ joint_ids: set[int] = set()
+ for urdf_name in (robot.get_urdf_joint_name(name) for name in joint_names):
+ if not model.existJointName(urdf_name):
+ raise ValueError(f"control joint mapping references unknown joint: {urdf_name}")
+ joint_id = int(model.getJointId(urdf_name))
+ if joint_id <= 0 or joint_id >= len(model.joints):
+ raise ValueError(f"invalid control joint index for {urdf_name}")
+ joint = model.joints[joint_id]
+ if int(joint.nv) != 1 or int(joint.nq) not in (1, 2):
+ raise ValueError(f"control joint must be one-DoF: {urdf_name}")
+ q_indices.append(int(joint.idx_q))
+ v_indices.append(int(joint.idx_v))
+ q_widths.append(int(joint.nq))
+ joint_ids.add(joint_id)
+
+ return _CoordinateMapping(
+ joint_names=joint_names,
+ q_indices=tuple(q_indices),
+ v_indices=tuple(v_indices),
+ q_widths=tuple(q_widths),
+ joint_ids=frozenset(joint_ids),
+ )
+
+ @staticmethod
+ def _build_reference_q(
+ model: pinocchio.Model,
+ configured_reference_q: list[float] | None,
+ ) -> NDArray[np.float64]:
+ if configured_reference_q is not None:
+ q = np.asarray(configured_reference_q, dtype=np.float64).reshape(-1)
+ if q.size != model.nq or not np.all(np.isfinite(q)):
+ raise ValueError("Pink reference_q must match model nq and be finite")
+ else:
+ q = np.asarray(pinocchio.neutral(model), dtype=np.float64)
+ for joint_id in range(1, len(model.joints)):
+ joint = model.joints[joint_id]
+ start = int(joint.idx_q)
+ width = int(joint.nq)
+ if width == 2 and int(joint.nv) == 1:
+ q[start : start + 2] = (1.0, 0.0)
+ continue
+ if width != 1:
+ continue
+ lower = model.lowerPositionLimit[start]
+ upper = model.upperPositionLimit[start]
+ if np.isfinite(lower) and np.isfinite(upper):
+ q[start] = (lower + upper) / 2.0
+ elif np.isfinite(lower):
+ q[start] = max(0.0, lower)
+ elif np.isfinite(upper):
+ q[start] = min(0.0, upper)
+ else:
+ q[start] = 0.0
+ if not np.all(np.isfinite(q)):
+ raise ValueError("Pink reference configuration is not finite")
+ bounded = np.isfinite(model.lowerPositionLimit) & np.isfinite(model.upperPositionLimit)
+ if np.any(q[bounded] < model.lowerPositionLimit[bounded]) or np.any(
+ q[bounded] > model.upperPositionLimit[bounded]
+ ):
+ raise ValueError("Pink reference configuration violates model limits")
+ return q
+
+ @staticmethod
+ def _uncontrolled_ee_chain(
+ model: pinocchio.Model,
+ frame_id: int,
+ controlled_joint_ids: frozenset[int],
+ ) -> bool:
+ joint_id = int(model.frames[frame_id].parentJoint)
+ while joint_id > 0:
+ if joint_id not in controlled_joint_ids:
+ return True
+ joint_id = int(model.parents[joint_id])
+ return False
+
+ @staticmethod
+ def _validate_frame(model: pinocchio.Model, frame_name: str) -> int:
+ if not model.existFrame(frame_name):
+ raise ValueError(f"unknown control end-effector frame: {frame_name}")
+ frame_id = int(model.getFrameId(frame_name))
+ if frame_id < 0 or frame_id >= len(model.frames):
+ raise ValueError(f"invalid control end-effector frame: {frame_name}")
+ return frame_id
+
+ def _apply_limits(
+ self,
+ model: pinocchio.Model,
+ mapping: _CoordinateMapping,
+ robot: RobotModelConfig,
+ ) -> list[object]:
+ if robot.joint_limits_lower is not None or robot.joint_limits_upper is not None:
+ if robot.joint_limits_lower is None or robot.joint_limits_upper is None:
+ raise ValueError("both configured joint limit bounds are required")
+ if len(robot.joint_limits_lower) != len(mapping.joint_names) or len(
+ robot.joint_limits_upper
+ ) != len(mapping.joint_names):
+ raise ValueError("configured joint limits do not match control joints")
+ for index, width, lower, upper in zip(
+ mapping.q_indices,
+ mapping.q_widths,
+ robot.joint_limits_lower,
+ robot.joint_limits_upper,
+ strict=True,
+ ):
+ if not np.isfinite(lower) or not np.isfinite(upper) or lower >= upper:
+ raise ValueError("configured joint limits must be finite and ordered")
+ if width == 2:
+ raise ValueError(
+ "configured position limits for continuous joints require "
+ "tangent-space angular limit handling"
+ )
+ model.lowerPositionLimit[index] = lower
+ model.upperPositionLimit[index] = upper
+ if robot.velocity_limits is not None:
+ if len(robot.velocity_limits) != len(mapping.joint_names) or any(
+ not np.isfinite(value) or value <= 0.0 for value in robot.velocity_limits
+ ):
+ raise ValueError("configured velocity limits are invalid")
+ for index, limit in zip(mapping.v_indices, robot.velocity_limits, strict=True):
+ model.velocityLimit[index] = limit
+ for index in mapping.v_indices:
+ model.velocityLimit[index] = min(model.velocityLimit[index], self._config.max_velocity)
+ return [ConfigurationLimit(model), VelocityLimit(model)]
+
+
+class PinkControlIK:
+ """One-step Pink control IK assembled by :func:`create_pink_control_ik`."""
+
+ def __init__(self, runtime: _PinkRuntime) -> None:
+ self._runtime = runtime
+
+ @property
+ def nq(self) -> int:
+ """Number of controlled coordinates, matching the task contract."""
+ return len(self._runtime.mapping.joint_names)
+
+ def forward_kinematics(self, q: NDArray[np.float64]) -> pinocchio.SE3:
+ runtime = self._runtime
+ full_q = self._full_q(q)
+ pinocchio.forwardKinematics(runtime.model, runtime.data, full_q)
+ pinocchio.updateFramePlacements(runtime.model, runtime.data)
+ return runtime.data.oMf[runtime.ee_frame_id].copy()
+
+ def solve(
+ self,
+ target: pinocchio.SE3,
+ measured: NDArray[np.float64],
+ dt: float,
+ ) -> ControlIKResult:
+ runtime = self._runtime
+ measured = np.asarray(measured, dtype=np.float64).reshape(-1)
+ if measured.size != self.nq or not np.all(np.isfinite(measured)):
+ raise ValueError("measured joint state is invalid")
+ if not np.isfinite(dt) or dt <= 0.0:
+ raise ValueError("control IK dt must be finite and positive")
+
+ configuration = runtime.configuration
+ frame_task = runtime.frame_task
+ try:
+ configuration.update(self._full_q(measured))
+ frame_task.set_target(target)
+ if runtime.posture_task is not None:
+ runtime.posture_task.set_target(configuration.q.copy())
+ velocity = solve_ik(
+ configuration,
+ runtime.tasks,
+ dt,
+ solver=runtime.config.solver,
+ damping=runtime.config.lm_damping,
+ limits=runtime.limits,
+ **runtime.config.qpsolver_options,
+ )
+ velocity = np.asarray(velocity, dtype=np.float64).reshape(-1)
+ if velocity.size != runtime.model.nv or not np.all(np.isfinite(velocity)):
+ raise IKControlRuntimeError("Pink produced an invalid velocity")
+ configuration.integrate_inplace(velocity, dt)
+ candidate = self._project_controlled_positions(configuration.q, measured)
+ if candidate.size != measured.size or not np.all(np.isfinite(candidate)):
+ raise IKControlRuntimeError("Pink produced an invalid joint candidate")
+ candidate = self._clamp_position_limits(candidate)
+ return ControlIKResult(candidate, self._controlled_velocity(velocity))
+ except IKControlRuntimeError:
+ raise
+ except Exception as exc:
+ raise IKControlRuntimeError(f"Pink control solve failed: {exc}") from exc
+
+ def _full_q(self, controlled: NDArray[np.float64]) -> NDArray[np.float64]:
+ runtime = self._runtime
+ mapping = runtime.mapping
+ q = runtime.reference_q.copy()
+ for value, index, width in zip(
+ controlled, mapping.q_indices, mapping.q_widths, strict=True
+ ):
+ if width == 2:
+ q[index] = np.cos(value)
+ q[index + 1] = np.sin(value)
+ else:
+ q[index] = value
+ return q
+
+ def _project_controlled_positions(
+ self, full_q: NDArray[np.float64], reference: NDArray[np.float64] | None = None
+ ) -> NDArray[np.float64]:
+ """Project model coordinates to coordinator joints and unwrap continuous angles."""
+ mapping = self._runtime.mapping
+ positions = np.array(
+ [
+ np.arctan2(full_q[index + 1], full_q[index]) if width == 2 else full_q[index]
+ for index, width in zip(mapping.q_indices, mapping.q_widths, strict=True)
+ ],
+ dtype=np.float64,
+ )
+ if reference is not None:
+ for index, width in enumerate(mapping.q_widths):
+ if width == 2:
+ positions[index] = reference[index] + float(
+ (positions[index] - reference[index] + np.pi) % (2.0 * np.pi) - np.pi
+ )
+ return positions
+
+ def _controlled_velocity(self, velocity: NDArray[np.float64]) -> NDArray[np.float64]:
+ return np.array(
+ [velocity[index] for index in self._runtime.mapping.v_indices], dtype=np.float64
+ )
+
+ def _clamp_position_limits(self, candidate: NDArray[np.float64]) -> NDArray[np.float64]:
+ runtime = self._runtime
+ mapping = runtime.mapping
+ bounded = candidate.copy()
+ for index, width in enumerate(mapping.q_widths):
+ if width != 1:
+ continue
+ q_index = mapping.q_indices[index]
+ lower = runtime.model.lowerPositionLimit[q_index]
+ upper = runtime.model.upperPositionLimit[q_index]
+ value = bounded[index]
+ if value < lower:
+ if lower - value <= _POSITION_LIMIT_EPSILON_RAD:
+ bounded[index] = lower
+ else:
+ raise IKControlRuntimeError("Pink produced an out-of-bounds joint candidate")
+ elif value > upper:
+ if value - upper <= _POSITION_LIMIT_EPSILON_RAD:
+ bounded[index] = upper
+ else:
+ raise IKControlRuntimeError("Pink produced an out-of-bounds joint candidate")
+ return bounded
+
+
+def create_pink_control_ik(config: PinkControlIKConfig) -> PinkControlIK:
+ """Construct the default Cartesian control IK backend."""
+ return PinkControlIK(_PinkControlIKBuilder(config).build())
diff --git a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py
new file mode 100644
index 0000000000..de7adada09
--- /dev/null
+++ b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py
@@ -0,0 +1,189 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from pathlib import Path
+import subprocess
+import sys
+from typing import cast
+
+import numpy as np
+import pinocchio
+import pytest
+
+from dimos.control.coordinator import TaskConfig
+from dimos.control.task import CoordinatorState, JointStateSnapshot
+from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import (
+ CartesianIKTask,
+ CartesianIKTaskConfig,
+)
+from dimos.control.tasks.cartesian_ik_task.pink_control_ik import (
+ ControlIKResult,
+ IKControlRuntimeError,
+ PinkControlIKConfig,
+)
+from dimos.control.tasks.registry import control_task_registry
+from dimos.manipulation.planning.groups.models import PlanningGroupDefinition
+from dimos.manipulation.planning.spec.config import RobotModelConfig
+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
+
+
+def _robot(path: Path) -> RobotModelConfig:
+ return RobotModelConfig(
+ name="tiny",
+ model_path=path,
+ base_pose=PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]),
+ joint_names=["joint1"],
+ planning_groups=[
+ PlanningGroupDefinition(
+ name="manipulator",
+ joint_names=("joint1",),
+ base_link="base",
+ tip_link="tool",
+ )
+ ],
+ home_joints=[0.0],
+ )
+
+
+def _state(t_now: float, dt: float = 0.01) -> CoordinatorState:
+ return CoordinatorState(
+ joints=JointStateSnapshot(joint_positions={"joint1": 0.0}), t_now=t_now, dt=dt
+ )
+
+
+class _FakeControlIK:
+ nq = 1
+
+ def __init__(self) -> None:
+ self.target: object | None = None
+ self.dt: float | None = None
+
+ def solve(self, target: object, measured: np.ndarray, dt: float) -> ControlIKResult:
+ self.target = target
+ self.dt = dt
+ return ControlIKResult(measured.copy(), np.zeros(1))
+
+
+def test_cartesian_pipeline_passes_se3_target_and_bounded_dt(tmp_path: Path, mocker) -> None:
+ backend = _FakeControlIK()
+ mocker.patch(
+ "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik",
+ return_value=backend,
+ )
+ task = CartesianIKTask(
+ "cartesian",
+ CartesianIKTaskConfig(
+ joint_names=["joint1"],
+ control_ik=PinkControlIKConfig(robot_model=_robot(tmp_path / "unused.urdf")),
+ min_dt=0.01,
+ max_dt=0.05,
+ ),
+ )
+ assert task.on_cartesian_command(
+ PoseStamped(position=[0.2, -0.3, 0.4], orientation=[0, 0, 0, 2]), 1.0
+ )
+
+ assert task.compute(_state(1.01, dt=1.0)) is not None
+ target = cast("pinocchio.SE3", backend.target)
+ assert isinstance(target, pinocchio.SE3)
+ assert np.allclose(target.translation, [0.2, -0.3, 0.4])
+ assert np.allclose(target.rotation, np.eye(3))
+ assert backend.dt == 0.05
+
+
+def test_cartesian_pipeline_rejects_invalid_quaternion_with_hold(tmp_path: Path, mocker) -> None:
+ backend = _FakeControlIK()
+ mocker.patch(
+ "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik",
+ return_value=backend,
+ )
+ task = CartesianIKTask(
+ "cartesian",
+ CartesianIKTaskConfig(
+ joint_names=["joint1"],
+ control_ik=PinkControlIKConfig(robot_model=_robot(tmp_path / "unused.urdf")),
+ ),
+ )
+ assert task.on_cartesian_command(PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 0]), 1.0)
+
+ hold = task.compute(_state(1.01))
+ assert hold is not None
+ assert hold.positions == [0.0]
+ assert backend.target is None
+
+
+def test_factory_rejects_invalid_default_pink_configuration() -> None:
+ config = TaskConfig(
+ name="cartesian", type="cartesian_ik", joint_names=["j1"], priority=10, params={}
+ )
+ with pytest.raises(ValueError, match="control_ik"):
+ control_task_registry.create("cartesian_ik", config, hardware={})
+
+
+@pytest.mark.parametrize(
+ "module_name",
+ [
+ "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task",
+ "dimos.control.tasks.eef_twist_task.eef_twist_task",
+ ],
+)
+def test_control_task_import_fails_actionably_without_pink(module_name: str) -> None:
+ script = f"""
+import sys
+
+class BlockPink:
+ def find_spec(self, fullname, path=None, target=None):
+ if fullname == "pink":
+ raise ModuleNotFoundError("No module named 'pink'", name="pink")
+ return None
+
+sys.meta_path.insert(0, BlockPink())
+import {module_name}
+"""
+ result = subprocess.run(
+ [sys.executable, "-c", script],
+ check=False,
+ capture_output=True,
+ text=True,
+ )
+
+ assert result.returncode != 0
+ assert "Install it with `uv sync`" in result.stderr
+
+
+def test_cartesian_runtime_error_is_a_measured_state_hold(
+ tmp_path: Path, monkeypatch: pytest.MonkeyPatch, mocker
+) -> None:
+ backend = _FakeControlIK()
+
+ def fail(target: object, measured: np.ndarray, dt: float) -> ControlIKResult:
+ raise IKControlRuntimeError("solver failed")
+
+ monkeypatch.setattr(backend, "solve", fail)
+ mocker.patch(
+ "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik",
+ return_value=backend,
+ )
+ task = CartesianIKTask(
+ "cartesian",
+ CartesianIKTaskConfig(
+ joint_names=["joint1"],
+ control_ik=PinkControlIKConfig(robot_model=_robot(tmp_path / "unused.urdf")),
+ ),
+ )
+ assert task.on_cartesian_command(PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), 1.0)
+
+ hold = task.compute(_state(1.01))
+ assert hold is not None
+ assert hold.positions == [0.0]
diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py
new file mode 100644
index 0000000000..f4d976d6f7
--- /dev/null
+++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py
@@ -0,0 +1,405 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from pathlib import Path
+
+import numpy as np
+from pink import Configuration
+from pink.tasks import PostureTask
+import pytest
+
+from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import CartesianIKTaskConfig
+from dimos.control.tasks.cartesian_ik_task.pink_control_ik import (
+ IKControlRuntimeError,
+ PinkControlIKConfig,
+ create_pink_control_ik,
+)
+from dimos.manipulation.planning.groups.models import PlanningGroupDefinition
+from dimos.manipulation.planning.spec.config import RobotModelConfig
+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
+
+_URDF = """\
+
+
+
+
+
+
+
+
+
+
+
+
+
+"""
+
+_CONTINUOUS_URDF = """\
+
+
+
+
+
+
+
+"""
+
+_UNCONTROLLED_URDF = """\
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+"""
+
+
+def _robot(
+ path: Path,
+ *,
+ frame: str = "tool",
+ joints: list[str] | None = None,
+) -> RobotModelConfig:
+ joint_names = joints or ["joint1", "joint2"]
+ joint_count = len(joint_names)
+ return RobotModelConfig(
+ name="tiny",
+ model_path=path,
+ base_pose=PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]),
+ joint_names=joint_names,
+ planning_groups=[
+ PlanningGroupDefinition(
+ name="manipulator",
+ joint_names=tuple(joint_names),
+ base_link="base",
+ tip_link=frame,
+ )
+ ],
+ home_joints=[0.4] * joint_count,
+ joint_limits_lower=[-2.0] * joint_count,
+ joint_limits_upper=[2.0] * joint_count,
+ velocity_limits=[1.0] * joint_count,
+ )
+
+
+def _write_urdf(tmp_path: Path, name: str = "tiny.urdf", content: str = _URDF) -> Path:
+ path = tmp_path / name
+ path.write_text(content)
+ return path
+
+
+def test_pink_requires_robot_model() -> None:
+ with pytest.raises(ValueError, match="robot_model"):
+ PinkControlIKConfig()
+
+
+def test_pink_settings_use_finite_declarative_validation(tmp_path: Path) -> None:
+ robot = _robot(_write_urdf(tmp_path))
+
+ with pytest.raises(ValueError, match="finite"):
+ PinkControlIKConfig(robot_model=robot, max_velocity=np.inf)
+ with pytest.raises(ValueError, match="finite"):
+ PinkControlIKConfig(robot_model=robot, qpsolver_options={"eps": np.nan})
+ with pytest.raises(ValueError, match="ordered"):
+ CartesianIKTaskConfig(
+ joint_names=["joint1", "joint2"],
+ control_ik=PinkControlIKConfig(robot_model=robot),
+ min_dt=0.1,
+ max_dt=0.01,
+ )
+
+
+def test_pink_prepares_xacro_with_package_paths_and_arguments(
+ tmp_path: Path, monkeypatch: pytest.MonkeyPatch
+) -> None:
+ model_path = _write_urdf(tmp_path)
+ package_path = tmp_path / "description"
+ package_path.mkdir()
+ robot = _robot(model_path).model_copy(
+ update={
+ "model_path": tmp_path / "robot.xacro",
+ "package_paths": {"description": package_path},
+ "xacro_args": {"dof": "2"},
+ }
+ )
+ prepared: dict[str, object] = {}
+
+ def prepare(
+ path: Path,
+ package_paths: dict[str, Path],
+ xacro_args: dict[str, str],
+ convert_meshes: bool,
+ ) -> str:
+ prepared.update(
+ path=path,
+ package_paths=package_paths,
+ xacro_args=xacro_args,
+ convert_meshes=convert_meshes,
+ )
+ return str(model_path)
+
+ monkeypatch.setattr(
+ "dimos.control.tasks.cartesian_ik_task.pink_control_ik.prepare_urdf_for_drake",
+ prepare,
+ )
+
+ create_pink_control_ik(
+ PinkControlIKConfig(robot_model=robot),
+ )
+
+ assert prepared == {
+ "path": tmp_path / "robot.xacro",
+ "package_paths": {"description": package_path},
+ "xacro_args": {"dof": "2"},
+ "convert_meshes": False,
+ }
+
+
+def test_pink_validates_named_frame_and_exact_joint_mapping(tmp_path: Path) -> None:
+ model_path = _write_urdf(tmp_path)
+
+ with pytest.raises(ValueError, match="end-effector frame"):
+ create_pink_control_ik(
+ PinkControlIKConfig(robot_model=_robot(model_path, frame="missing")),
+ )
+
+ mismatched = _robot(model_path).model_copy(
+ update={"joint_name_mapping": {"joint1": "missing", "joint2": "joint2"}}
+ )
+ with pytest.raises(ValueError, match="unknown joint"):
+ create_pink_control_ik(
+ PinkControlIKConfig(robot_model=mismatched),
+ )
+
+
+def test_pink_reanchors_measured_state_and_runs_one_frame_task_step(
+ tmp_path: Path, monkeypatch: pytest.MonkeyPatch
+) -> None:
+ model_path = _write_urdf(tmp_path)
+ backend = create_pink_control_ik(
+ PinkControlIKConfig(robot_model=_robot(model_path), qpsolver_options={"eps": 1e-6}),
+ )
+ measured = np.array([0.3, 0.1])
+ target = backend.forward_kinematics(measured)
+ calls: list[tuple[Configuration, list[object], float, dict[str, object]]] = []
+
+ def solve(
+ configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+ ) -> np.ndarray:
+ calls.append((configuration, tasks, dt, kwargs))
+ return np.zeros(configuration.model.nv)
+
+ monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve)
+ result = backend.solve(target, measured, 0.01)
+
+ assert np.array_equal(result.positions, measured)
+ assert len(calls) == 1
+ configuration, tasks, dt, kwargs = calls[0]
+ assert len(tasks) == 2
+ assert isinstance(tasks[1], PostureTask)
+ assert np.array_equal(tasks[1].target_q, configuration.q)
+ assert dt == 0.01
+ assert kwargs["solver"] == "proxqp"
+ assert kwargs["damping"] == 1e-4
+ assert kwargs["eps"] == 1e-6
+ assert isinstance(kwargs["limits"], list)
+ assert len(kwargs["limits"]) == 2
+
+
+def test_pink_solver_dependency_failure_is_translated_to_runtime_error(
+ tmp_path: Path, monkeypatch: pytest.MonkeyPatch
+) -> None:
+ backend = create_pink_control_ik(PinkControlIKConfig(robot_model=_robot(_write_urdf(tmp_path))))
+
+ def solve(
+ configuration: object, tasks: list[object], dt: float, **kwargs: object
+ ) -> np.ndarray:
+ raise RuntimeError("solver dependency failed")
+
+ monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve)
+ measured = np.array([0.3, 0.1])
+ with pytest.raises(IKControlRuntimeError, match="solver dependency failed"):
+ backend.solve(backend.forward_kinematics(measured), measured, 0.01)
+
+
+def test_pink_receives_pre_bounded_dt_unchanged(
+ tmp_path: Path, monkeypatch: pytest.MonkeyPatch
+) -> None:
+ model_path = _write_urdf(tmp_path)
+ backend = create_pink_control_ik(
+ PinkControlIKConfig(robot_model=_robot(model_path)),
+ )
+ calls: list[float] = []
+
+ def solve(
+ configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+ ) -> np.ndarray:
+ calls.append(dt)
+ return np.zeros(configuration.model.nv)
+
+ monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve)
+ measured = np.array([0.3, 0.1])
+ backend.solve(backend.forward_kinematics(measured), measured, 0.05)
+
+ assert calls == [0.05]
+
+
+def test_pink_posture_task_can_be_disabled(tmp_path: Path, monkeypatch: pytest.MonkeyPatch) -> None:
+ model_path = _write_urdf(tmp_path)
+ backend = create_pink_control_ik(
+ PinkControlIKConfig(robot_model=_robot(model_path), posture_cost=0.0)
+ )
+ calls: list[list[object]] = []
+
+ def solve(
+ configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+ ) -> np.ndarray:
+ calls.append(tasks)
+ return np.zeros(configuration.model.nv)
+
+ monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve)
+ measured = np.array([0.3, 0.1])
+ backend.solve(backend.forward_kinematics(measured), measured, 0.01)
+
+ assert calls and len(calls[0]) == 1
+
+
+def test_pink_rejects_uncontrolled_end_effector_chain_without_reference(
+ tmp_path: Path,
+) -> None:
+ model_path = _write_urdf(tmp_path, "uncontrolled.urdf", _UNCONTROLLED_URDF)
+ with pytest.raises(ValueError, match="reference_q.*uncontrolled joint"):
+ create_pink_control_ik(
+ PinkControlIKConfig(robot_model=_robot(model_path)),
+ )
+
+
+def test_continuous_joint_scalar_limits_fail_with_actionable_diagnostic(
+ tmp_path: Path, monkeypatch: pytest.MonkeyPatch
+) -> None:
+ model_path = _write_urdf(tmp_path, "continuous.urdf", _CONTINUOUS_URDF)
+ robot = _robot(model_path, joints=["joint1"])
+
+ with pytest.raises(ValueError, match="continuous joints.*tangent-space"):
+ create_pink_control_ik(
+ PinkControlIKConfig(robot_model=robot),
+ )
+
+ roundtrip_robot = robot.model_copy(
+ update={"joint_limits_lower": None, "joint_limits_upper": None}
+ )
+ backend = create_pink_control_ik(
+ PinkControlIKConfig(robot_model=roundtrip_robot),
+ )
+ angle = np.array([3.0])
+
+ def solve(
+ configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+ ) -> np.ndarray:
+ return np.zeros(configuration.model.nv)
+
+ monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve)
+ result = backend.solve(backend.forward_kinematics(angle), angle, 0.01)
+
+ assert np.allclose(result.positions, angle)
+
+
+def test_pink_applies_position_velocity_limits_and_finite_output(
+ tmp_path: Path, monkeypatch: pytest.MonkeyPatch
+) -> None:
+ model_path = _write_urdf(tmp_path)
+ robot = _robot(model_path).model_copy(
+ update={"joint_limits_lower": [-0.5, -0.25], "joint_limits_upper": [0.5, 0.25]}
+ )
+ backend = create_pink_control_ik(
+ PinkControlIKConfig(robot_model=robot, max_velocity=0.2),
+ )
+ solver_inputs: dict[str, np.ndarray] = {}
+
+ def solve(
+ configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+ ) -> np.ndarray:
+ solver_inputs["lower_position"] = configuration.model.lowerPositionLimit.copy()
+ solver_inputs["velocity"] = configuration.model.velocityLimit.copy()
+ return np.zeros(configuration.model.nv)
+
+ monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve)
+ result = backend.solve(
+ backend.forward_kinematics(np.array([0.1, 0.1])), np.array([0.1, 0.1]), 0.01
+ )
+
+ assert np.array_equal(solver_inputs["lower_position"][:2], np.array([-0.5, -0.25]))
+ assert np.all(solver_inputs["velocity"][:2] <= 0.2)
+ assert result.positions.shape == (2,)
+ assert np.all(np.isfinite(result.positions))
+
+
+def test_pink_clamps_tiny_position_limit_overshoot(
+ tmp_path: Path, monkeypatch: pytest.MonkeyPatch
+) -> None:
+ model_path = _write_urdf(tmp_path)
+ robot = _robot(model_path).model_copy(
+ update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]}
+ )
+ backend = create_pink_control_ik(PinkControlIKConfig(robot_model=robot))
+ measured = np.array([1.22, 0.1])
+
+ def solve(
+ configuration: object, tasks: list[object], dt: float, **kwargs: object
+ ) -> np.ndarray:
+ return np.array([0.00013784674535, -0.2])
+
+ monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve)
+ result = backend.solve(backend.forward_kinematics(measured), measured, 0.01)
+
+ assert np.array_equal(result.positions, np.array([1.22, 0.098]))
+ assert np.array_equal(result.velocity, np.array([0.00013784674535, -0.2]))
+
+
+def test_pink_rejects_material_position_limit_violation(
+ tmp_path: Path, monkeypatch: pytest.MonkeyPatch
+) -> None:
+ model_path = _write_urdf(tmp_path)
+ robot = _robot(model_path).model_copy(
+ update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]}
+ )
+ backend = create_pink_control_ik(PinkControlIKConfig(robot_model=robot))
+ measured = np.array([1.22, 0.1])
+
+ def solve(
+ configuration: object, tasks: list[object], dt: float, **kwargs: object
+ ) -> np.ndarray:
+ return np.array([0.01, -0.2])
+
+ monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve)
+ with pytest.raises(IKControlRuntimeError, match="out-of-bounds"):
+ backend.solve(backend.forward_kinematics(measured), measured, 0.01)
+
+
+@pytest.mark.parametrize("legacy_field", ["backend", "ee_joint_id", "self_collision_enabled"])
+def test_pink_rejects_legacy_configuration_fields(tmp_path: Path, legacy_field: str) -> None:
+ model_path = _write_urdf(tmp_path)
+ with pytest.raises(ValueError, match=legacy_field):
+ PinkControlIKConfig.model_validate(
+ {"robot_model": _robot(model_path), legacy_field: "pinocchio"}
+ )
diff --git a/dimos/control/tasks/eef_twist_task/eef_twist_task.py b/dimos/control/tasks/eef_twist_task/eef_twist_task.py
index 1d272f5310..0de24cd968 100644
--- a/dimos/control/tasks/eef_twist_task/eef_twist_task.py
+++ b/dimos/control/tasks/eef_twist_task/eef_twist_task.py
@@ -12,29 +12,25 @@
# See the License for the specific language governing permissions and
# limitations under the License.
+"""Measured-state end-effector twist control."""
+
from __future__ import annotations
from dataclasses import dataclass
-from pathlib import Path
import threading
-from typing import TYPE_CHECKING, Any
+from typing import TYPE_CHECKING
import numpy as np
-from numpy.typing import NDArray
import pinocchio
+from pydantic import FiniteFloat
-from dimos.control.task import (
- BaseControlTask,
- ControlMode,
- CoordinatorState,
- JointCommandOutput,
- ResourceClaim,
-)
-from dimos.manipulation.planning.kinematics.pinocchio_ik import (
- PinocchioIK,
- check_joint_delta,
- get_worst_joint_delta,
+from dimos.control.coordinator import TaskConfig
+from dimos.control.task import CoordinatorState, JointCommandOutput, ResourceClaim
+from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import (
+ CartesianIKTask,
+ CartesianIKTaskConfig,
)
+from dimos.control.tasks.cartesian_ik_task.pink_control_ik import PinkControlIKConfig
from dimos.protocol.service.spec import BaseConfig
from dimos.utils.logging_config import setup_logger
from dimos.utils.transform_utils import twist_to_numpy
@@ -45,210 +41,169 @@
logger = setup_logger()
-_MAX_DT = 0.05
-
@dataclass
-class EEFTwistTaskConfig:
- joint_names: list[str]
- model_path: str | Path
- ee_joint_id: int
- timeout: float
- max_joint_delta_deg: float
- priority: int = 10
+class EEFTwistTaskConfig(CartesianIKTaskConfig):
+ """Configuration for measured-FK-relative EEF twist control."""
+
gripper_joint: str | None = None
gripper_open_pos: float = 0.0
gripper_closed_pos: float = 0.0
-class EEFTwistTask(BaseControlTask):
+class EEFTwistTask(CartesianIKTask):
+ """Cartesian task specialization whose target is prepared from a twist."""
+
+ _config: EEFTwistTaskConfig
+
def __init__(self, name: str, config: EEFTwistTaskConfig) -> None:
- if not config.joint_names:
- raise ValueError(f"EEFTwistTask '{name}' requires at least one joint")
- if not config.model_path:
- raise ValueError(f"EEFTwistTask '{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._ik = PinocchioIK.from_model_path(config.model_path, config.ee_joint_id)
- if self._ik.nq != len(config.joint_names):
- raise ValueError(
- f"EEFTwistTask {name}: model DOF ({self._ik.nq}) != "
- f"joint_names count ({len(config.joint_names)})"
- )
- self._lock = threading.Lock()
+ super().__init__(name, config)
+ self._twist_lock = threading.Lock()
self._latest_twist: TwistStamped | None = None
- self._last_update_time = 0.0
self._estopped = False
-
- self._hold_target: NDArray[np.floating[Any]] | None = None
- self._gripper_target: float = config.gripper_open_pos
+ self._gripper_target = config.gripper_open_pos
def claim(self) -> ResourceClaim:
- joints = self._joint_names
- if self._config.gripper_joint:
- joints = joints | frozenset([self._config.gripper_joint])
- return ResourceClaim(joints, self._config.priority, ControlMode.SERVO_POSITION)
+ claim = super().claim()
+ if self._config.gripper_joint is None:
+ return claim
+ return ResourceClaim(
+ joints=claim.joints | frozenset([self._config.gripper_joint]),
+ priority=claim.priority,
+ mode=claim.mode,
+ )
def is_active(self) -> bool:
+ with self._twist_lock:
+ has_twist = self._latest_twist is not None
+ estopped = self._estopped
with self._lock:
- return not self._estopped
+ has_gripper_hold = self._config.gripper_joint is not None and self._active
+ return not estopped and (has_twist or has_gripper_hold) and self._active
- def set_estop(self, estopped: bool) -> None:
- """Latch/clear E-STOP. On latch, drop the pending jog and hold anchor so
- clearing resumes from the current pose, not a stale target. The gripper
- target is kept so a held payload isn't released."""
- with self._lock:
- self._estopped = estopped
- if estopped:
- self._latest_twist = None
- self._hold_target = None
+ def is_tracking(self) -> bool:
+ return self.is_active()
+
+ def _uses_prepared_target(self) -> bool:
+ return True
+
+ def on_cartesian_command(self, pose: object, t_now: float) -> bool:
+ """Reject Cartesian stream commands; twist is this task's only input."""
+ logger.warning("EEFTwistTask rejects Cartesian commands", task=self.name)
+ return False
def on_ee_twist_command(self, twist: TwistStamped, t_now: float) -> bool:
values = twist_to_numpy(twist)
- if not np.all(np.isfinite(values)):
- logger.warning("EEFTwistTask rejecting non-finite twist", task=self._name)
+ if values.shape != (6,) or not np.all(np.isfinite(values)):
+ logger.warning("EEFTwistTask rejecting invalid twist", task=self.name)
return False
- with self._lock:
+ with self._twist_lock:
if self._estopped:
- # A twist in transit when E-STOP latched must not be stored, or
- # it would replay on the next tick after the latch clears.
return False
+ if np.allclose(values, 0.0):
+ self._latest_twist = None
+ cleared = True
+ else:
+ self._latest_twist = twist
+ cleared = False
+ if cleared and self._config.gripper_joint is None:
+ super().clear()
+ return True
+ with self._lock:
self._last_update_time = t_now
- # Zero twist → hold (None); non-zero → jog. The anchor persists either way.
- self._latest_twist = None if np.allclose(values, 0.0) else twist
+ self._active = True
return True
def on_gripper_command(self, msg: Bool, t_now: float) -> bool:
- if not self._config.gripper_joint:
+ if self._config.gripper_joint is None:
return False
- with self._lock:
+ with self._twist_lock:
if self._estopped:
- # Reject new grip changes during a stop; the held target (from
- # before E-STOP) is kept so a payload isn't dropped.
return False
self._gripper_target = (
self._config.gripper_closed_pos if msg.data else self._config.gripper_open_pos
)
+ with self._lock:
+ self._last_update_time = t_now
+ self._active = True
return True
- def _with_gripper(self, joint_names: list[str], positions: list[float]) -> JointCommandOutput:
- if self._config.gripper_joint:
- with self._lock:
- gripper_pos = self._gripper_target
- joint_names = [*joint_names, self._config.gripper_joint]
- positions = [*positions, gripper_pos]
+ def set_estop(self, estopped: bool) -> None:
+ with self._twist_lock:
+ self._estopped = estopped
+ if estopped:
+ self._latest_twist = None
+
+ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
+ output = super().compute(state)
+ if output is None or self._config.gripper_joint is None:
+ return output
+ with self._twist_lock:
+ gripper_target = self._gripper_target
return JointCommandOutput(
- joint_names=joint_names,
- positions=positions,
- mode=ControlMode.SERVO_POSITION,
+ joint_names=[*output.joint_names, self._config.gripper_joint],
+ positions=[*(output.positions or []), gripper_target],
+ mode=output.mode,
)
- def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
- with self._lock:
- if (
- self._latest_twist is not None
- and self._config.timeout > 0
- and state.t_now - self._last_update_time > self._config.timeout
- ):
- self._latest_twist = None
+ def _prepare_target(
+ self,
+ state: CoordinatorState,
+ q_current: np.ndarray,
+ dt: float,
+ ) -> pinocchio.SE3 | None:
+ with self._twist_lock:
twist = self._latest_twist
- anchor = self._hold_target # last commanded joint target (not live pos)
-
- q_current = self._get_current_joints(state)
- if q_current is None or not np.all(np.isfinite(q_current)):
- return None
-
- if anchor is None:
- anchor = q_current
- with self._lock:
- self._hold_target = anchor
-
if twist is None:
- return self._with_gripper(self._joint_names_list, anchor.flatten().tolist())
-
- target_pose = self._ik.forward_kinematics(anchor)
- dt = min(max(state.dt, 0.0), _MAX_DT)
- candidate = self._integrate_twist(target_pose, twist, dt)
-
- q_solution, converged, final_error = self._ik.solve(candidate, anchor)
- if not np.all(np.isfinite(q_solution)):
return None
- if not converged:
- logger.debug(
- "EEFTwistTask IK did not converge, using partial solution",
- task=self._name,
- error=final_error,
- )
- if not check_joint_delta(q_solution, anchor, self._config.max_joint_delta_deg):
- worst_idx, worst_deg = get_worst_joint_delta(q_solution, anchor)
- logger.warning(
- "EEFTwistTask rejecting solution: joint delta exceeds limit",
- task=self._name,
- joint=self._joint_names_list[worst_idx],
- delta_deg=worst_deg,
- max_delta_deg=self._config.max_joint_delta_deg,
- )
+ pose = self.forward_kinematics(q_current)
+ values = twist_to_numpy(twist)
+ pose.translation = pose.translation + values[:3] * dt
+ angular_step = values[3:] * dt
+ if np.linalg.norm(angular_step) > 0.0:
+ pose.rotation = pinocchio.exp3(angular_step) @ pose.rotation
+ if not np.all(np.isfinite(pose.translation)) or not np.all(np.isfinite(pose.rotation)):
return None
+ return pose
- # Advance the anchor so the next tick (and any hold) continues from here.
- q_solution = q_solution.flatten()
- with self._lock:
- self._hold_target = q_solution
- return self._with_gripper(self._joint_names_list, q_solution.tolist())
-
- def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:
- if joints & self._joint_names:
- logger.warning(
- "EEFTwistTask preempted", task=self._name, by_task=by_task, joints=joints
- )
- with self._lock:
- self._hold_target = None
- self._latest_twist = None
+ def stop(self) -> None:
+ with self._twist_lock:
+ self._latest_twist = None
+ super().stop()
- def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.floating[Any]] | None:
- positions = []
- for joint_name in self._joint_names_list:
- pos = state.joints.get_position(joint_name)
- if pos is None:
- return None
- positions.append(pos)
- return np.array(positions, dtype=np.float64)
+ def _on_timeout(self) -> None:
+ with self._twist_lock:
+ self._latest_twist = None
- def _integrate_twist(
- self, pose: pinocchio.SE3, twist: TwistStamped, dt: float
- ) -> pinocchio.SE3:
- candidate = pose.copy()
- values = twist_to_numpy(twist)
- candidate.translation = candidate.translation + values[:3] * dt
- angular_step = values[3:] * dt
- if np.linalg.norm(angular_step) > 0.0:
- candidate.rotation = pinocchio.exp3(angular_step) @ candidate.rotation
- return candidate
+ def clear(self) -> None:
+ with self._twist_lock:
+ self._latest_twist = None
+ super().clear()
class EEFTwistTaskParams(BaseConfig):
- model_path: str | Path
- ee_joint_id: int = 6
timeout: float = 0.3
max_joint_delta_deg: float = 15.0
+ min_dt: FiniteFloat = 1e-4
+ max_dt: FiniteFloat = 0.05
+ control_ik: PinkControlIKConfig
gripper_joint: str | None = None
gripper_open_pos: float = 0.0
gripper_closed_pos: float = 0.0
-def create_task(cfg: Any, hardware: Any) -> EEFTwistTask:
+def create_task(cfg: TaskConfig, hardware: object) -> EEFTwistTask:
params = EEFTwistTaskParams.model_validate(cfg.params)
return EEFTwistTask(
cfg.name,
EEFTwistTaskConfig(
joint_names=cfg.joint_names,
- model_path=params.model_path,
- ee_joint_id=params.ee_joint_id,
priority=cfg.priority,
timeout=params.timeout,
max_joint_delta_deg=params.max_joint_delta_deg,
+ min_dt=params.min_dt,
+ max_dt=params.max_dt,
+ control_ik=params.control_ik,
gripper_joint=params.gripper_joint,
gripper_open_pos=params.gripper_open_pos,
gripper_closed_pos=params.gripper_closed_pos,
diff --git a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py
index b11b35bc10..051b01d65d 100644
--- a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py
+++ b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py
@@ -21,7 +21,15 @@
import pytest
from dimos.control.task import ControlMode, CoordinatorState, JointStateSnapshot
+from dimos.control.tasks.cartesian_ik_task.pink_control_ik import (
+ ControlIKResult,
+ IKControlRuntimeError,
+ PinkControlIKConfig,
+)
from dimos.control.tasks.eef_twist_task.eef_twist_task import EEFTwistTask, EEFTwistTaskConfig
+from dimos.manipulation.planning.groups.models import PlanningGroupDefinition
+from dimos.manipulation.planning.spec.config import RobotModelConfig
+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped
from dimos.msgs.std_msgs.Bool import Bool
@@ -40,41 +48,66 @@ def __init__(self) -> None:
self.nq = 3
self.fk_calls: list[np.ndarray] = []
self.solve_calls: list[FakePose] = []
+ self.dt_calls: list[float] = []
self.solution = np.array([0.01, 0.02, 0.03], dtype=np.float64)
- self.converged = True
- self.final_error = 0.0
+ self.raise_runtime = False
def forward_kinematics(self, q_current: NDArray[np.float64]) -> FakePose:
self.fk_calls.append(q_current.copy())
return FakePose(q_current.copy(), np.eye(3, dtype=np.float64))
- def solve(
- self, pose: FakePose, q_current: NDArray[np.float64]
- ) -> tuple[NDArray[np.float64], bool, float]:
+ def solve(self, pose: FakePose, q_current: NDArray[np.float64], dt: float) -> ControlIKResult:
+ if self.raise_runtime:
+ raise IKControlRuntimeError("synthetic solver failure")
self.solve_calls.append(pose.copy())
- return self.solution.copy(), self.converged, self.final_error
+ self.dt_calls.append(dt)
+ return ControlIKResult(self.solution.copy(), self.solution - q_current)
@pytest.fixture
def fake_ik(mocker) -> FakeIK:
ik = FakeIK()
mocker.patch(
- "dimos.control.tasks.eef_twist_task.eef_twist_task.PinocchioIK.from_model_path",
+ "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik",
return_value=ik,
)
return ik
+def _fake_robot_model() -> RobotModelConfig:
+ local_joints = ["joint1", "joint2", "joint3"]
+ return RobotModelConfig(
+ name="fake",
+ model_path="fake.urdf",
+ base_pose=PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]),
+ joint_names=local_joints,
+ planning_groups=[
+ PlanningGroupDefinition(
+ name="manipulator",
+ joint_names=tuple(local_joints),
+ base_link="base",
+ tip_link="tool",
+ )
+ ],
+ joint_name_mapping={
+ f"arm/joint{index}": joint_name
+ for index, joint_name in enumerate(local_joints, start=1)
+ },
+ home_joints=[0.0, 0.0, 0.0],
+ )
+
+
@pytest.fixture
def task(fake_ik: FakeIK) -> EEFTwistTask:
return EEFTwistTask(
"eef",
EEFTwistTaskConfig(
joint_names=["arm/joint1", "arm/joint2", "arm/joint3"],
- model_path="fake.urdf",
- ee_joint_id=3,
+ control_ik=PinkControlIKConfig(robot_model=_fake_robot_model()),
timeout=0.3,
max_joint_delta_deg=15.0,
+ min_dt=0.02,
+ max_dt=0.03,
),
)
@@ -111,25 +144,38 @@ def test_first_nonzero_command_activates_seeds_from_fk_and_outputs_servo_positio
assert fake_ik.solve_calls[0].translation[0] > 0.0
-def test_jog_integrates_from_commanded_anchor_not_live_state(
+def test_twist_task_rejects_cartesian_commands_without_activation(task: EEFTwistTask) -> None:
+ assert not task.on_cartesian_command(object(), t_now=1.0)
+ assert not task.is_active()
+
+
+def test_ik_runtime_error_is_a_bounded_hold(task: EEFTwistTask, fake_ik: FakeIK) -> None:
+ assert task.on_ee_twist_command(_twist(), t_now=1.0)
+ fake_ik.raise_runtime = True
+ hold = task.compute(_state(1.01))
+ assert hold is not None
+ assert hold.mode == ControlMode.SERVO_POSITION
+ assert hold.positions == [0.0, 0.0, 0.0]
+
+
+def test_integration_uses_current_fk_and_coordinator_dt(
task: EEFTwistTask, fake_ik: FakeIK
) -> None:
assert task.on_ee_twist_command(_twist(1.0), t_now=1.0)
first = task.compute(_state(1.01, dt=0.01))
+ fake_ik.solution = np.array([0.51, 0.0, 0.0], dtype=np.float64)
second = task.compute(_state(1.04, positions=[0.5, 0.0, 0.0], dt=0.01))
assert first is not None
assert second is not None
+ assert fake_ik.dt_calls == [0.02, 0.02]
assert fake_ik.solve_calls[1].translation[0] > fake_ik.solve_calls[0].translation[0]
-def test_non_converged_ik_solution_is_accepted_when_joint_delta_is_safe(
+def test_control_ik_result_positions_are_used_when_shape_is_valid(
task: EEFTwistTask, fake_ik: FakeIK
) -> None:
- fake_ik.converged = False
- fake_ik.final_error = 1.0
-
assert task.on_ee_twist_command(_twist(), t_now=1.0)
output = task.compute(_state(1.01))
@@ -143,16 +189,19 @@ def test_non_finite_ik_solution_is_rejected(task: EEFTwistTask, fake_ik: FakeIK)
assert task.on_ee_twist_command(_twist(), t_now=1.0)
output = task.compute(_state(1.01))
- assert output is None
+ assert output is not None
+ assert output.mode == ControlMode.SERVO_POSITION
+ assert output.positions == [0.0, 0.0, 0.0]
-def test_non_finite_twist_is_rejected(task: EEFTwistTask) -> None:
+def test_non_finite_twist_is_rejected_without_activating_task(task: EEFTwistTask) -> None:
accepted = task.on_ee_twist_command(
TwistStamped(frame_id="eef", linear=[np.nan, 0.0, 0.0], angular=[0.0, 0.0, 0.0]),
t_now=1.0,
)
assert accepted is False
+ assert not task.is_active()
def test_missing_joint_state_skips_fk_and_ik(task: EEFTwistTask, fake_ik: FakeIK) -> None:
@@ -165,64 +214,33 @@ def test_missing_joint_state_skips_fk_and_ik(task: EEFTwistTask, fake_ik: FakeIK
assert fake_ik.solve_calls == []
-def test_joint_delta_rejection_returns_none(task: EEFTwistTask, fake_ik: FakeIK) -> None:
+def test_joint_delta_rejection_returns_a_hold(task: EEFTwistTask, fake_ik: FakeIK) -> None:
assert task.on_ee_twist_command(_twist(), t_now=1.0)
fake_ik.solution = np.array([10.0, 0.0, 0.0], dtype=np.float64)
rejected = task.compute(_state(1.01))
- assert rejected is None
-
-
-def test_active_from_spawn_holds_current_pose_when_idle(
- task: EEFTwistTask, fake_ik: FakeIK
-) -> None:
- assert task.is_active()
- held = task.compute(_state(0.5, positions=[0.1, 0.2, 0.3]))
- assert held is not None
- assert held.mode == ControlMode.SERVO_POSITION
- assert held.positions == [0.1, 0.2, 0.3]
- assert fake_ik.solve_calls == []
+ assert rejected is not None
+ assert rejected.mode == ControlMode.SERVO_POSITION
+ assert rejected.positions == [0.0, 0.0, 0.0]
-def test_zero_twist_holds_the_commanded_anchor_not_live_state(
+def test_timeout_and_zero_command_clear_then_next_nonzero_reseeds(
task: EEFTwistTask, fake_ik: FakeIK
) -> None:
assert task.on_ee_twist_command(_twist(), t_now=1.0)
assert task.compute(_state(1.01)) is not None
- assert task.on_ee_twist_command(_twist(0.0), t_now=1.5)
- assert task.is_active()
- held = task.compute(_state(1.51, positions=[0.4, 0.0, 0.0]))
- assert held is not None
- assert held.positions == fake_ik.solution.tolist()
+ assert task.compute(_state(1.5)) is None
+ assert not task.is_active()
- prev_calls = len(fake_ik.solve_calls)
+ fake_ik.solution = np.array([1.01, 0.0, 0.0], dtype=np.float64)
assert task.on_ee_twist_command(_twist(), t_now=2.0)
assert task.compute(_state(2.01, positions=[1.0, 0.0, 0.0])) is not None
- assert len(fake_ik.solve_calls) > prev_calls
-
-
-def test_stale_jog_times_out_and_holds(task: EEFTwistTask, fake_ik: FakeIK) -> None:
- assert task.on_ee_twist_command(_twist(), t_now=1.0)
- assert task.compute(_state(1.01)) is not None
- jog_calls = len(fake_ik.solve_calls)
-
- held = task.compute(_state(1.5))
- assert held is not None
- assert len(fake_ik.solve_calls) == jog_calls
- assert held.positions == fake_ik.solution.tolist()
-
-
-def test_preempt_reseeds_anchor_from_live_pose(task: EEFTwistTask, fake_ik: FakeIK) -> None:
- assert task.on_ee_twist_command(_twist(), t_now=1.0)
- assert task.compute(_state(1.01)) is not None
-
- task.on_preempted("teleop_xarm", frozenset(["arm/joint1"]))
+ assert fake_ik.solve_calls[-1].translation[0] > 1.0
- held = task.compute(_state(2.0, positions=[0.7, 0.8, 0.9]))
- assert held is not None
- assert held.positions == [0.7, 0.8, 0.9]
+ assert task.on_ee_twist_command(_twist(0.0), t_now=2.02)
+ assert not task.is_active()
@pytest.fixture
@@ -231,9 +249,8 @@ def gripper_task(fake_ik: FakeIK) -> EEFTwistTask:
"eef",
EEFTwistTaskConfig(
joint_names=["arm/joint1", "arm/joint2", "arm/joint3"],
- model_path="fake.urdf",
- ee_joint_id=3,
- timeout=0.3,
+ control_ik=PinkControlIKConfig(robot_model=_fake_robot_model()),
+ timeout=0.0,
max_joint_delta_deg=15.0,
gripper_joint="arm/gripper",
gripper_open_pos=0.85,
@@ -242,62 +259,34 @@ def gripper_task(fake_ik: FakeIK) -> EEFTwistTask:
)
-def test_claim_includes_gripper_joint(gripper_task: EEFTwistTask) -> None:
- assert "arm/gripper" in gripper_task.claim().joints
-
+def test_gripper_task_claims_and_outputs_gripper(gripper_task: EEFTwistTask) -> None:
+ gripper_task.start()
-def test_gripper_defaults_open_and_appends_to_output(gripper_task: EEFTwistTask) -> None:
output = gripper_task.compute(_state(0.5, positions=[0.1, 0.2, 0.3]))
+ assert "arm/gripper" in gripper_task.claim().joints
assert output is not None
assert output.joint_names[-1] == "arm/gripper"
assert output.positions[-1] == 0.85
-def test_gripper_command_toggles_target(gripper_task: EEFTwistTask) -> None:
+def test_gripper_command_updates_target(gripper_task: EEFTwistTask) -> None:
assert gripper_task.on_gripper_command(Bool(data=True), 0.0)
- closed = gripper_task.compute(_state(0.5, positions=[0.1, 0.2, 0.3]))
- assert closed is not None
- assert closed.positions[-1] == 0.0
-
- assert gripper_task.on_gripper_command(Bool(data=False), 0.0)
- opened = gripper_task.compute(_state(0.6, positions=[0.1, 0.2, 0.3]))
- assert opened is not None
- assert opened.positions[-1] == 0.85
+ output = gripper_task.compute(_state(0.5, positions=[0.1, 0.2, 0.3]))
-def test_gripper_command_rejected_without_gripper_joint(task: EEFTwistTask) -> None:
- assert task.on_gripper_command(Bool(data=True), 0.0) is False
-
-
-def test_estop_makes_task_inert(task: EEFTwistTask, fake_ik: FakeIK) -> None:
- assert task.on_ee_twist_command(_twist(), t_now=1.0)
- assert task.is_active()
-
- task.set_estop(True)
- assert not task.is_active()
-
- task.set_estop(False)
- held = task.compute(_state(2.0, positions=[0.1, 0.2, 0.3]))
- assert held is not None
- assert held.positions == [0.1, 0.2, 0.3]
-
+ assert output is not None
+ assert output.positions[-1] == 0.0
-def test_twist_in_transit_during_estop_is_rejected(task: EEFTwistTask, fake_ik: FakeIK) -> None:
- task.set_estop(True)
- assert task.on_ee_twist_command(_twist(), t_now=1.0) is False
- task.set_estop(False)
- # Nothing was stored, so clearing holds the live pose (no replayed jog).
- held = task.compute(_state(2.0, positions=[0.5, 0.6, 0.7]))
- assert held is not None
- assert held.positions == [0.5, 0.6, 0.7]
+def test_commands_during_estop_are_rejected(gripper_task: EEFTwistTask) -> None:
+ gripper_task.start()
+ gripper_task.set_estop(True)
+ assert not gripper_task.is_active()
+ assert not gripper_task.on_ee_twist_command(_twist(), t_now=1.0)
+ assert not gripper_task.on_gripper_command(Bool(data=True), 1.0)
-def test_gripper_command_in_transit_during_estop_is_rejected(gripper_task: EEFTwistTask) -> None:
- gripper_task.set_estop(True)
- assert gripper_task.on_gripper_command(Bool(data=True), 0.0) is False
- # Held target (default open) is untouched by the rejected close.
gripper_task.set_estop(False)
output = gripper_task.compute(_state(2.0, positions=[0.1, 0.2, 0.3]))
assert output is not None
diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py
index 99834b090f..cbd3dd9f48 100644
--- a/dimos/control/test_control.py
+++ b/dimos/control/test_control.py
@@ -291,7 +291,7 @@ def start_coordinator(tasks):
name="eef",
type="eef_twist",
joint_names=["arm/joint1"],
- params={"model_path": "fake", "ee_joint_id": 1},
+ params={"model_path": "fake"},
)
]
)
diff --git a/dimos/robot/manipulators/a1z/blueprints/teleop.py b/dimos/robot/manipulators/a1z/blueprints/teleop.py
index ca5b20a8c6..6207746b64 100644
--- a/dimos/robot/manipulators/a1z/blueprints/teleop.py
+++ b/dimos/robot/manipulators/a1z/blueprints/teleop.py
@@ -20,8 +20,6 @@
from dimos.core.coordination.blueprints import autoconnect
from dimos.manipulation.manipulation_module import ManipulationModule
from dimos.robot.manipulators.a1z.config import (
- A1Z_DOF,
- A1Z_FK_MODEL,
a1z_hardware,
make_a1z_model_config,
)
@@ -29,6 +27,7 @@
from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule
_a1z_keyboard_hw = a1z_hardware("arm")
+_a1z_model = make_a1z_model_config()
keyboard_teleop_a1z = autoconnect(
KeyboardTeleopModule.blueprint(),
@@ -37,8 +36,7 @@
tasks=[
eef_twist_task(
_a1z_keyboard_hw,
- model_path=A1Z_FK_MODEL,
- ee_joint_id=A1Z_DOF,
+ robot_model=_a1z_model,
),
TaskConfig(
name="servo_gripper",
@@ -51,7 +49,7 @@
],
),
ManipulationModule.blueprint(
- robots=[make_a1z_model_config()],
+ robots=[_a1z_model],
visualization={"backend": "viser"},
),
)
diff --git a/dimos/robot/manipulators/a750/blueprints/teleop.py b/dimos/robot/manipulators/a750/blueprints/teleop.py
index 062c869cab..90435cb267 100644
--- a/dimos/robot/manipulators/a750/blueprints/teleop.py
+++ b/dimos/robot/manipulators/a750/blueprints/teleop.py
@@ -20,7 +20,6 @@
from dimos.core.coordination.blueprints import autoconnect
from dimos.manipulation.manipulation_module import ManipulationModule
from dimos.robot.manipulators.a750.config import (
- A750_FK_MODEL,
a750_hardware,
make_a750_model_config,
)
@@ -28,6 +27,7 @@
from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule
_a750_hw = a750_hardware("arm", mock_without_address=True)
+_a750_model = make_a750_model_config()
keyboard_teleop_a750 = autoconnect(
KeyboardTeleopModule.blueprint(),
@@ -36,10 +36,15 @@
publish_joint_state=True,
joint_state_frame_id="coordinator",
hardware=[_a750_hw],
- tasks=[eef_twist_task(_a750_hw, model_path=A750_FK_MODEL, ee_joint_id=6)],
+ tasks=[
+ eef_twist_task(
+ _a750_hw,
+ robot_model=_a750_model,
+ )
+ ],
),
ManipulationModule.blueprint(
- robots=[make_a750_model_config()],
+ robots=[_a750_model],
visualization={"backend": "meshcat"},
),
)
diff --git a/dimos/robot/manipulators/common/blueprints.py b/dimos/robot/manipulators/common/blueprints.py
index 7845c39d8c..783943d9d1 100644
--- a/dimos/robot/manipulators/common/blueprints.py
+++ b/dimos/robot/manipulators/common/blueprints.py
@@ -16,7 +16,7 @@
from __future__ import annotations
-from collections.abc import Sequence
+from collections.abc import Mapping, Sequence
from pathlib import Path
from typing import Any
@@ -58,33 +58,60 @@ def trajectory_task(
)
+def _resolve_control_ik(
+ hardware: HardwareComponent,
+ robot_model: RobotModelConfig,
+ control_ik: Mapping[str, object] | None,
+) -> dict[str, object]:
+ coordinator_joints = robot_model.get_coordinator_joint_names()
+ if hardware.joints != coordinator_joints:
+ raise ValueError("hardware joints must match RobotModelConfig coordinator joints")
+ payload = dict(control_ik or {})
+ payload["robot_model"] = robot_model
+ return payload
+
+
def cartesian_ik_task(
hardware: HardwareComponent,
*,
- model_path: Path,
- ee_joint_id: int,
name: str = CARTESIAN_IK_TASK_NAME,
priority: int = 10,
+ min_dt: float = 1e-4,
+ max_dt: float = 0.05,
+ control_ik: Mapping[str, object] | None = None,
+ robot_model: RobotModelConfig,
) -> TaskConfig:
+ resolved_control_ik = _resolve_control_ik(hardware, robot_model, control_ik)
return TaskConfig(
name=name,
type="cartesian_ik",
joint_names=hardware.joints,
priority=priority,
- params={"model_path": model_path, "ee_joint_id": ee_joint_id},
+ params={
+ "control_ik": resolved_control_ik,
+ "min_dt": min_dt,
+ "max_dt": max_dt,
+ },
)
def eef_twist_task(
hardware: HardwareComponent,
*,
- model_path: Path,
- ee_joint_id: int,
name: str = EEF_TWIST_TASK_NAME,
priority: int = 10,
- params: dict[str, Any] | None = None,
+ min_dt: float = 1e-4,
+ max_dt: float = 0.05,
+ control_ik: Mapping[str, object] | None = None,
+ robot_model: RobotModelConfig,
+ params: Mapping[str, object] | None = None,
) -> TaskConfig:
- task_params: dict[str, Any] = {"model_path": model_path, "ee_joint_id": ee_joint_id}
+ resolved_control_ik = _resolve_control_ik(hardware, robot_model, control_ik)
+ task_params: dict[str, object] = {
+ "control_ik": resolved_control_ik,
+ "min_dt": min_dt,
+ "max_dt": max_dt,
+ }
if params:
task_params.update(params)
return TaskConfig(
diff --git a/dimos/robot/manipulators/openarm/blueprints/teleop.py b/dimos/robot/manipulators/openarm/blueprints/teleop.py
index 33e8b27f98..eb6baae066 100644
--- a/dimos/robot/manipulators/openarm/blueprints/teleop.py
+++ b/dimos/robot/manipulators/openarm/blueprints/teleop.py
@@ -22,22 +22,27 @@
from dimos.robot.manipulators.common.blueprints import eef_twist_task
from dimos.robot.manipulators.openarm.config import (
LEFT_CAN,
- OPENARM_V10_FK_MODEL,
openarm_single_hardware,
openarm_single_model_config,
)
from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule
_teleop_hw = openarm_single_hardware()
+_openarm_model = openarm_single_model_config()
keyboard_teleop_openarm_mock = autoconnect(
KeyboardTeleopModule.blueprint(),
ControlCoordinator.blueprint(
hardware=[_teleop_hw],
- tasks=[eef_twist_task(_teleop_hw, model_path=OPENARM_V10_FK_MODEL, ee_joint_id=7)],
+ tasks=[
+ eef_twist_task(
+ _teleop_hw,
+ robot_model=_openarm_model,
+ )
+ ],
),
ManipulationModule.blueprint(
- robots=[openarm_single_model_config()],
+ robots=[_openarm_model],
visualization={"backend": "meshcat"},
),
)
@@ -51,13 +56,12 @@
tasks=[
eef_twist_task(
_teleop_real_hw,
- model_path=OPENARM_V10_FK_MODEL,
- ee_joint_id=7,
+ robot_model=_openarm_model,
)
],
),
ManipulationModule.blueprint(
- robots=[openarm_single_model_config()],
+ robots=[_openarm_model],
visualization={"backend": "meshcat"},
),
)
diff --git a/dimos/robot/manipulators/openyam/blueprints/teleop.py b/dimos/robot/manipulators/openyam/blueprints/teleop.py
index 5c0fb521e1..16a5fdffff 100644
--- a/dimos/robot/manipulators/openyam/blueprints/teleop.py
+++ b/dimos/robot/manipulators/openyam/blueprints/teleop.py
@@ -21,14 +21,13 @@
from dimos.manipulation.manipulation_module import ManipulationModule
from dimos.robot.manipulators.common.blueprints import eef_twist_task
from dimos.robot.manipulators.openyam.config import (
- OPENYAM_DOF,
- OPENYAM_MODEL_PATH,
make_openyam_hardware,
make_openyam_model_config,
)
from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule
_openyam_keyboard_hw = make_openyam_hardware("arm")
+_openyam_model = make_openyam_model_config(name="arm")
keyboard_teleop_openyam = autoconnect(
KeyboardTeleopModule.blueprint(),
@@ -37,13 +36,12 @@
tasks=[
eef_twist_task(
_openyam_keyboard_hw,
- model_path=OPENYAM_MODEL_PATH,
- ee_joint_id=OPENYAM_DOF,
+ robot_model=_openyam_model,
)
],
),
ManipulationModule.blueprint(
- robots=[make_openyam_model_config(name="arm")],
+ robots=[_openyam_model],
visualization={"backend": "viser"},
),
)
diff --git a/dimos/robot/manipulators/piper/blueprints/teleop.py b/dimos/robot/manipulators/piper/blueprints/teleop.py
index 49b7546281..8f3e17acc8 100644
--- a/dimos/robot/manipulators/piper/blueprints/teleop.py
+++ b/dimos/robot/manipulators/piper/blueprints/teleop.py
@@ -57,7 +57,7 @@
joint_state_frame_id="coordinator",
hardware=[_piper_keyboard_hw],
tasks=[
- eef_twist_task(_piper_keyboard_hw, model_path=PIPER_FK_MODEL, ee_joint_id=6),
+ eef_twist_task(_piper_keyboard_hw, robot_model=_piper_model),
TaskConfig(
name="servo_gripper",
type="servo",
@@ -81,7 +81,7 @@
coordinator_cartesian_ik_mock = ControlCoordinator.blueprint(
hardware=[_piper_mock_cartesian_hw],
- tasks=[cartesian_ik_task(_piper_mock_cartesian_hw, model_path=PIPER_FK_MODEL, ee_joint_id=6)],
+ tasks=[cartesian_ik_task(_piper_mock_cartesian_hw, robot_model=_piper_model)],
)
_piper_teleop_hw = piper_hardware("arm", gripper_open_position=0.07, gripper_closed_position=0.0)
@@ -128,5 +128,5 @@ class _PiperTeleopCoordinator(ControlCoordinator):
coordinator_cartesian_ik_piper = ControlCoordinator.blueprint(
hardware=[_piper_cartesian_hw],
- tasks=[cartesian_ik_task(_piper_cartesian_hw, model_path=PIPER_FK_MODEL, ee_joint_id=6)],
+ tasks=[cartesian_ik_task(_piper_cartesian_hw, robot_model=_piper_model)],
)
diff --git a/dimos/robot/manipulators/xarm/blueprints/teleop.py b/dimos/robot/manipulators/xarm/blueprints/teleop.py
index 44ec1fd871..b41e615b37 100644
--- a/dimos/robot/manipulators/xarm/blueprints/teleop.py
+++ b/dimos/robot/manipulators/xarm/blueprints/teleop.py
@@ -43,6 +43,9 @@
_xarm6_hw = xarm6_hardware("arm", gripper=True, mock_without_address=True)
_xarm7_hw = xarm7_hardware("arm", gripper=True, mock_without_address=True)
+_xarm6_control_model = make_xarm6_model_config(add_gripper=False)
+_xarm7_control_model = make_xarm7_model_config(add_gripper=False)
+_xarm_eef_params = {**XARM_GRIPPER_PARAMS, "timeout": 0.0}
keyboard_teleop_xarm6 = autoconnect(
KeyboardTeleopModule.blueprint(),
@@ -54,15 +57,14 @@
tasks=[
eef_twist_task(
_xarm6_hw,
- model_path=XARM6_FK_MODEL,
- ee_joint_id=6,
- params=XARM_GRIPPER_PARAMS,
+ robot_model=_xarm6_control_model,
+ params=_xarm_eef_params,
)
],
),
ManipulationModule.blueprint(
robots=[make_xarm6_model_config(add_gripper=True)],
- visualization={"backend": "meshcat"},
+ visualization={"backend": "viser"},
),
)
@@ -76,15 +78,14 @@
tasks=[
eef_twist_task(
_xarm7_hw,
- model_path=XARM7_FK_MODEL,
- ee_joint_id=7,
- params=XARM_GRIPPER_PARAMS,
+ robot_model=_xarm7_control_model,
+ params=_xarm_eef_params,
)
],
),
ManipulationModule.blueprint(
robots=[make_xarm7_model_config(add_gripper=True)],
- visualization={"backend": "meshcat"},
+ visualization={"backend": "viser"},
),
)
@@ -172,10 +173,9 @@ class _XArm7TeleopCoordinator(ControlCoordinator):
),
eef_twist_task(
_xarm7_teleop_hw,
- model_path=XARM7_FK_MODEL,
- ee_joint_id=7,
+ robot_model=_xarm7_control_model,
priority=10,
- params=XARM_GRIPPER_PARAMS,
+ params=_xarm_eef_params,
),
],
),
@@ -197,10 +197,9 @@ class _XArm7TeleopCoordinator(ControlCoordinator):
),
eef_twist_task(
_xarm6_teleop_hw,
- model_path=XARM6_FK_MODEL,
- ee_joint_id=6,
+ robot_model=_xarm6_control_model,
priority=10,
- params=XARM_GRIPPER_PARAMS,
+ params=_xarm_eef_params,
),
],
),
diff --git a/dimos/robot/manipulators/xarm/blueprints/test_teleop.py b/dimos/robot/manipulators/xarm/blueprints/test_teleop.py
new file mode 100644
index 0000000000..65ddad5813
--- /dev/null
+++ b/dimos/robot/manipulators/xarm/blueprints/test_teleop.py
@@ -0,0 +1,36 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import pytest
+
+from dimos.core.coordination.blueprints import Blueprint
+from dimos.manipulation.manipulation_module import (
+ ManipulationModule,
+ ManipulationModuleConfig,
+)
+from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig
+from dimos.robot.manipulators.xarm.blueprints.teleop import (
+ keyboard_teleop_xarm6,
+ keyboard_teleop_xarm7,
+)
+
+
+@pytest.mark.parametrize("blueprint", [keyboard_teleop_xarm6, keyboard_teleop_xarm7])
+def test_keyboard_teleop_uses_roboplan_compatible_visualization(blueprint: Blueprint) -> None:
+ manipulation = next(atom for atom in blueprint.blueprints if atom.module is ManipulationModule)
+ config = ManipulationModuleConfig.model_validate(manipulation.kwargs)
+
+ assert config.world_backend == "roboplan"
+ assert isinstance(config.visualization, ViserVisualizationConfig)
+ assert config.visualization.requires_world_visualization is False
diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md
index 4f9df72a86..832acb49ac 100644
--- a/docs/capabilities/manipulation/adding_a_custom_arm.md
+++ b/docs/capabilities/manipulation/adding_a_custom_arm.md
@@ -578,6 +578,34 @@ target frames. `base_link` is only the robot-scoped link placed by
`base_pose`; do not use it as a substitute for planning-group chain metadata.
See [Planning Groups](/docs/capabilities/manipulation/planning_groups.md).
+### 4d. Configure Cartesian and EEF-twist control IK
+
+Cartesian and EEF-twist tasks use the direct URDF or Xacro in
+`RobotModelConfig`. Set `package_paths` and `xacro_args` when needed, name the
+end-effector link, and map coordinator joints to model joints. The task validates
+the prepared model, frame, and joint mapping at startup.
+
+Pass the same model configuration to the common helpers:
+
+```python skip
+from dimos.robot.manipulators.common.blueprints import cartesian_ik_task, eef_twist_task
+
+cartesian_task = cartesian_ik_task(
+ hardware,
+ robot_model=robot_model,
+)
+twist_task = eef_twist_task(
+ hardware,
+ robot_model=robot_model,
+)
+```
+
+Each tick starts from measured joints and applies model position and velocity
+limits. Twist targets are derived from measured forward kinematics. Invalid
+models or mappings fail at startup; invalid runtime output holds the measured
+position. Validate Cartesian and twist behavior in simulation or replay before
+hardware use.
+
## Step 5: Register Blueprints
The blueprint registry in `dimos/robot/all_blueprints.py` is **auto-generated** by scanning the codebase for blueprint declarations. After adding your blueprints:
diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md
index c31428d013..8319245371 100644
--- a/docs/capabilities/manipulation/index.md
+++ b/docs/capabilities/manipulation/index.md
@@ -151,6 +151,32 @@ RoboPlan plans all target groups simultaneously. The Viser panel constructs a
two-waypoint absolute path for interactive planning. There is no skill, MCP
tool, or CLI motion command yet.
+### Cartesian control IK
+
+Cartesian and keyboard EEF-twist tasks use the direct URDF/Xacro model from
+`RobotModelConfig`. The configuration supplies package paths, Xacro arguments,
+the named end-effector frame, and coordinator-to-model joint mapping. Invalid
+models, frames, or mappings fail at startup.
+
+Each control tick starts from measured joints, applies model position and
+velocity limits, and holds the measured position when a solve cannot produce a
+safe command. This local control path is separate from manipulation planning and
+does not use `WorldSpec` or provide world-obstacle avoidance.
+
+For a custom robot, pass the typed model configuration to the helper:
+
+```python skip
+from dimos.robot.manipulators.common.blueprints import cartesian_ik_task
+
+task = cartesian_ik_task(
+ hardware,
+ robot_model=robot_model,
+)
+```
+
+Validate Cartesian and twist behavior in simulation or replay before hardware
+use.
+
Install the manipulation dependencies:
```bash
@@ -271,7 +297,7 @@ KeyboardTeleopModule ──→ ControlCoordinator ──→ ManipulationModule
(pygame UI) (100Hz tick loop) (WorldSpec backend)
│ │ │
TwistStamped EEFTwistTask RRT planner
- spatial EEF twist (Pinocchio FK/IK) JacobianIK
+ spatial EEF twist (control IK) JacobianIK
│ DrakeWorld
JointState ────────────→ (visualization)
```
diff --git a/pyproject.toml b/pyproject.toml
index 6441ea3202..37bc540ae5 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -108,6 +108,8 @@ dependencies = [
"numpy>=1.26.4",
"scipy>=1.15.1",
"pin>=3.3.0", # Pinocchio IK library
+ "pin-pink>=4.3.0",
+ "qpsolvers[proxqp]>=4.12.0",
"cmeel-tinyxml2>=11,<12", # Pinocchio 4.1 requires the v11 ABI.
"reactivex",
"sortedcontainers==2.4.0",
@@ -271,8 +273,6 @@ manipulation = [
# Planning (Drake)
"drake==1.45.0; sys_platform == 'darwin' and platform_machine != 'aarch64'",
"drake>=1.40.0; sys_platform != 'darwin' and platform_machine != 'aarch64'",
- "pin-pink>=4.2.0",
- "qpsolvers[proxqp]>=4.12.0",
# Hardware SDKs
"piper-sdk",
@@ -591,6 +591,8 @@ module = [
"nav_msgs.*",
"open_clip",
"pinocchio",
+ "pink",
+ "pink.*",
"piper_sdk.*",
"plotext",
"plum.*",
diff --git a/uv.lock b/uv.lock
index 799831a286..2292f5837e 100644
--- a/uv.lock
+++ b/uv.lock
@@ -1561,6 +1561,7 @@ dependencies = [
{ name = "opencv-contrib-python" },
{ name = "packaging" },
{ name = "pin" },
+ { name = "pin-pink" },
{ name = "plotext" },
{ name = "plum-dispatch" },
{ name = "protobuf" },
@@ -1569,6 +1570,7 @@ dependencies = [
{ name = "pydantic-settings" },
{ name = "python-dotenv" },
{ name = "pyturbojpeg" },
+ { name = "qpsolvers", extra = ["proxqp"] },
{ name = "reactivex" },
{ name = "rerun-sdk" },
{ name = "scipy", version = "1.15.3", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" },
@@ -1636,7 +1638,6 @@ all = [
{ name = "open-clip-torch" },
{ name = "openai" },
{ name = "pillow" },
- { name = "pin-pink" },
{ name = "piper-sdk" },
{ name = "playground" },
{ name = "portal" },
@@ -1646,7 +1647,6 @@ all = [
{ name = "pyrealsense2-extended", marker = "sys_platform != 'darwin'" },
{ name = "python-multipart" },
{ name = "pyyaml" },
- { name = "qpsolvers", extra = ["proxqp"] },
{ name = "reportlab" },
{ name = "rerun-sdk" },
{ name = "roboplan" },
@@ -1726,12 +1726,10 @@ manipulation = [
{ name = "drake", version = "1.45.0", source = { registry = "https://pypi.org/simple" }, marker = "platform_machine != 'aarch64' and sys_platform == 'darwin'" },
{ name = "drake", version = "1.49.0", source = { registry = "https://pypi.org/simple" }, marker = "platform_machine != 'aarch64' and sys_platform != 'darwin'" },
{ name = "matplotlib" },
- { name = "pin-pink" },
{ name = "piper-sdk" },
{ name = "pycollada" },
{ name = "pyrealsense2-extended", marker = "sys_platform != 'darwin'" },
{ name = "pyyaml" },
- { name = "qpsolvers", extra = ["proxqp"] },
{ name = "roboplan" },
{ name = "trimesh" },
{ name = "viser", extra = ["urdf"] },
@@ -2098,7 +2096,7 @@ requires-dist = [
{ name = "pandas", marker = "extra == 'learning'" },
{ name = "pillow", marker = "extra == 'perception'" },
{ name = "pin", specifier = ">=3.3.0" },
- { name = "pin-pink", marker = "extra == 'manipulation'", specifier = ">=4.2.0" },
+ { name = "pin-pink", specifier = ">=4.3.0" },
{ name = "piper-sdk", marker = "extra == 'manipulation'" },
{ name = "playground", marker = "extra == 'sim'", specifier = ">=0.0.5" },
{ name = "plotext", specifier = "==5.3.2" },
@@ -2117,7 +2115,7 @@ requires-dist = [
{ name = "python-multipart", marker = "extra == 'misc'", specifier = ">=0.0.27" },
{ name = "pyturbojpeg", specifier = "==1.8.2" },
{ name = "pyyaml", marker = "extra == 'manipulation'", specifier = ">=6.0" },
- { name = "qpsolvers", extras = ["proxqp"], marker = "extra == 'manipulation'", specifier = ">=4.12.0" },
+ { name = "qpsolvers", extras = ["proxqp"], specifier = ">=4.12.0" },
{ name = "reactivex" },
{ name = "reportlab", marker = "extra == 'apriltag'", specifier = ">=4.5.0" },
{ name = "rerun-sdk", specifier = "==0.32.0" },
@@ -6246,7 +6244,7 @@ wheels = [
[[package]]
name = "pin-pink"
-version = "4.2.0"
+version = "4.3.0"
source = { registry = "https://pypi.org/simple" }
dependencies = [
{ name = "loop-rate-limiters" },
@@ -6254,10 +6252,11 @@ dependencies = [
{ name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" },
{ name = "pin" },
{ name = "qpsolvers" },
+ { name = "typing-extensions" },
]
-sdist = { url = "https://files.pythonhosted.org/packages/1a/aa/52c817dea0240f41c6b2b0e0872561a63942c1cc5f26fe367f7da60fe183/pin_pink-4.2.0.tar.gz", hash = "sha256:21ffbb4624377d74036c4c4d1d9ba252a0c912cd7a2469117e768f36179c17e1", size = 284729, upload-time = "2026-04-20T09:47:38.34Z" }
+sdist = { url = "https://files.pythonhosted.org/packages/7f/5b/0bfab6a426c051215753995c65f0e2859e038e49e75510b64ce60a9712c1/pin_pink-4.3.0.tar.gz", hash = "sha256:65964e4a2e125d9f5f927f37ee8b85dcf57e125b4845fe6be034fa64c6ed3379", size = 52854, upload-time = "2026-07-15T18:58:20.819Z" }
wheels = [
- { url = "https://files.pythonhosted.org/packages/d9/de/9c1f8e4fe703ac917fa0bb161a928704b31c5fafce90a055dcabd5674ae4/pin_pink-4.2.0-py3-none-any.whl", hash = "sha256:8c405607eb94c92540a7b28147b5d638ba1e5dbfda8ef307e3e51572e3dd4477", size = 64871, upload-time = "2026-04-20T09:47:34.899Z" },
+ { url = "https://files.pythonhosted.org/packages/01/dc/863a1cbc36fcb269ebdaa0e42c4e31f1c9d28f6aa0bdbacbd923cf6b16cc/pin_pink-4.3.0-py3-none-any.whl", hash = "sha256:6a38c07e0f01a754f827166242acb2b9f0b03e726712a078d2e243fdbdbf6f6a", size = 64305, upload-time = "2026-07-15T18:58:19.173Z" },
]
[[package]]