From 4faee98be654a6ef620b09b6edbe2abe466fee78 Mon Sep 17 00:00:00 2001
From: cc <cc7@duck.com>
Date: Mon, 27 Jul 2026 21:28:58 -0700
Subject: [PATCH 1/8] feat: use Pink IK for teleop tasks

---
 .../control/tasks/teleop_task/teleop_task.py  | 389 ++++++------------
 .../tasks/teleop_task/test_teleop_task.py     | 298 ++++++++++++++
 dimos/robot/manipulators/common/blueprints.py |  13 +-
 dimos/robot/manipulators/common/mixed.py      |  29 +-
 .../manipulators/piper/blueprints/teleop.py   |   4 +-
 dimos/robot/manipulators/piper/config.py      |   1 -
 .../manipulators/xarm/blueprints/teleop.py    |   8 +-
 dimos/robot/manipulators/xarm/config.py       |   2 -
 ...01-use-pink-control-ik-for-teleop-tasks.md |   3 +
 .../manipulation/adding_a_custom_arm.md       |  32 +-
 docs/capabilities/manipulation/index.md       |  27 +-
 .../.openspec.yaml                            |   2 +
 .../use-pink-control-ik-for-teleop/design.md  |  89 ++++
 .../proposal.md                               |  28 ++
 .../specs/teleop-ik-control/spec.md           | 110 +++++
 .../use-pink-control-ik-for-teleop/tasks.md   |  36 ++
 16 files changed, 764 insertions(+), 307 deletions(-)
 create mode 100644 dimos/control/tasks/teleop_task/test_teleop_task.py
 create mode 100644 docs/adr/0001-use-pink-control-ik-for-teleop-tasks.md
 create mode 100644 openspec/changes/use-pink-control-ik-for-teleop/.openspec.yaml
 create mode 100644 openspec/changes/use-pink-control-ik-for-teleop/design.md
 create mode 100644 openspec/changes/use-pink-control-ik-for-teleop/proposal.md
 create mode 100644 openspec/changes/use-pink-control-ik-for-teleop/specs/teleop-ik-control/spec.md
 create mode 100644 openspec/changes/use-pink-control-ik-for-teleop/tasks.md

diff --git a/dimos/control/tasks/teleop_task/teleop_task.py b/dimos/control/tasks/teleop_task/teleop_task.py
index d7ba0709f5..c34422fb57 100644
--- a/dimos/control/tasks/teleop_task/teleop_task.py
+++ b/dimos/control/tasks/teleop_task/teleop_task.py
@@ -12,37 +12,24 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-"""Teleop cartesian control task with internal Pinocchio IK solver.
-
-Accepts streaming cartesian delta poses from teleoperation and computes
-inverse kinematics internally to output joint commands. Deltas are applied
-relative to the EE pose captured at engage time.
-
-Participates in joint-level arbitration.
-"""
+"""Engagement-relative teleop control through measured-state Pink IK."""
 
 from __future__ import annotations
 
 from dataclasses import dataclass
-from pathlib import Path
-import threading
-from typing import TYPE_CHECKING, Any, Literal
+from typing import TYPE_CHECKING, Literal
 
 import numpy as np
 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,
-    pose_to_se3,
+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
 
@@ -57,336 +44,214 @@
 
 
 @dataclass
-class TeleopIKTaskConfig:
-    """Configuration for teleop IK task.
-
-    Attributes:
-        joint_names: List of joint names this task controls (must match model DOF)
-        model_path: Path to URDF or MJCF file for IK solver
-        ee_joint_id: End-effector joint ID in the kinematic chain
-        priority: Priority for arbitration (higher wins)
-        timeout: If no command received for this many seconds, go inactive (0 = never)
-        max_joint_delta_deg: Maximum allowed joint change per tick (safety limit)
-        hand: "left" or "right" — which controller's primary button to listen to
-        gripper_joint: Optional joint name for the gripper (e.g. "arm/gripper").
-        gripper_open_pos: Gripper position (adapter units) at trigger value 0.0 (no press).
-        gripper_closed_pos: Gripper position (adapter units) at trigger value 1.0 (full press).
-    """
-
-    joint_names: list[str]
-    model_path: str | Path
-    ee_joint_id: int
-    priority: int = 10
-    timeout: float = 0.5
-    max_joint_delta_deg: float = 5.0  # ~500°/s at 100Hz
+class TeleopIKTaskConfig(CartesianIKTaskConfig):
+    """Configuration for engagement-relative teleop IK."""
+
+    max_joint_delta_deg: float = 5.0
     hand: Literal["left", "right"] | None = None
     gripper_joint: str | None = None
     gripper_open_pos: float = 0.0
     gripper_closed_pos: float = 0.0
 
 
-class TeleopIKTask(BaseControlTask):
-    """Teleop cartesian control task with internal Pinocchio IK solver.
-
-    Accepts streaming cartesian delta poses via on_cartesian_command() and computes IK
-    internally to output joint commands. Deltas are applied relative to the EE pose
-    captured at engage time (first compute).
-
-    Uses current joint state from CoordinatorState as IK warm-start for fast convergence.
-    Outputs JointCommandOutput and participates in joint-level arbitration.
-
-    Example:
-        >>> from dimos.utils.data import get_data
-        >>> piper_path = get_data("piper_description")
-        >>> task = TeleopIKTask(
-        ...     name="teleop_arm",
-        ...     config=TeleopIKTaskConfig(
-        ...         joint_names=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"],
-        ...         model_path=piper_path / "mujoco_model" / "piper_no_gripper_description.xml",
-        ...         ee_joint_id=6,
-        ...         priority=10,
-        ...         timeout=0.5,
-        ...         hand="right",
-        ...     ),
-        ... )
-        >>> coordinator.add_task(task)
-        >>> task.start()
-        >>>
-        >>> # From teleop callback:
-        >>> task.on_cartesian_command(delta_pose, t_now=time.perf_counter())
-    """
+class TeleopIKTask(CartesianIKTask):
+    """Cartesian IK specialization for engagement-relative teleoperation."""
+
+    _config: TeleopIKTaskConfig
 
     def __init__(self, name: str, config: TeleopIKTaskConfig) -> None:
-        """Initialize teleop IK task.
-
-        Args:
-            name: Unique task name
-            config: Task configuration
-        """
-        if not config.joint_names:
-            raise ValueError(f"TeleopIKTask '{name}' requires at least one joint")
-        if not config.model_path:
-            raise ValueError(f"TeleopIKTask '{name}' requires model_path for IK solver")
         if config.hand not in ("left", "right"):
             raise ValueError(f"TeleopIKTask '{name}' requires hand='left' or 'right'")
-
-        self._name = name
-        self._config = config
-        self._joint_names = frozenset(config.joint_names)
-        self._joint_names_list = list(config.joint_names)
-        self._num_joints = len(config.joint_names)
-
-        # Create IK solver from model
-        self._ik = PinocchioIK.from_model_path(config.model_path, config.ee_joint_id)
-
-        # Validate DOF matches joint names
-        if self._ik.nq != self._num_joints:
-            logger.warning(
-                f"TeleopIKTask {name}: model DOF ({self._ik.nq}) != "
-                f"joint_names count ({self._num_joints})"
-            )
-
-        # Thread-safe target state
-        self._lock = threading.Lock()
-        self._target_pose: Pose | PoseStamped | None = None
-        self._last_update_time: float = 0.0
-        self._active = False
-        self._estopped = False
-
-        # Initial EE pose for delta application
+        super().__init__(name, config)
         self._initial_ee_pose: pinocchio.SE3 | None = None
-        self._prev_primary: bool = False
-
-        self._gripper_target: float = config.gripper_open_pos
-
-        logger.info(
-            f"TeleopIKTask {name} initialized with model: {config.model_path}, "
-            f"ee_joint_id={config.ee_joint_id}, joints={config.joint_names}"
-        )
+        self._prev_primary = False
+        self._estopped = False
+        self._gripper_target = config.gripper_open_pos
 
     def claim(self) -> ResourceClaim:
-        """Declare resource requirements."""
-        joints = self._joint_names
-        if self._config.gripper_joint:
-            joints = joints | frozenset([self._config.gripper_joint])
+        """Claim arm joints and the optional gripper joint."""
+        claim = super().claim()
+        if self._config.gripper_joint is None:
+            return claim
         return ResourceClaim(
-            joints=joints,
-            priority=self._config.priority,
-            mode=ControlMode.SERVO_POSITION,
+            joints=claim.joints | frozenset([self._config.gripper_joint]),
+            priority=claim.priority,
+            mode=claim.mode,
         )
 
     def is_active(self) -> bool:
-        """Check if task should run this tick."""
+        """Run only when a non-E-STOPped pose target is active."""
         with self._lock:
             return not self._estopped and self._active and self._target_pose is not None
 
+    def is_tracking(self) -> bool:
+        """Report whether teleop currently participates in control."""
+        return self.is_active()
+
     def set_estop(self, estopped: bool) -> None:
-        """Latch/clear E-STOP. On latch, disengage and drop the target so the
-        task goes inert (is_active() False → compute() is skipped)."""
+        """Latch or clear E-STOP without retaining replayable commands."""
         with self._lock:
             self._estopped = estopped
             if estopped:
                 self._active = False
                 self._target_pose = None
                 self._initial_ee_pose = None
+                self._prev_primary = False
+
+    def _prepare_target(
+        self,
+        state: CoordinatorState,
+        q_current: NDArray[np.float64],
+        dt: float,
+    ) -> pinocchio.SE3 | None:
+        """Compose the controller delta with the measured engagement baseline."""
+        delta = super()._prepare_target(state, q_current, dt)
+        if delta is None:
+            return None
 
-    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:
+            if self._estopped or self._target_pose is None:
                 return None
+            baseline = self._initial_ee_pose
 
-            # Timeout safety: stop if teleop stream drops
-            if self._config.timeout > 0:
-                time_since_update = state.t_now - self._last_update_time
-                if time_since_update > self._config.timeout:
-                    logger.warning(
-                        f"TeleopIKTask {self._name} timed out "
-                        f"(no update for {time_since_update:.3f}s)"
-                    )
-                    self._target_pose = None
-                    self._active = False
-                    return None
-            raw_pose = self._target_pose
-
-        # Convert to SE3 right before use
-        delta_se3 = pose_to_se3(raw_pose)
-        # Capture initial EE pose if not set (first command after engage)
-        with self._lock:
-            need_capture = self._initial_ee_pose is None
-
-        if need_capture:
-            q_current = self._get_current_joints(state)
-            if q_current is None:
-                logger.debug(
-                    f"TeleopIKTask {self._name}: cannot capture initial pose, joint state unavailable"
-                )
+        if baseline is None:
+            captured = self.forward_kinematics(q_current)
+            values = np.concatenate((captured.translation, captured.rotation.reshape(-1)))
+            if not np.all(np.isfinite(values)):
                 return None
-            initial_pose = self._ik.forward_kinematics(q_current)
             with self._lock:
-                self._initial_ee_pose = initial_pose
-
-        # Apply delta to initial pose: target = initial + delta
-        with self._lock:
-            if self._initial_ee_pose is None:
-                return None
-            target_pose = pinocchio.SE3(
-                delta_se3.rotation @ self._initial_ee_pose.rotation,
-                self._initial_ee_pose.translation + delta_se3.translation,
-            )
-
-        # Get current joint positions for IK warm-start
-        q_current = self._get_current_joints(state)
-        if q_current is None:
-            logger.debug(f"TeleopIKTask {self._name}: missing joint state for IK warm-start")
-            return None
+                if self._estopped or self._target_pose is None:
+                    return None
+                if self._initial_ee_pose is None:
+                    self._initial_ee_pose = captured.copy()
+                baseline = self._initial_ee_pose
 
-        # Compute IK
-        q_solution, converged, final_error = self._ik.solve(target_pose, q_current)
-        # Use the solution even if it didn't fully converge
-        if not converged:
-            logger.debug(
-                f"TeleopIKTask {self._name}: IK did not converge "
-                f"(error={final_error:.4f}), using partial solution"
-            )
-        # Safety: reject if any joint would jump too far in one tick
-        if not check_joint_delta(q_solution, q_current, self._config.max_joint_delta_deg):
-            logger.warning(
-                f"TeleopIKTask {self._name}: joint delta exceeds "
-                f"{self._config.max_joint_delta_deg}°, rejecting solution"
-            )
+        target = pinocchio.SE3(
+            delta.rotation @ baseline.rotation,
+            baseline.translation + delta.translation,
+        )
+        values = np.concatenate((target.translation, target.rotation.reshape(-1)))
+        if not np.all(np.isfinite(values)):
             return None
+        return target
 
-        joint_names = list(self._joint_names_list)
-        positions = q_solution.flatten().tolist()
-
-        # Append gripper joint if configured — routed to ConnectedHardware by tick loop
-        if self._config.gripper_joint:
-            with self._lock:
-                gripper_pos = self._gripper_target
-            joint_names.append(self._config.gripper_joint)
-            positions.append(gripper_pos)
-
+    def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
+        """Run the inherited Pink solve and append the optional gripper target."""
+        output = super().compute(state)
+        if output is None or self._config.gripper_joint is None:
+            return output
+        with self._lock:
+            if self._estopped:
+                return None
+            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 _get_current_joints(self, state: CoordinatorState) -> NDArray[np.floating[Any]] | None:
-        """Get current joint positions from coordinator state."""
-        positions = []
-        for joint_name in self._joint_names_list:
-            pos = state.joints.get_position(joint_name)
-            if pos is None:
-                return None
-            positions.append(pos)
-        return np.array(positions)
-
-    def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:
-        """Handle preemption by higher-priority task.
-
-        Args:
-            by_task: Name of preempting task
-            joints: Joints that were preempted
-        """
-        if joints & self._joint_names:
-            logger.warning(f"TeleopIKTask {self._name} preempted by {by_task} on joints {joints}")
-
     def on_buttons(self, msg: Buttons) -> bool:
-        """Press-and-hold engage: hold primary button to track, release to stop."""
+        """Use the configured primary button as press-and-hold engagement."""
         is_left = self._config.hand == "left"
         primary = msg.left_primary if is_left else msg.right_primary
+        trigger = msg.left_trigger_analog if is_left else msg.right_trigger_analog
 
-        if primary and not self._prev_primary:
-            logger.info(f"TeleopIKTask {self._name}: engage")
-            with self._lock:
+        with self._lock:
+            if self._estopped:
+                return False
+            if primary and not self._prev_primary:
                 self._initial_ee_pose = None
-        elif not primary and self._prev_primary:
-            logger.info(f"TeleopIKTask {self._name}: disengage")
-            with self._lock:
+            elif not primary and self._prev_primary:
+                self._active = False
                 self._target_pose = None
                 self._initial_ee_pose = None
-        self._prev_primary = primary
+            self._prev_primary = primary
 
-        if self._config.gripper_joint:
-            trigger = msg.left_trigger_analog if is_left else msg.right_trigger_analog
+        if self._config.gripper_joint is not None:
             self.on_gripper_trigger(trigger)
-
         return True
 
     def on_teleop_buttons(self, msg: Buttons, t_now: float) -> bool:
-        """Uniform stream handler; ``on_buttons`` predates the (msg, t_now) contract."""
+        """Uniform stream handler for broadcast controller buttons."""
         return self.on_buttons(msg)
 
     def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool:
-        """Handle incoming cartesian command (delta pose from teleop)"""
+        """Accept an engagement-relative pose delta unless E-STOP is latched."""
         with self._lock:
-            self._target_pose = pose  # Store raw, convert to SE3 in compute()
+            if self._estopped:
+                return False
+            self._target_pose = pose
             self._last_update_time = t_now
             self._active = True
-
         return True
 
     def on_gripper_trigger(self, value: float, _t_now: float = 0.0) -> bool:
-        """Map analog trigger (0-1) to gripper position"""
-        if not self._config.gripper_joint:
+        """Map an analog trigger value onto the configured gripper range."""
+        if self._config.gripper_joint is None or not np.isfinite(value):
             return False
-
         clamped = max(0.0, min(1.0, value))
-        pos = (
+        position = (
             self._config.gripper_open_pos
             + (self._config.gripper_closed_pos - self._config.gripper_open_pos) * clamped
         )
-
         with self._lock:
-            self._gripper_target = pos
-
+            if self._estopped:
+                return False
+            self._gripper_target = position
         return True
 
-    def start(self) -> None:
-        """Activate the task (start accepting and outputting commands)."""
-        with self._lock:
-            self._active = True
-        logger.info(f"TeleopIKTask {self._name} started")
+    def _on_timeout(self) -> None:
+        """Discard the baseline while the parent holds the task lock."""
+        self._initial_ee_pose = None
+        self._prev_primary = False
 
     def stop(self) -> None:
-        """Deactivate the task (stop outputting commands)."""
+        """Stop output and discard engagement-relative state."""
+        super().stop()
         with self._lock:
             self._active = False
-        logger.info(f"TeleopIKTask {self._name} stopped")
+            self._target_pose = None
+            self._initial_ee_pose = None
+            self._prev_primary = False
+
+    def clear(self) -> None:
+        """Clear output and discard engagement-relative state."""
+        super().clear()
+        with self._lock:
+            self._active = False
+            self._target_pose = None
+            self._initial_ee_pose = None
+            self._prev_primary = False
 
 
 class TeleopIKTaskParams(BaseConfig):
-    model_path: str | Path
-    ee_joint_id: int = 6
+    control_ik: PinkControlIKConfig
     hand: Literal["left", "right"] | None = None
+    timeout: float = 0.5
+    max_joint_delta_deg: float = 5.0
+    min_dt: FiniteFloat = 1e-4
+    max_dt: FiniteFloat = 0.05
     gripper_joint: str | None = None
     gripper_open_pos: float = 0.0
     gripper_closed_pos: float = 0.0
-    max_joint_delta_deg: float = TeleopIKTaskConfig.max_joint_delta_deg
 
 
-def create_task(cfg: Any, hardware: Any) -> TeleopIKTask:
+def create_task(cfg: TaskConfig, hardware: object) -> TeleopIKTask:
+    """Create a Pink-backed teleop task from declarative configuration."""
     params = TeleopIKTaskParams.model_validate(cfg.params)
     return TeleopIKTask(
         cfg.name,
         TeleopIKTaskConfig(
             joint_names=cfg.joint_names,
-            model_path=params.model_path,
-            ee_joint_id=params.ee_joint_id,
+            control_ik=params.control_ik,
             priority=cfg.priority,
+            timeout=params.timeout,
+            max_joint_delta_deg=params.max_joint_delta_deg,
+            min_dt=params.min_dt,
+            max_dt=params.max_dt,
             hand=params.hand,
             gripper_joint=params.gripper_joint,
             gripper_open_pos=params.gripper_open_pos,
             gripper_closed_pos=params.gripper_closed_pos,
-            max_joint_delta_deg=params.max_joint_delta_deg,
         ),
     )
diff --git a/dimos/control/tasks/teleop_task/test_teleop_task.py b/dimos/control/tasks/teleop_task/test_teleop_task.py
new file mode 100644
index 0000000000..ca7110f4e2
--- /dev/null
+++ b/dimos/control/tasks/teleop_task/test_teleop_task.py
@@ -0,0 +1,298 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+
+import numpy as np
+from numpy.typing import NDArray
+import pinocchio
+import pytest
+from pytest_mock import MockerFixture
+
+from dimos.control.coordinator import TaskConfig
+from dimos.control.task import ControlMode, CoordinatorState, JointStateSnapshot
+from dimos.control.tasks.cartesian_ik_task.pink_control_ik import (
+    ControlIKResult,
+    IKControlRuntimeError,
+    PinkControlIKConfig,
+)
+from dimos.control.tasks.teleop_task.teleop_task import (
+    TeleopIKTask,
+    TeleopIKTaskConfig,
+    create_task,
+)
+from dimos.manipulation.planning.groups.models import PlanningGroupDefinition
+from dimos.manipulation.planning.spec.config import RobotModelConfig
+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
+from dimos.teleop.quest.quest_types import Buttons
+
+
+@dataclass
+class _FakePinkIK:
+    nq: int = 2
+
+    def __post_init__(self) -> None:
+        self.fk_calls: list[NDArray[np.float64]] = []
+        self.solve_calls: list[tuple[pinocchio.SE3, NDArray[np.float64], float]] = []
+        self.solution = np.array([0.01, 0.02], dtype=np.float64)
+        self.raise_runtime = False
+
+    def forward_kinematics(self, q: NDArray[np.float64]) -> pinocchio.SE3:
+        self.fk_calls.append(q.copy())
+        return pinocchio.SE3(
+            pinocchio.exp3(np.array([0.0, 0.0, 0.2], dtype=np.float64)),
+            np.array([q[0], q[1], 0.3], dtype=np.float64),
+        )
+
+    def solve(
+        self,
+        target: pinocchio.SE3,
+        measured: NDArray[np.float64],
+        dt: float,
+    ) -> ControlIKResult:
+        if self.raise_runtime:
+            raise IKControlRuntimeError("synthetic Pink failure")
+        self.solve_calls.append((target.copy(), measured.copy(), dt))
+        return ControlIKResult(self.solution.copy(), self.solution - measured)
+
+
+def _robot(path: Path) -> RobotModelConfig:
+    return RobotModelConfig(
+        name="arm",
+        model_path=path,
+        base_pose=PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]),
+        joint_names=["joint1", "joint2"],
+        planning_groups=[
+            PlanningGroupDefinition(
+                name="manipulator",
+                joint_names=("joint1", "joint2"),
+                base_link="base",
+                tip_link="tool",
+            )
+        ],
+        joint_name_mapping={"arm/joint1": "joint1", "arm/joint2": "joint2"},
+        home_joints=[0.0, 0.0],
+    )
+
+
+def _pink_config(path: Path) -> PinkControlIKConfig:
+    return PinkControlIKConfig.model_validate({"robot_model": _robot(path)})
+
+
+def _state(
+    t_now: float,
+    positions: tuple[float, ...] = (0.0, 0.0),
+    *,
+    dt: float = 0.01,
+) -> CoordinatorState:
+    return CoordinatorState(
+        joints=JointStateSnapshot(
+            joint_positions={
+                f"arm/joint{index + 1}": position for index, position in enumerate(positions)
+            }
+        ),
+        t_now=t_now,
+        dt=dt,
+    )
+
+
+def _delta(
+    position: tuple[float, float, float] = (0.1, -0.2, 0.4),
+    angle: float = 0.3,
+) -> PoseStamped:
+    quaternion = pinocchio.Quaternion(pinocchio.exp3(np.array([0.0, 0.0, angle])))
+    return PoseStamped(
+        position=list(position),
+        orientation=[quaternion.x, quaternion.y, quaternion.z, quaternion.w],
+    )
+
+
+@pytest.fixture
+def fake_ik(mocker: MockerFixture) -> _FakePinkIK:
+    backend = _FakePinkIK()
+    mocker.patch(
+        "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.PinkControlIK",
+        return_value=backend,
+    )
+    return backend
+
+
+@pytest.fixture
+def task(tmp_path: Path, fake_ik: _FakePinkIK) -> TeleopIKTask:
+    return TeleopIKTask(
+        "teleop_arm",
+        TeleopIKTaskConfig(
+            joint_names=["arm/joint1", "arm/joint2"],
+            control_ik=_pink_config(tmp_path / "unused.urdf"),
+            hand="right",
+            min_dt=0.02,
+            max_dt=0.03,
+            max_joint_delta_deg=5.0,
+        ),
+    )
+
+
+@pytest.fixture
+def gripper_task(tmp_path: Path, fake_ik: _FakePinkIK) -> TeleopIKTask:
+    return TeleopIKTask(
+        "teleop_arm",
+        TeleopIKTaskConfig(
+            joint_names=["arm/joint1", "arm/joint2"],
+            control_ik=_pink_config(tmp_path / "unused.urdf"),
+            hand="right",
+            gripper_joint="arm/gripper",
+            gripper_open_pos=0.8,
+            gripper_closed_pos=0.0,
+        ),
+    )
+
+
+def test_delta_is_composed_with_one_measured_engagement_baseline(
+    task: TeleopIKTask, fake_ik: _FakePinkIK
+) -> None:
+    assert task.on_cartesian_command(_delta(), t_now=1.0)
+
+    first = task.compute(_state(1.01, (1.0, 2.0), dt=1.0))
+    second = task.compute(_state(1.02, (1.5, 2.5), dt=0.001))
+
+    assert first is not None
+    assert second is not None
+    assert len(fake_ik.fk_calls) == 1
+    first_target, first_measured, first_dt = fake_ik.solve_calls[0]
+    second_target, second_measured, second_dt = fake_ik.solve_calls[1]
+    baseline_rotation = pinocchio.exp3(np.array([0.0, 0.0, 0.2]))
+    delta_rotation = pinocchio.exp3(np.array([0.0, 0.0, 0.3]))
+    assert np.allclose(first_target.translation, [1.1, 1.8, 0.7])
+    assert np.allclose(first_target.rotation, delta_rotation @ baseline_rotation)
+    assert np.allclose(second_target.translation, first_target.translation)
+    assert np.allclose(first_measured, [1.0, 2.0])
+    assert np.allclose(second_measured, [1.5, 2.5])
+    assert (first_dt, second_dt) == (0.03, 0.02)
+
+
+def test_missing_joint_state_defers_baseline_and_output(
+    task: TeleopIKTask, fake_ik: _FakePinkIK
+) -> None:
+    assert task.on_cartesian_command(_delta(), t_now=1.0)
+
+    output = task.compute(_state(1.01, (0.0,)))
+
+    assert output is None
+    assert fake_ik.fk_calls == []
+    assert fake_ik.solve_calls == []
+
+
+def test_solver_failure_and_excessive_delta_return_measured_hold(
+    task: TeleopIKTask, fake_ik: _FakePinkIK
+) -> None:
+    assert task.on_cartesian_command(_delta(), t_now=1.0)
+    fake_ik.raise_runtime = True
+    failed = task.compute(_state(1.01, (0.4, 0.5)))
+
+    fake_ik.raise_runtime = False
+    fake_ik.solution = np.array([2.0, 0.5], dtype=np.float64)
+    rejected = task.compute(_state(1.02, (0.4, 0.5)))
+
+    assert failed is not None
+    assert failed.positions == [0.4, 0.5]
+    assert rejected is not None
+    assert rejected.positions == [0.4, 0.5]
+    assert rejected.mode == ControlMode.SERVO_POSITION
+
+
+def test_release_timeout_stop_and_clear_force_fresh_baselines(
+    task: TeleopIKTask, fake_ik: _FakePinkIK
+) -> None:
+    pressed = Buttons()
+    pressed.right_primary = True
+    released = Buttons()
+
+    assert task.on_teleop_buttons(pressed, 0.0)
+    assert task.on_cartesian_command(_delta(), 1.0)
+    assert task.compute(_state(1.01)) is not None
+    assert task.on_teleop_buttons(released, 1.02)
+    assert not task.is_active()
+
+    assert task.on_teleop_buttons(pressed, 2.0)
+    assert task.on_cartesian_command(_delta(), 2.0)
+    assert task.compute(_state(2.01, (0.1, 0.2))) is not None
+    assert task.compute(_state(3.0, (0.1, 0.2))) is None
+
+    assert task.on_cartesian_command(_delta(), 4.0)
+    assert task.compute(_state(4.01, (0.2, 0.3))) is not None
+    task.stop()
+    task.start()
+    assert task.on_cartesian_command(_delta(), 5.0)
+    assert task.compute(_state(5.01, (0.3, 0.4))) is not None
+    task.clear()
+    assert len(fake_ik.fk_calls) == 4
+
+
+def test_estop_rejects_commands_and_never_replays_them(
+    gripper_task: TeleopIKTask, fake_ik: _FakePinkIK
+) -> None:
+    assert gripper_task.on_cartesian_command(_delta(), 1.0)
+    assert gripper_task.compute(_state(1.01)) is not None
+
+    gripper_task.set_estop(True)
+    assert not gripper_task.on_cartesian_command(_delta((9.0, 0.0, 0.0)), 2.0)
+    assert not gripper_task.on_gripper_trigger(1.0)
+    assert not gripper_task.is_active()
+
+    gripper_task.set_estop(False)
+    assert gripper_task.compute(_state(2.01)) is None
+    assert gripper_task.on_cartesian_command(_delta(), 3.0)
+    assert gripper_task.compute(_state(3.01, (0.2, 0.3))) is not None
+    assert len(fake_ik.fk_calls) == 2
+
+
+def test_gripper_claim_interpolation_and_hold_output(
+    gripper_task: TeleopIKTask, fake_ik: _FakePinkIK
+) -> None:
+    assert gripper_task.on_gripper_trigger(0.25)
+    assert gripper_task.on_cartesian_command(_delta(), 1.0)
+    fake_ik.raise_runtime = True
+
+    output = gripper_task.compute(_state(1.01, (0.4, 0.5)))
+
+    assert gripper_task.claim().joints == frozenset({"arm/joint1", "arm/joint2", "arm/gripper"})
+    assert output is not None
+    assert output.joint_names == ["arm/joint1", "arm/joint2", "arm/gripper"]
+    assert output.positions == pytest.approx([0.4, 0.5, 0.6])
+
+
+def test_factory_requires_pink_configuration_and_matching_model(tmp_path: Path) -> None:
+    legacy = TaskConfig(
+        name="teleop",
+        type="teleop_ik",
+        joint_names=["arm/joint1", "arm/joint2"],
+        params={"model_path": "legacy.xml", "ee_joint_id": 2, "hand": "right"},
+    )
+    with pytest.raises(ValueError, match="control_ik"):
+        create_task(legacy, {})
+
+    mismatched = TaskConfig(
+        name="teleop",
+        type="teleop_ik",
+        joint_names=["wrong/joint1", "wrong/joint2"],
+        params={
+            "control_ik": {"robot_model": _robot(tmp_path / "unused.urdf")},
+            "hand": "right",
+        },
+    )
+    with pytest.raises(ValueError, match="task joints must match"):
+        create_task(mismatched, {})
diff --git a/dimos/robot/manipulators/common/blueprints.py b/dimos/robot/manipulators/common/blueprints.py
index 783943d9d1..66120cb279 100644
--- a/dimos/robot/manipulators/common/blueprints.py
+++ b/dimos/robot/manipulators/common/blueprints.py
@@ -17,7 +17,6 @@
 from __future__ import annotations
 
 from collections.abc import Mapping, Sequence
-from pathlib import Path
 from typing import Any
 
 from dimos.control.components import HardwareComponent
@@ -126,16 +125,16 @@ def eef_twist_task(
 def teleop_ik_task(
     hardware: HardwareComponent,
     *,
-    model_path: Path,
-    ee_joint_id: int,
     hand: str,
     name: str,
+    robot_model: RobotModelConfig,
     priority: int = 10,
-    params: dict[str, Any] | None = None,
+    control_ik: Mapping[str, object] | None = None,
+    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,
         "hand": hand,
     }
     if params:
diff --git a/dimos/robot/manipulators/common/mixed.py b/dimos/robot/manipulators/common/mixed.py
index 591f5fbfa0..fcd0615c2a 100644
--- a/dimos/robot/manipulators/common/mixed.py
+++ b/dimos/robot/manipulators/common/mixed.py
@@ -18,8 +18,15 @@
 
 from dimos.control.coordinator import ControlCoordinator, TaskConfig
 from dimos.core.global_config import global_config
-from dimos.robot.manipulators.piper.config import PIPER_FK_MODEL, make_piper_hardware
-from dimos.robot.manipulators.xarm.config import XARM6_FK_MODEL, make_xarm_hardware
+from dimos.robot.manipulators.common.blueprints import teleop_ik_task
+from dimos.robot.manipulators.piper.config import (
+    make_piper_hardware,
+    make_piper_model_config,
+)
+from dimos.robot.manipulators.xarm.config import (
+    make_xarm6_model_config,
+    make_xarm_hardware,
+)
 
 _xarm6_dual = make_xarm_hardware(
     "xarm_arm",
@@ -59,23 +66,25 @@
     address=global_config.can_port or "can0",
     gripper=True,
 )
+_xarm6_teleop_model = make_xarm6_model_config(name="xarm_arm", add_gripper=False)
+_piper_teleop_model = make_piper_model_config(name="piper_arm")
 
 coordinator_teleop_dual = ControlCoordinator.blueprint(
     hardware=[_xarm6_teleop_hw, _piper_teleop_hw],
     tasks=[
-        TaskConfig(
+        teleop_ik_task(
+            _xarm6_teleop_hw,
             name="teleop_xarm",
-            type="teleop_ik",
-            joint_names=_xarm6_teleop_hw.joints,
+            hand="left",
+            robot_model=_xarm6_teleop_model,
             priority=10,
-            params={"model_path": XARM6_FK_MODEL, "ee_joint_id": 6, "hand": "left"},
         ),
-        TaskConfig(
+        teleop_ik_task(
+            _piper_teleop_hw,
             name="teleop_piper",
-            type="teleop_ik",
-            joint_names=_piper_teleop_hw.joints,
+            hand="right",
+            robot_model=_piper_teleop_model,
             priority=10,
-            params={"model_path": PIPER_FK_MODEL, "ee_joint_id": 6, "hand": "right"},
         ),
     ],
 )
diff --git a/dimos/robot/manipulators/piper/blueprints/teleop.py b/dimos/robot/manipulators/piper/blueprints/teleop.py
index 8f3e17acc8..8933aa14b0 100644
--- a/dimos/robot/manipulators/piper/blueprints/teleop.py
+++ b/dimos/robot/manipulators/piper/blueprints/teleop.py
@@ -31,7 +31,6 @@
 )
 from dimos.robot.manipulators.common.sim import mujoco_if_sim
 from dimos.robot.manipulators.piper.config import (
-    PIPER_FK_MODEL,
     PIPER_SIM_PATH,
     make_piper_hardware,
     make_piper_model_config,
@@ -99,10 +98,9 @@ class _PiperTeleopCoordinator(ControlCoordinator):
         tasks=[
             teleop_ik_task(
                 _piper_teleop_hw,
-                model_path=PIPER_FK_MODEL,
-                ee_joint_id=6,
                 hand="left",
                 name="teleop_piper",
+                robot_model=_piper_model,
                 params={
                     "gripper_joint": make_gripper_joints("arm")[0],
                     "gripper_open_pos": 1.0,
diff --git a/dimos/robot/manipulators/piper/config.py b/dimos/robot/manipulators/piper/config.py
index 98943012ed..1d390aa553 100644
--- a/dimos/robot/manipulators/piper/config.py
+++ b/dimos/robot/manipulators/piper/config.py
@@ -41,7 +41,6 @@
     "piper_description": LfsPath("piper_description"),
     "piper_gazebo": LfsPath("piper_description"),
 }
-PIPER_FK_MODEL = LfsPath("piper_description/mujoco_model/piper_no_gripper_description.xml")
 PIPER_SIM_PATH = LfsPath("piper/scene.xml")
 PIPER_HOME_JOINTS = [
     0.793,
diff --git a/dimos/robot/manipulators/xarm/blueprints/teleop.py b/dimos/robot/manipulators/xarm/blueprints/teleop.py
index b41e615b37..c760d38d58 100644
--- a/dimos/robot/manipulators/xarm/blueprints/teleop.py
+++ b/dimos/robot/manipulators/xarm/blueprints/teleop.py
@@ -28,9 +28,7 @@
 )
 from dimos.robot.manipulators.common.sim import mujoco_if_sim
 from dimos.robot.manipulators.xarm.config import (
-    XARM6_FK_MODEL,
     XARM6_SIM_PATH,
-    XARM7_FK_MODEL,
     XARM7_SIM_PATH,
     XARM_GRIPPER_PARAMS,
     make_xarm6_model_config,
@@ -164,10 +162,9 @@ class _XArm7TeleopCoordinator(ControlCoordinator):
         tasks=[
             teleop_ik_task(
                 _xarm7_teleop_hw,
-                model_path=XARM7_FK_MODEL,
-                ee_joint_id=7,
                 hand="right",
                 name="teleop_xarm",
+                robot_model=_xarm7_control_model,
                 priority=20,
                 params=XARM_GRIPPER_PARAMS,
             ),
@@ -188,10 +185,9 @@ class _XArm7TeleopCoordinator(ControlCoordinator):
         tasks=[
             teleop_ik_task(
                 _xarm6_teleop_hw,
-                model_path=XARM6_FK_MODEL,
-                ee_joint_id=6,
                 hand="right",
                 name="teleop_xarm",
+                robot_model=_xarm6_control_model,
                 priority=20,
                 params=XARM_GRIPPER_PARAMS,
             ),
diff --git a/dimos/robot/manipulators/xarm/config.py b/dimos/robot/manipulators/xarm/config.py
index bb7d577927..5ce262f70b 100644
--- a/dimos/robot/manipulators/xarm/config.py
+++ b/dimos/robot/manipulators/xarm/config.py
@@ -56,8 +56,6 @@
 
 XARM_MODEL_PATH = LfsPath("xarm_description") / "urdf/xarm_device.urdf.xacro"
 XARM_PACKAGE_PATHS: dict[str, Path] = {"xarm_description": LfsPath("xarm_description")}
-XARM6_FK_MODEL = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf")
-XARM7_FK_MODEL = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf")
 XARM6_SIM_PATH = LfsPath("xarm6/scene.xml")
 XARM7_SIM_PATH = LfsPath("xarm7/scene.xml")
 XARM_GRIPPER_PARAMS = {
diff --git a/docs/adr/0001-use-pink-control-ik-for-teleop-tasks.md b/docs/adr/0001-use-pink-control-ik-for-teleop-tasks.md
new file mode 100644
index 0000000000..18d412aaf8
--- /dev/null
+++ b/docs/adr/0001-use-pink-control-ik-for-teleop-tasks.md
@@ -0,0 +1,3 @@
+# Use Pink control IK for teleop tasks
+
+Teleop IK tasks will specialize the Cartesian IK task so every shipped manipulator reuses its measured-state Pink solve and safety pipeline while retaining engage-relative target preparation, E-STOP behavior, and gripper control. The migration is atomic: Piper, xArm6, xArm7, and mixed-arm blueprints will replace the legacy model-path and numeric end-effector-joint configuration with an authoritative `RobotModelConfig`; commands received during E-STOP are discarded, clearing E-STOP requires a fresh engagement baseline, and each model's named end-effector frame is used even where this intentionally changes legacy operator feel. Composition may replace the inheritance seam later if multiple specializations demonstrate a concrete need for a separate control-IK module.
diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md
index 832acb49ac..13d5ff826d 100644
--- a/docs/capabilities/manipulation/adding_a_custom_arm.md
+++ b/docs/capabilities/manipulation/adding_a_custom_arm.md
@@ -578,17 +578,23 @@ target frames. `base_link` is only the robot-scoped link placed by
 `base_pose`; do not use it as a substitute for planning-group chain metadata.
 See [Planning Groups](/docs/capabilities/manipulation/planning_groups.md).
 
-### 4d. Configure Cartesian and EEF-twist control IK
+### 4d. Configure Cartesian, EEF-twist, and teleop control IK
 
-Cartesian and EEF-twist tasks use the direct URDF or Xacro in
-`RobotModelConfig`. Set `package_paths` and `xacro_args` when needed, name the
-end-effector link, and map coordinator joints to model joints. The task validates
-the prepared model, frame, and joint mapping at startup.
+Cartesian, EEF-twist, and engagement-relative teleop tasks use the direct URDF
+or Xacro in `RobotModelConfig`. Set `package_paths` and `xacro_args` when needed,
+name the end-effector link, and map coordinator joints to model joints. The task
+validates the prepared model, frame, and joint mapping at startup. Teleop uses
+the named frame and does not accept a separate model path or numeric
+end-effector joint ID.
 
 Pass the same model configuration to the common helpers:
 
 ```python skip
-from dimos.robot.manipulators.common.blueprints import cartesian_ik_task, eef_twist_task
+from dimos.robot.manipulators.common.blueprints import (
+    cartesian_ik_task,
+    eef_twist_task,
+    teleop_ik_task,
+)
 
 cartesian_task = cartesian_ik_task(
     hardware,
@@ -598,12 +604,20 @@ twist_task = eef_twist_task(
     hardware,
     robot_model=robot_model,
 )
+teleop_task = teleop_ik_task(
+    hardware,
+    name="teleop_arm",
+    hand="right",
+    robot_model=robot_model,
+)
 ```
 
 Each tick starts from measured joints and applies model position and velocity
-limits. Twist targets are derived from measured forward kinematics. Invalid
-models or mappings fail at startup; invalid runtime output holds the measured
-position. Validate Cartesian and twist behavior in simulation or replay before
+limits. Twist targets are derived from measured forward kinematics. Teleop
+targets apply controller deltas to a measured engagement baseline and discard
+that baseline across disengage, timeout, stop, clear, or E-STOP. Invalid models
+or mappings fail at startup; invalid runtime output holds the measured position.
+Validate Cartesian, twist, and teleop behavior in simulation or replay before
 hardware use.
 
 ## Step 5: Register Blueprints
diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md
index 8319245371..fe4d8460db 100644
--- a/docs/capabilities/manipulation/index.md
+++ b/docs/capabilities/manipulation/index.md
@@ -153,10 +153,12 @@ tool, or CLI motion command yet.
 
 ### Cartesian control IK
 
-Cartesian and keyboard EEF-twist tasks use the direct URDF/Xacro model from
-`RobotModelConfig`. The configuration supplies package paths, Xacro arguments,
-the named end-effector frame, and coordinator-to-model joint mapping. Invalid
-models, frames, or mappings fail at startup.
+Cartesian, keyboard EEF-twist, and engagement-relative teleop IK tasks use the
+direct URDF/Xacro model from `RobotModelConfig`. The configuration supplies
+package paths, Xacro arguments, the named end-effector frame, and
+coordinator-to-model joint mapping. Invalid models, frames, or mappings fail at
+startup; teleop configuration does not use a separate model path or numeric
+end-effector joint ID.
 
 Each control tick starts from measured joints, applies model position and
 velocity limits, and holds the measured position when a solve cannot produce a
@@ -166,16 +168,27 @@ does not use `WorldSpec` or provide world-obstacle avoidance.
 For a custom robot, pass the typed model configuration to the helper:
 
 ```python skip
-from dimos.robot.manipulators.common.blueprints import cartesian_ik_task
+from dimos.robot.manipulators.common.blueprints import cartesian_ik_task, teleop_ik_task
 
 task = cartesian_ik_task(
     hardware,
     robot_model=robot_model,
 )
+teleop_task = teleop_ik_task(
+    hardware,
+    name="teleop_arm",
+    hand="right",
+    robot_model=robot_model,
+)
 ```
 
-Validate Cartesian and twist behavior in simulation or replay before hardware
-use.
+Teleop pose commands are deltas from an end-effector pose captured from measured
+joints at engagement. Disengage, timeout, stop, clear, or E-STOP discards that
+baseline; commands received during E-STOP are rejected rather than replayed
+after clear.
+
+Validate Cartesian, twist, and teleop behavior in simulation or replay before
+hardware use.
 
 Install the manipulation dependencies:
 
diff --git a/openspec/changes/use-pink-control-ik-for-teleop/.openspec.yaml b/openspec/changes/use-pink-control-ik-for-teleop/.openspec.yaml
new file mode 100644
index 0000000000..e8209ffaac
--- /dev/null
+++ b/openspec/changes/use-pink-control-ik-for-teleop/.openspec.yaml
@@ -0,0 +1,2 @@
+schema: spec-driven
+created: 2026-07-28
diff --git a/openspec/changes/use-pink-control-ik-for-teleop/design.md b/openspec/changes/use-pink-control-ik-for-teleop/design.md
new file mode 100644
index 0000000000..e17e78f0a1
--- /dev/null
+++ b/openspec/changes/use-pink-control-ik-for-teleop/design.md
@@ -0,0 +1,89 @@
+## Context
+
+`TeleopIKTask` currently duplicates Cartesian task concerns around measured joint access, forward kinematics, inverse kinematics, output construction, and joint-delta safety. It constructs a legacy iterative `PinocchioIK` solver from a model file and numeric joint ID, while `CartesianIKTask` and `EEFTwistTask` use `PinkControlIK` configured by the authoritative `RobotModelConfig`. `EEFTwistTask` already demonstrates a target-source specialization of `CartesianIKTask`.
+
+The teleop task must continue interpreting Quest/hosted-controller messages as pose deltas relative to the robot pose at engagement, must remain a declarative coordinator task, and must retain gripper and E-STOP behavior. The migration affects Piper, xArm6, xArm7, and their mixed-arm composition.
+
+## Goals / Non-Goals
+
+**Goals:**
+
+- Give every shipped teleop IK task the same measured-state Pink solve and safety pipeline as Cartesian IK.
+- Preserve engage-relative pose semantics, gripper behavior, task-name routing, arbitration, and preemption.
+- Make `RobotModelConfig` the sole authority for controlled joints and the named end-effector frame.
+- Fail closed across E-STOP and runtime solver failures.
+- Migrate every in-repository teleop IK configuration atomically.
+
+**Non-Goals:**
+
+- Refactor Cartesian, twist, and teleop tasks around a new composed control engine.
+- Change controller-side delta generation, task registry routing, coordinator arbitration, or gripper units.
+- Add collision avoidance to the real-time Pink control task.
+- Preserve the legacy model-path and numeric end-effector-joint configuration.
+- Tune final hardware-specific control gains without simulation and hardware evidence.
+
+## Decisions
+
+### Teleop is a focused Cartesian IK specialization
+
+`TeleopIKTaskConfig` will extend `CartesianIKTaskConfig` with hand and gripper fields, and `TeleopIKTask` will extend `CartesianIKTask`. The parent will own joint/model validation, Pink construction, measured-state reads, bounded coordinator `dt`, IK execution, result validation, joint-delta checks, measured-state holds, timeout bookkeeping, and the arm resource claim.
+
+The child will own engagement-baseline state, delta-to-absolute target preparation, E-STOP gating, controller buttons, gripper interpolation, the extended gripper claim, and appending the gripper command to the parent output.
+
+Composition was considered because repository guidance generally prefers it. It is deferred because the current parent already exposes target-preparation and timeout hooks, `EEFTwistTask` proves the specialization pattern, and extracting a new engine would broaden this safety migration. A composed module can replace the inheritance seam later if concrete strain appears across multiple specializations.
+
+### The child prepares an absolute target from an engagement-relative delta
+
+On the first compute after a new engaged delta arrives, the task will capture forward kinematics from the current measured coordinator joints as the engagement baseline. It will preserve the established transform:
+
+- target translation = baseline translation + delta translation
+- target rotation = delta rotation × baseline rotation
+
+The resulting normalized, finite `SE3` target is passed through the parent's Pink compute pipeline. Disengage, timeout, stop, clear, or E-STOP discards the baseline so the next engagement starts from the then-current measured pose.
+
+### E-STOP discards rather than defers commands
+
+Latching E-STOP will make the task inert and clear the pose target, engagement baseline, and transient engagement state. Pose and gripper commands received while latched will return rejection without changing cached state. Clearing E-STOP will not restore any prior command; a fresh post-clear engagement and baseline are required.
+
+This avoids replaying in-flight or stale commands after a safety discontinuity.
+
+### RobotModelConfig is the only model authority
+
+The teleop factory and blueprint helper will accept the same nested Pink control configuration used by Cartesian and EEF-twist tasks. `model_path` and `ee_joint_id` will be removed from teleop parameters without a compatibility branch.
+
+Pink will target `RobotModelConfig.end_effector_link` and map the ordered coordinator joints through `joint_name_mapping`. Mixed-arm blueprints will construct robot models whose names match their hardware namespaces. Piper will intentionally move from legacy joint 6 to the model's `gripper_base` frame.
+
+### Existing declarative routing remains stable
+
+The task type remains `teleop_ik`. Its task card continues consuming task-name-routed Cartesian commands and broadcast teleop buttons. No coordinator stream or transport changes are required. The blueprint helper changes only how task parameters are assembled.
+
+### Verification is behavior-focused
+
+Task tests will exercise behavior through the task interface with a surgical fake Pink backend. They will cover exact delta composition, measured-state baseline capture and reseeding, bounded `dt`, valid output, measured holds, joint-delta rejection, E-STOP rejection, timeout, and gripper claim/output. Blueprint tests will verify that every shipped teleop task carries an authoritative reconstructable Pink configuration and no legacy model fields.
+
+Simulation smoke tests will validate named frames and operator motion before real Piper or xArm hardware rollout.
+
+## Risks / Trade-offs
+
+- **Piper's controlled point changes from joint 6 to `gripper_base`** → Treat this as intentional, validate translation and rotation behavior in simulation, then perform a low-speed hardware check.
+- **Pink's one-step differential response differs from the legacy multi-iteration solver** → Start with conservative existing Pink limits, retain the outer joint-delta guard, and tune gains only from measured simulation/hardware behavior.
+- **Inheritance couples teleop to protected Cartesian task state** → Keep overrides limited to documented hooks and teleop policy; defer a composition refactor until concrete additional variation justifies a new seam.
+- **Pink becomes mandatory for teleop task construction** → Keep imports actionable when the manipulation extra is absent and cover the failure path in tests.
+- **Atomic configuration removal is breaking** → Update all known in-repository callers and blueprint assertions in the same change so no shipped mixed solver configuration remains.
+- **Pose and button streams can arrive concurrently** → Guard all task-owned transient state consistently and test E-STOP/engagement transitions rather than relying on stream ordering.
+
+## Migration Plan
+
+1. Refactor the teleop configuration and class onto the Cartesian/Pink pipeline while preserving its registry type and streams.
+2. Change the shared teleop blueprint helper to resolve `RobotModelConfig` into Pink control configuration.
+3. Update Piper, xArm6, xArm7, and mixed-arm call sites atomically, including hardware-namespace-specific robot models.
+4. Replace legacy teleop solver tests with behavior tests at the task interface and extend blueprint configuration coverage.
+5. Update manipulation documentation and remove teleop references to model paths or numeric end-effector joint IDs.
+6. Run focused unit and blueprint tests, then the relevant broader test suite and type/style checks.
+7. Exercise each shipped teleop blueprint in simulation; perform low-speed hardware validation after simulation succeeds.
+
+Rollback is a source-level revert of the task, helper, call-site, test, and documentation changes as one unit. There is no persisted data migration.
+
+## Open Questions
+
+None. Hardware-specific Pink gains remain rollout tuning rather than an unresolved architecture decision.
diff --git a/openspec/changes/use-pink-control-ik-for-teleop/proposal.md b/openspec/changes/use-pink-control-ik-for-teleop/proposal.md
new file mode 100644
index 0000000000..e6b201515e
--- /dev/null
+++ b/openspec/changes/use-pink-control-ik-for-teleop/proposal.md
@@ -0,0 +1,28 @@
+## Why
+
+Declarative teleop IK still uses a separate legacy Pinocchio solver and identifies robot models with file paths and numeric end-effector joint IDs. Moving teleop onto the measured-state Pink control pipeline gives Cartesian, twist, and teleop control one model authority and one safety-critical IK implementation.
+
+## What Changes
+
+- Make the teleop IK task a focused specialization of the Cartesian IK task, reusing Pink solving, measured-state anchoring, bounded tick timing, output validation, joint-limit handling, and measured-state holds.
+- Preserve teleop-specific engage-relative pose interpretation, controller-button behavior, gripper control, task-name routing, arbitration, and preemption.
+- Discard cached targets and engagement baselines on disengage, timeout, stop, clear, or E-STOP; reject pose and gripper commands while E-STOP is latched.
+- Use each authoritative `RobotModelConfig` named end-effector frame, including Piper's `gripper_base`.
+- Migrate Piper, xArm6, xArm7, and mixed xArm/Piper teleop blueprints together.
+- **BREAKING**: Replace teleop IK's `model_path` and numeric `ee_joint_id` parameters with `RobotModelConfig`-backed Pink control configuration; the legacy configuration is not supported concurrently.
+
+## Capabilities
+
+### New Capabilities
+
+- `teleop-ik-control`: Engage-relative manipulator teleoperation through measured-state Pink control IK, including model authority, safety, E-STOP, gripper, and lifecycle behavior.
+
+### Modified Capabilities
+
+None.
+
+## Impact
+
+- Affects the teleop control task and factory, shared manipulator blueprint helpers, Piper and xArm teleop blueprints, the mixed-manipulator coordinator, and their tests and manipulation documentation.
+- Makes the existing optional Pink/manipulation dependencies required when constructing a teleop IK task.
+- Intentionally changes Piper's controlled frame from legacy joint 6 to the model's named `gripper_base` frame and may require simulation and hardware tuning.
diff --git a/openspec/changes/use-pink-control-ik-for-teleop/specs/teleop-ik-control/spec.md b/openspec/changes/use-pink-control-ik-for-teleop/specs/teleop-ik-control/spec.md
new file mode 100644
index 0000000000..5696d2d630
--- /dev/null
+++ b/openspec/changes/use-pink-control-ik-for-teleop/specs/teleop-ik-control/spec.md
@@ -0,0 +1,110 @@
+## ADDED Requirements
+
+### Requirement: Authoritative teleop robot model
+The system SHALL configure every teleop IK task with Pink control IK backed by an authoritative `RobotModelConfig`. The task's ordered controlled joints SHALL match the model's ordered coordinator joint names, and the target frame SHALL be the model's named end-effector frame.
+
+#### Scenario: Construct teleop for a named robot model
+- **WHEN** a teleop IK task is constructed with a valid robot model whose coordinator joints match the task joints
+- **THEN** the task uses Pink control IK and targets the model's named end-effector frame
+
+#### Scenario: Reject mismatched controlled joints
+- **WHEN** a teleop IK task's ordered joints do not match the robot model's ordered coordinator joints
+- **THEN** task construction fails with an actionable configuration error
+
+#### Scenario: Reject legacy model parameters
+- **WHEN** a teleop IK task is configured only with a model path and numeric end-effector joint ID
+- **THEN** task construction rejects the legacy configuration instead of selecting the legacy solver
+
+### Requirement: Engagement-relative Cartesian target
+The system SHALL interpret a teleop pose command as an end-effector delta relative to an engagement baseline captured by forward kinematics from the current measured coordinator joints. Translation SHALL be added to the baseline translation, and delta rotation SHALL left-multiply the baseline rotation.
+
+#### Scenario: First command captures the measured baseline
+- **WHEN** the first pose delta of an engagement is computed with a complete measured joint state
+- **THEN** the task captures the corresponding measured end-effector pose as its engagement baseline and solves for the composed absolute target
+
+#### Scenario: Subsequent commands retain the engagement baseline
+- **WHEN** additional pose deltas arrive during the same engagement while measured joints change
+- **THEN** each target is composed from the same engagement baseline rather than recapturing it from the latest joint state
+
+#### Scenario: Missing measured joints defer baseline capture
+- **WHEN** the task receives an engaged pose delta but the coordinator state lacks any controlled joint position
+- **THEN** the task emits no joint command and does not capture a partial engagement baseline
+
+### Requirement: Measured-state Pink control safety
+For each active tick, the system SHALL seed Pink control IK from the current finite measured joint state, bound the coordinator timestep to configured limits, validate solver output, and enforce the configured per-tick joint-delta limit. An expected target-preparation or IK runtime failure after a complete measured state is available SHALL produce a measured-state servo-position hold.
+
+#### Scenario: Valid target produces an arbitrated joint command
+- **WHEN** Pink returns a finite, correctly shaped solution within configured limits
+- **THEN** the task emits a servo-position command for the controlled arm joints
+
+#### Scenario: Coordinator timestep is bounded
+- **WHEN** the coordinator timestep is outside the configured positive minimum and maximum
+- **THEN** the task uses the nearest configured bound for target preparation and Pink solving
+
+#### Scenario: Solver failure holds measured state
+- **WHEN** target preparation or Pink solving raises an expected runtime failure after measured joints are available
+- **THEN** the task emits a servo-position hold at the measured arm positions
+
+#### Scenario: Invalid or excessive solution holds measured state
+- **WHEN** Pink returns a non-finite, incorrectly shaped, or excessive joint-delta solution
+- **THEN** the task rejects the candidate and emits a servo-position hold at the measured arm positions
+
+### Requirement: Fresh baseline after lifecycle discontinuity
+The system SHALL discard the current pose target and engagement baseline on disengage, timeout, stop, clear, or E-STOP. The next accepted engagement SHALL capture a fresh baseline from the then-current measured robot state.
+
+#### Scenario: Disengage and re-engage
+- **WHEN** an operator disengages after commanding motion and later re-engages
+- **THEN** the next computed delta is based on a newly measured engagement baseline
+
+#### Scenario: Command stream times out
+- **WHEN** no teleop pose update arrives within the configured nonzero timeout
+- **THEN** the task becomes inactive, discards its target and baseline, and emits no command until a new engagement
+
+#### Scenario: Task is stopped or cleared
+- **WHEN** the task is stopped or cleared
+- **THEN** its target and engagement baseline are discarded and it no longer participates in arbitration
+
+### Requirement: Fail-closed E-STOP behavior
+The system SHALL make a teleop IK task inert while E-STOP is latched, SHALL reject pose and gripper commands received while latched, and SHALL NOT replay pre-latch or latched commands when E-STOP is cleared.
+
+#### Scenario: E-STOP while teleop is active
+- **WHEN** E-STOP is latched during an active engagement
+- **THEN** the task immediately clears its target and baseline and becomes inactive
+
+#### Scenario: Commands arrive during E-STOP
+- **WHEN** pose or gripper commands arrive while E-STOP is latched
+- **THEN** the commands are rejected without changing the cached target, baseline, or gripper target
+
+#### Scenario: E-STOP is cleared
+- **WHEN** E-STOP is cleared after one or more commands were rejected
+- **THEN** the task remains free of replayable commands and requires a fresh engagement baseline before arm motion resumes
+
+### Requirement: Teleop gripper and arbitration behavior
+The system SHALL preserve teleop gripper interpolation, resource claims, task-name-routed pose delivery, broadcast controller-button delivery, and joint-level arbitration while using Pink control IK.
+
+#### Scenario: Analog trigger commands the gripper
+- **WHEN** a configured hand supplies an analog trigger value from zero through one
+- **THEN** the task clamps the value, interpolates between configured open and closed positions, and appends the gripper target to an active arm output
+
+#### Scenario: Task claims the gripper
+- **WHEN** a teleop task is configured with a gripper joint
+- **THEN** its resource claim includes both the controlled arm joints and the gripper joint at the task priority
+
+#### Scenario: Cartesian command is routed by task name
+- **WHEN** a pose delta names a registered teleop task
+- **THEN** the coordinator delivers it only to that named task
+
+#### Scenario: Higher-priority task wins arbitration
+- **WHEN** a higher-priority task claims any of the same joints as an active teleop task
+- **THEN** coordinator arbitration gives those joints to the higher-priority task and reports preemption to teleop
+
+### Requirement: Atomic shipped-blueprint migration
+The system SHALL configure every shipped Piper, xArm6, xArm7, and mixed xArm/Piper teleop IK task through the authoritative Pink robot-model interface, with no shipped teleop task retaining legacy model-path or numeric end-effector-joint parameters.
+
+#### Scenario: Inspect shipped teleop task configurations
+- **WHEN** the shipped teleop blueprints are resolved
+- **THEN** each teleop IK task contains a reconstructable Pink control configuration whose robot-model coordinator joints match its hardware joints
+
+#### Scenario: Resolve mixed-arm teleop
+- **WHEN** the mixed xArm/Piper teleop coordinator is resolved
+- **THEN** each teleop task uses a robot model mapped to its own hardware namespace
diff --git a/openspec/changes/use-pink-control-ik-for-teleop/tasks.md b/openspec/changes/use-pink-control-ik-for-teleop/tasks.md
new file mode 100644
index 0000000000..f4300f6b99
--- /dev/null
+++ b/openspec/changes/use-pink-control-ik-for-teleop/tasks.md
@@ -0,0 +1,36 @@
+## 1. Teleop Pink Task
+
+- [x] 1.1 Add focused teleop task tests with a surgical fake Pink backend covering exact engagement-relative translation/rotation composition, first-tick measured FK capture, retained baseline, missing joint state, bounded `dt`, valid output, measured-state holds, and joint-delta rejection.
+- [x] 1.2 Refactor `TeleopIKTaskConfig` and `TeleopIKTask` to specialize the Cartesian IK task and remove direct construction or use of legacy `PinocchioIK`, `model_path`, and `ee_joint_id`.
+- [x] 1.3 Implement and test lifecycle baseline resets for disengage, timeout, stop, and clear while preserving task activation and preemption behavior.
+- [x] 1.4 Implement and test fail-closed E-STOP handling that clears transient state, rejects pose and gripper commands while latched, and prevents command replay after clear.
+- [x] 1.5 Preserve and test analog gripper interpolation, combined arm/gripper resource claims, and appending gripper positions to both valid Pink outputs and measured-state holds.
+- [x] 1.6 Update teleop parameter validation and factory tests so authoritative Pink configuration is required, joint/model mismatches fail actionably, and missing optional Pink dependencies report the manipulation-extra installation path.
+
+## 2. Atomic Blueprint Migration
+
+- [x] 2.1 Change the shared `teleop_ik_task` blueprint helper to accept `RobotModelConfig`, resolve the nested Pink control configuration, and reject the removed legacy model parameters.
+- [x] 2.2 Migrate Piper teleop to its authoritative robot model and named `gripper_base` end-effector frame while preserving hand selection, routing, priority, and gripper units.
+- [x] 2.3 Migrate xArm6 and xArm7 teleop tasks to their authoritative no-gripper control models while preserving the lower-priority EEF-twist fallback and gripper behavior.
+- [x] 2.4 Migrate mixed xArm/Piper teleop to per-hardware-namespace robot models whose ordered coordinator joints match each hardware task.
+- [x] 2.5 Remove teleop-only legacy FK-model constants and imports that become unused after all call sites migrate.
+
+## 3. Routing and Configuration Verification
+
+- [x] 3.1 Extend blueprint tests to verify every shipped `teleop_ik` task has a reconstructable `PinkControlIKConfig`, matching hardware/model joints, the expected named end-effector frame, and no `model_path` or `ee_joint_id`.
+- [x] 3.2 Preserve and verify registry-card behavior for task-name-routed Cartesian pose deltas and broadcast teleop buttons.
+- [x] 3.3 Verify coordinator arbitration and preemption behavior remains unchanged for teleop versus lower-priority EEF-twist and higher-priority overlapping tasks.
+
+## 4. Documentation and Validation
+
+- [x] 4.1 Update manipulation and custom-arm documentation to describe Pink-based teleop configuration through `RobotModelConfig`, engagement-relative targets, named end-effector frames, and the removal of numeric joint IDs.
+- [x] 4.2 Run the focused teleop, Cartesian IK, EEF-twist, coordinator-routing, and manipulator-blueprint pytest suites and resolve failures.
+- [x] 4.3 Run repository formatting, lint, type checks, and `git diff --check` for all changed control and blueprint files.
+- [x] 4.4 Smoke-test Piper, xArm6, and xArm7 teleop in simulation at conservative settings, verifying fresh-baseline engagement, translation/rotation direction, E-STOP recovery, and gripper operation; record any hardware gain tuning as rollout follow-up rather than changing architecture.
+
+  Piper, xArm6, and xArm7 each passed the daemon startup health check with the
+  Quest server, MuJoCo adapter, Pink task, command routing, and gripper channel
+  active. Deterministic task tests verified fresh measured-state baselines,
+  translation/rotation composition, E-STOP clear without replay, and gripper
+  interpolation/output. No gain changes were needed; hardware-specific tuning
+  remains a rollout follow-up.

From 3fda95cc0b3bab2fd1f28ee18732961dacc807d0 Mon Sep 17 00:00:00 2001
From: cc <cc7@duck.com>
Date: Tue, 28 Jul 2026 00:20:41 -0700
Subject: [PATCH 2/8] feat: further tuning ik

---
 .../cartesian_ik_task/pink_control_ik.py      |  33 +++-
 .../cartesian_ik_task/test_pink_control_ik.py |  63 ++++++-
 .../control/tasks/teleop_task/teleop_task.py  |  14 +-
 .../tasks/teleop_task/test_teleop_task.py     |  44 +++++
 .../manipulators/xarm/blueprints/teleop.py    |  31 +++-
 dimos/robot/manipulators/xarm/config.py       |   1 -
 .../interlatent-historical-ik-comparison.md   | 160 ++++++++++++++++++
 7 files changed, 336 insertions(+), 10 deletions(-)
 create mode 100644 docs/research/interlatent-historical-ik-comparison.md

diff --git a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py
index 4cd9cdb2a7..d14fd59f6c 100644
--- a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py
+++ b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py
@@ -28,8 +28,8 @@
 
 try:
     from pink import Configuration, solve_ik
-    from pink.limits import ConfigurationLimit, VelocityLimit
-    from pink.tasks import FrameTask, PostureTask
+    from pink.limits import ConfigurationLimit
+    from pink.tasks import DampingTask, FrameTask, PostureTask
 except ModuleNotFoundError as exc:
     raise ModuleNotFoundError(
         f"{_PINK_INSTALL_ERROR} Missing module: {exc.name}",
@@ -55,6 +55,7 @@ class PinkControlIKConfig(BaseConfig):
     position_cost: FiniteFloat = Field(1.0, ge=0.0)
     orientation_cost: FiniteFloat = Field(1.0, ge=0.0)
     posture_cost: FiniteFloat = Field(1e-3, ge=0.0)
+    damping_cost: FiniteFloat = Field(0.0, ge=0.0)
     reference_q: list[float] | None = None
     qpsolver_options: dict[str, FiniteFloat] = Field(default_factory=dict)
 
@@ -89,8 +90,10 @@ class _PinkRuntime:
     configuration: Configuration
     frame_task: FrameTask
     posture_task: PostureTask | None
+    damping_task: DampingTask | None
     tasks: list[object]
     limits: list[object]
+    velocity_limits: NDArray[np.float64]
 
 
 class _PinkControlIKBuilder:
@@ -150,9 +153,20 @@ def build(self) -> _PinkRuntime:
             gain=config.task_gain,
         )
         posture_task = PostureTask(cost=config.posture_cost) if config.posture_cost > 0.0 else None
+        damping_task = DampingTask(cost=config.damping_cost) if config.damping_cost > 0.0 else None
         tasks: list[object] = [frame_task]
         if posture_task is not None:
             tasks.append(posture_task)
+        if damping_task is not None:
+            tasks.append(damping_task)
+
+        velocity_limits = np.asarray(model.velocityLimit, dtype=np.float64).copy()
+        if (
+            velocity_limits.size != model.nv
+            or not np.all(np.isfinite(velocity_limits))
+            or np.any(velocity_limits <= 0.0)
+        ):
+            raise ValueError("effective Pink velocity limits are invalid")
 
         return _PinkRuntime(
             config=config,
@@ -164,8 +178,10 @@ def build(self) -> _PinkRuntime:
             configuration=configuration,
             frame_task=frame_task,
             posture_task=posture_task,
+            damping_task=damping_task,
             tasks=tasks,
             limits=limits,
+            velocity_limits=velocity_limits,
         )
 
     @staticmethod
@@ -302,7 +318,10 @@ def _apply_limits(
                 model.velocityLimit[index] = limit
         for index in mapping.v_indices:
             model.velocityLimit[index] = min(model.velocityLimit[index], self._config.max_velocity)
-        return [ConfigurationLimit(model), VelocityLimit(model)]
+        # Keep position bounds in the QP, but apply velocity limits by uniformly
+        # scaling the solution. Tiny per-tick velocity boxes can make ProxQP
+        # misclassify feasible differential IK problems as primal-infeasible.
+        return [ConfigurationLimit(model)]
 
 
 class PinkControlIK:
@@ -355,6 +374,7 @@ def solve(
             velocity = np.asarray(velocity, dtype=np.float64).reshape(-1)
             if velocity.size != runtime.model.nv or not np.all(np.isfinite(velocity)):
                 raise IKControlRuntimeError("Pink produced an invalid velocity")
+            velocity = self._scale_velocity(velocity)
             configuration.integrate_inplace(velocity, dt)
             candidate = self._project_controlled_positions(configuration.q, measured)
             if candidate.size != measured.size or not np.all(np.isfinite(candidate)):
@@ -405,6 +425,13 @@ def _controlled_velocity(self, velocity: NDArray[np.float64]) -> NDArray[np.floa
             [velocity[index] for index in self._runtime.mapping.v_indices], dtype=np.float64
         )
 
+    def _scale_velocity(self, velocity: NDArray[np.float64]) -> NDArray[np.float64]:
+        """Uniformly scale a Pink solution to preserve its joint-space direction."""
+        max_ratio = float(np.max(np.abs(velocity) / self._runtime.velocity_limits))
+        if max_ratio <= 1.0:
+            return velocity
+        return velocity / max_ratio
+
     def _clamp_position_limits(self, candidate: NDArray[np.float64]) -> NDArray[np.float64]:
         runtime = self._runtime
         mapping = runtime.mapping
diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py
index f4d976d6f7..5e1808770a 100644
--- a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py
+++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py
@@ -118,6 +118,8 @@ def test_pink_settings_use_finite_declarative_validation(tmp_path: Path) -> None
 
     with pytest.raises(ValueError, match="finite"):
         PinkControlIKConfig(robot_model=robot, max_velocity=np.inf)
+    with pytest.raises(ValueError, match="greater than or equal to 0"):
+        PinkControlIKConfig(robot_model=robot, damping_cost=-1e-3)
     with pytest.raises(ValueError, match="finite"):
         PinkControlIKConfig(robot_model=robot, qpsolver_options={"eps": np.nan})
     with pytest.raises(ValueError, match="ordered"):
@@ -284,6 +286,34 @@ def solve(
     assert calls and len(calls[0]) == 1
 
 
+def test_pink_damping_task_replaces_posture_for_low_motion_policy(
+    tmp_path: Path, monkeypatch: pytest.MonkeyPatch
+) -> None:
+    model_path = _write_urdf(tmp_path)
+    backend = create_pink_control_ik(
+        PinkControlIKConfig(
+            robot_model=_robot(model_path),
+            posture_cost=0.0,
+            damping_cost=1e-3,
+        )
+    )
+    calls: list[list[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 backend._runtime.posture_task is None
+    assert backend._runtime.damping_task is not None
+    assert calls == [[backend._runtime.frame_task, backend._runtime.damping_task]]
+
+
 def test_pink_rejects_uncontrolled_end_effector_chain_without_reference(
     tmp_path: Path,
 ) -> None:
@@ -329,7 +359,11 @@ def test_pink_applies_position_velocity_limits_and_finite_output(
 ) -> None:
     model_path = _write_urdf(tmp_path)
     robot = _robot(model_path).model_copy(
-        update={"joint_limits_lower": [-0.5, -0.25], "joint_limits_upper": [0.5, 0.25]}
+        update={
+            "joint_limits_lower": [-0.5, -0.25],
+            "joint_limits_upper": [0.5, 0.25],
+            "velocity_limits": [0.1, 1.0],
+        }
     )
     backend = create_pink_control_ik(
         PinkControlIKConfig(robot_model=robot, max_velocity=0.2),
@@ -349,11 +383,36 @@ def solve(
     )
 
     assert np.array_equal(solver_inputs["lower_position"][:2], np.array([-0.5, -0.25]))
-    assert np.all(solver_inputs["velocity"][:2] <= 0.2)
+    assert np.array_equal(solver_inputs["velocity"][:2], np.array([0.1, 0.2]))
     assert result.positions.shape == (2,)
     assert np.all(np.isfinite(result.positions))
 
 
+def test_pink_uniformly_scales_solver_velocity_before_integration(
+    tmp_path: Path, monkeypatch: pytest.MonkeyPatch
+) -> None:
+    model_path = _write_urdf(tmp_path)
+    robot = _robot(model_path).model_copy(update={"velocity_limits": [0.1, 1.0]})
+    backend = create_pink_control_ik(PinkControlIKConfig(robot_model=robot, max_velocity=0.2))
+    measured = np.array([0.3, 0.1])
+    calls: list[dict[str, object]] = []
+
+    def solve(
+        configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+    ) -> np.ndarray:
+        calls.append(kwargs)
+        return np.array([1.0, 0.5])
+
+    monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve)
+
+    result = backend.solve(backend.forward_kinematics(measured), measured, 0.01)
+
+    assert np.allclose(result.velocity, [0.1, 0.05])
+    assert np.allclose(result.positions, [0.301, 0.1005])
+    assert calls[0]["limits"] == backend._runtime.limits
+    assert len(backend._runtime.limits) == 1
+
+
 def test_pink_clamps_tiny_position_limit_overshoot(
     tmp_path: Path, monkeypatch: pytest.MonkeyPatch
 ) -> None:
diff --git a/dimos/control/tasks/teleop_task/teleop_task.py b/dimos/control/tasks/teleop_task/teleop_task.py
index c34422fb57..da6c00ba6a 100644
--- a/dimos/control/tasks/teleop_task/teleop_task.py
+++ b/dimos/control/tasks/teleop_task/teleop_task.py
@@ -21,7 +21,7 @@
 
 import numpy as np
 import pinocchio
-from pydantic import FiniteFloat
+from pydantic import Field, FiniteFloat
 
 from dimos.control.coordinator import TaskConfig
 from dimos.control.task import CoordinatorState, JointCommandOutput, ResourceClaim
@@ -43,6 +43,16 @@
 logger = setup_logger()
 
 
+class TeleopControlIKConfig(PinkControlIKConfig):
+    """Pink control policy for engagement-relative arm teleoperation."""
+
+    max_velocity: FiniteFloat = Field(1.0, gt=0.0)
+    position_cost: FiniteFloat = Field(1.0, ge=0.0)
+    orientation_cost: FiniteFloat = Field(1.0, ge=0.0)
+    posture_cost: FiniteFloat = Field(0.0, ge=0.0)
+    damping_cost: FiniteFloat = Field(1e-3, ge=0.0)
+
+
 @dataclass
 class TeleopIKTaskConfig(CartesianIKTaskConfig):
     """Configuration for engagement-relative teleop IK."""
@@ -225,7 +235,7 @@ def clear(self) -> None:
 
 
 class TeleopIKTaskParams(BaseConfig):
-    control_ik: PinkControlIKConfig
+    control_ik: TeleopControlIKConfig
     hand: Literal["left", "right"] | None = None
     timeout: float = 0.5
     max_joint_delta_deg: float = 5.0
diff --git a/dimos/control/tasks/teleop_task/test_teleop_task.py b/dimos/control/tasks/teleop_task/test_teleop_task.py
index ca7110f4e2..683cef4de2 100644
--- a/dimos/control/tasks/teleop_task/test_teleop_task.py
+++ b/dimos/control/tasks/teleop_task/test_teleop_task.py
@@ -296,3 +296,47 @@ def test_factory_requires_pink_configuration_and_matching_model(tmp_path: Path)
     )
     with pytest.raises(ValueError, match="task joints must match"):
         create_task(mismatched, {})
+
+
+def test_factory_applies_balanced_teleop_control_policy(
+    tmp_path: Path, fake_ik: _FakePinkIK
+) -> None:
+    configured = TaskConfig(
+        name="teleop",
+        type="teleop_ik",
+        joint_names=["arm/joint1", "arm/joint2"],
+        params={
+            "control_ik": {"robot_model": _robot(tmp_path / "unused.urdf")},
+            "hand": "right",
+        },
+    )
+
+    task = create_task(configured, {})
+
+    assert task._config.control_ik.max_velocity == 1.0
+    assert task._config.control_ik.position_cost == 1.0
+    assert task._config.control_ik.orientation_cost == 1.0
+    assert task._config.control_ik.posture_cost == 0.0
+    assert task._config.control_ik.damping_cost == 1e-3
+    assert task._config.max_joint_delta_deg == 5.0
+
+
+def test_factory_preserves_explicit_teleop_orientation_cost_override(
+    tmp_path: Path, fake_ik: _FakePinkIK
+) -> None:
+    configured = TaskConfig(
+        name="teleop",
+        type="teleop_ik",
+        joint_names=["arm/joint1", "arm/joint2"],
+        params={
+            "control_ik": {
+                "robot_model": _robot(tmp_path / "unused.urdf"),
+                "orientation_cost": 0.2,
+            },
+            "hand": "right",
+        },
+    )
+
+    task = create_task(configured, {})
+
+    assert task._config.control_ik.orientation_cost == 0.2
diff --git a/dimos/robot/manipulators/xarm/blueprints/teleop.py b/dimos/robot/manipulators/xarm/blueprints/teleop.py
index c760d38d58..84fa632786 100644
--- a/dimos/robot/manipulators/xarm/blueprints/teleop.py
+++ b/dimos/robot/manipulators/xarm/blueprints/teleop.py
@@ -25,6 +25,7 @@
 from dimos.robot.manipulators.common.blueprints import (
     eef_twist_task,
     teleop_ik_task,
+    trajectory_task,
 )
 from dimos.robot.manipulators.common.sim import mujoco_if_sim
 from dimos.robot.manipulators.xarm.config import (
@@ -138,11 +139,21 @@
 )
 
 _xarm7_teleop_hw = xarm7_hardware(
-    "arm", gripper=True, gripper_open_position=0.85, gripper_closed_position=0.0
+    "arm",
+    gripper=True,
+    gripper_open_position=0.85,
+    gripper_closed_position=0.0,
+    mock_without_address=True,
 )
 _xarm6_teleop_hw = xarm6_hardware(
-    "arm", gripper=True, gripper_open_position=0.85, gripper_closed_position=0.0
+    "arm",
+    gripper=True,
+    gripper_open_position=0.85,
+    gripper_closed_position=0.0,
+    mock_without_address=True,
 )
+_xarm7_teleop_model = make_xarm7_model_config(add_gripper=True)
+_xarm6_teleop_model = make_xarm6_model_config(add_gripper=True)
 
 # Dual-input arm: VR (teleop_ik) preempts browser keyboard (eef_twist) via
 # higher priority; when VR is idle the always-active eef_twist holds/drives.
@@ -174,8 +185,16 @@ class _XArm7TeleopCoordinator(ControlCoordinator):
                 priority=10,
                 params=_xarm_eef_params,
             ),
+            trajectory_task(
+                _xarm7_teleop_hw,
+                name=_xarm7_teleop_model.coordinator_task_name,
+            ),
         ],
     ),
+    ManipulationModule.blueprint(
+        robots=[_xarm7_teleop_model],
+        visualization={"backend": "viser"},
+    ),
     *mujoco_if_sim(XARM7_SIM_PATH, len(_xarm7_teleop_hw.joints)),
 )
 
@@ -197,7 +216,15 @@ class _XArm7TeleopCoordinator(ControlCoordinator):
                 priority=10,
                 params=_xarm_eef_params,
             ),
+            trajectory_task(
+                _xarm6_teleop_hw,
+                name=_xarm6_teleop_model.coordinator_task_name,
+            ),
         ],
     ),
+    ManipulationModule.blueprint(
+        robots=[_xarm6_teleop_model],
+        visualization={"backend": "viser"},
+    ),
     *mujoco_if_sim(XARM6_SIM_PATH, len(_xarm6_teleop_hw.joints)),
 )
diff --git a/dimos/robot/manipulators/xarm/config.py b/dimos/robot/manipulators/xarm/config.py
index 5ce262f70b..0906610545 100644
--- a/dimos/robot/manipulators/xarm/config.py
+++ b/dimos/robot/manipulators/xarm/config.py
@@ -62,7 +62,6 @@
     "gripper_joint": make_gripper_joints("arm")[0],
     "gripper_open_pos": 0.85,
     "gripper_closed_pos": 0.0,
-    "max_joint_delta_deg": 50.0,
 }
 XARM7_SIM_HOME = [0.0, -0.247, 0.0, 0.909, 0.0, 1.15644, 0.0]
 
diff --git a/docs/research/interlatent-historical-ik-comparison.md b/docs/research/interlatent-historical-ik-comparison.md
new file mode 100644
index 0000000000..d074b72aa5
--- /dev/null
+++ b/docs/research/interlatent-historical-ik-comparison.md
@@ -0,0 +1,160 @@
+# DimOS Pink teleop IK compared with Interlatent's historical IK
+
+## Scope and evidence boundary
+
+This comparison uses Interlatent's last public teleop implementation before the
+package was removed, plus the public data that remains for its YAM robot. It is
+important not to conflate the two:
+
+- Interlatent's last public IK implementation was an SO-101-specific solver for
+  five arm joints, not the six-DoF YAM solver. The source is preserved at commit
+  [`8695afe`](https://github.com/Interlatent/Interlatent/blob/8695afe552f5bde94b2fea69aac47c79be7beafe/packages/teleop/src/interlatent_teleop/laptop/kinematics.py#L172-L293).
+- Interlatent removed that public teleop package in
+  [commit `347e9d1`](https://github.com/Interlatent/Interlatent/commit/347e9d1e1fbdaede09b51f9a336be149a58423dc).
+- Interlatent later published YAM's URDF and tuning/specification data, including
+  `solver_type: "decoupled_6dof"`, in
+  [commit `97693e6`](https://github.com/Interlatent/Interlatent/blob/97693e635ba3792b81bf86572d0691b8f9d7164f/robots/yam/ik_config.json).
+  It did not publish the implementation of that solver. Its current ADR states
+  explicitly that the SDK ships kinematic data but the IK solver and
+  retargeting stay on the closed platform
+  ([ADR 0012](https://github.com/Interlatent/Interlatent/blob/31a5df280a7a6a53bdbc74aa850a8faac309dfe2/docs/adr/0012-teleop-receiver-stub-open-core-boundary.md)).
+
+Therefore, exact algorithm-to-algorithm comparison is possible only against the
+historical SO-101 solver. The public YAM files can inform a design comparison,
+but not prove the closed solver's equations, step ordering, convergence
+criteria, or failure behavior.
+
+## Algorithm comparison
+
+| Concern | Historical Interlatent SO-101 | Current DimOS |
+|---|---|---|
+| Robot/model scope | Hand-coded elementary-transform chain for SO-101 | URDF-driven Pinocchio model with validated coordinator-to-URDF joint mapping and named end-effector frame |
+| Controlled task | Analytic pan, direct wrist roll, and a numerical solve over lift/elbow/wrist-flex for `[radius, z, pitch]` | Full six-coordinate SE(3) frame task: three translation and three orientation errors |
+| Linearization | Finite-difference 3×3 Jacobian, perturbing each joint by 0.3° | Analytic Pinocchio frame Jacobian and SE(3) log/Jlog through Pink |
+| Solve | Up to 60 damped pseudoinverse iterations per target | One differential-IK QP per coordinator tick, re-anchored to measured joints |
+| Regularization | Adaptive scalar `lambda = 1e-6 * (trace(J J^T) + 1e-12)` | Frame-task Levenberg-Marquardt damping, global QP damping, plus a small joint-velocity damping task |
+| Translation/orientation trade-off | No general trade-off: the residual mixes `r`, `z`, and one pitch angle without weights | Explicit position and orientation task costs; teleop currently sets both to `1.0` |
+| Posture | None | Disabled for teleop (`posture_cost = 0`); no canonical-posture pull |
+| Step/velocity limiting | Uniformly scales each inner Newton update to at most 8° per joint, then runtime independently clips each joint toward the result using profile velocity × control period | Uniformly scales the complete QP velocity vector against `min(model velocity limit, 1.0 rad/s)` and integrates it for the measured tick duration |
+| Position limits | Cylindrical Cartesian target clamp before IK; joint targets clipped downstream | Robot position limits are constraints in the QP, followed by a tolerance-only boundary clamp/check |
+| Failure | A linear-algebra error stops iteration; iteration exhaustion also returns the current partial result silently | Invalid inputs/results and QP failure are explicit errors; the parent task catches them and commands a measured-state hold |
+
+### Historical Interlatent equation
+
+For the three numerically solved joints, Interlatent formed the mixed-unit error
+
+```text
+e = [target_radius - radius(q),
+     target_z      - z(q),
+     target_pitch  - pitch(q)]
+```
+
+and applied the damped right pseudoinverse
+
+```text
+dq = J^T (J J^T + lambda I)^-1 e
+lambda = 1e-6 * (trace(J J^T) + 1e-12)
+```
+
+The implementation then uniformly scaled `dq` when its largest absolute
+component exceeded 8° and accumulated it into the iterative seed. These steps
+are directly visible in
+[`kinematics.py` lines 222–269](https://github.com/Interlatent/Interlatent/blob/8695afe552f5bde94b2fea69aac47c79be7beafe/packages/teleop/src/interlatent_teleop/laptop/kinematics.py#L222-L269).
+That 8° value is an inner iteration bound, not a time-based robot velocity
+limit: a target can consume as many as 60 such iterations before the result is
+sent downstream.
+
+The downstream safety gate first clips the finished joint target to profile
+position bounds. It then independently clips each joint displacement to
+`max_velocity[i] * control_dt`, anchored to the last commanded vector rather
+than the current measured vector
+([`safety.py` lines 147–169](https://github.com/Interlatent/Interlatent/blob/8695afe552f5bde94b2fea69aac47c79be7beafe/packages/teleop/src/interlatent_teleop/pi/safety.py#L147-L169)).
+This gives each joint its full allowed progress but changes the joint-space
+direction whenever only some axes saturate.
+
+### Current DimOS equation and policy
+
+Pink's task objective has the form
+
+```text
+min_Delta_q  1/2 ||J Delta_q + alpha e||_W^2
+```
+
+with the configured task costs forming `W`, plus regularization and position
+constraints. Pink returns `v = Delta_q / dt`; see Pink 4.2's
+[`Task.compute_qp_objective`](https://github.com/stephane-caron/pink/blob/v4.2.0/pink/tasks/task.py)
+and
+[`solve_ik`](https://github.com/stephane-caron/pink/blob/v4.2.0/pink/solve_ik.py).
+
+DimOS updates Pink's configuration from the current measured joint snapshot
+before every solve, sets the full pose target, solves once, uniformly scales
+the returned velocity if any effective joint-speed bound is exceeded, and then
+integrates for the current bounded `dt`. The implementation is in
+[`pink_control_ik.py`](../../dimos/control/tasks/cartesian_ik_task/pink_control_ik.py).
+The teleop policy uses equal numeric position/orientation costs, no posture task,
+a small damping task, and a global 1.0 rad/s ceiling in
+[`teleop_task.py`](../../dimos/control/tasks/teleop_task/teleop_task.py).
+
+Equal numeric costs do not make meters and radians physically identical.
+They mean Pink normalizes each translational coordinate by `1 cost/m` and each
+rotational coordinate by `1 cost/rad`. With a nonredundant six-DoF arm, both can
+normally be tracked together; the relative cost matters when the pose is
+unreachable, constrained, or poorly conditioned.
+
+## Orientation and posture details
+
+The historical SO-101 code did not solve full orientation. Pan was derived from
+target position, roll was passed directly, and pitch was the only orientation
+coordinate in the numerical residual. There is also a source-level mismatch
+worth preserving:
+
+- The retargeter comment says it drops the explicit pitch constraint and relies
+  on a minimum-norm solution
+  ([`retargeting.py` lines 172–179](https://github.com/Interlatent/Interlatent/blob/8695afe552f5bde94b2fea69aac47c79be7beafe/packages/teleop/src/interlatent_teleop/laptop/retargeting.py#L172-L179)).
+- The executed call passes `target_pitch_rad=None`; `ik_jacobian` replaces that
+  with `gripper_pitch(current_joints)` and passes it to the three-residual solve
+  ([`kinematics.py` lines 276–293](https://github.com/Interlatent/Interlatent/blob/8695afe552f5bde94b2fea69aac47c79be7beafe/packages/teleop/src/interlatent_teleop/laptop/kinematics.py#L276-L293)).
+
+The code therefore does preserve the warm-start pitch as an explicit residual,
+despite the comment. Wrist roll remains at the calibration/home value in this
+retargeting path.
+
+For public YAM data, `w_rot: 0.1`, separate position/rotation damping fields,
+and `solver_type: "decoupled_6dof"` strongly suggest a translation-favoring,
+decoupled design. The public data does not establish how the closed solver uses
+those values, so it cannot support a stronger algorithmic claim. The spec also
+contains per-joint `max_dq` values of 0.05 rad for the first three joints and
+0.1 rad for the wrist joints
+([YAM `kinematic_spec.json`](https://github.com/Interlatent/Interlatent/blob/31a5df280a7a6a53bdbc74aa850a8faac309dfe2/packages/sdk/src/interlatent_robots/yam/kinematic_spec.json)).
+
+## Is the current DimOS implementation better?
+
+For the intended six-DoF, multi-robot teleop use case, **yes, structurally**:
+
+- It solves the actual full-pose problem rather than a robot-specific planar
+  reduction.
+- It uses analytic model Jacobians and proper SE(3) orientation error.
+- It treats joint position bounds as part of optimization.
+- It starts every tick from measured state, preventing command-state drift.
+- Its time-based speed cap uniformly preserves the QP's coordinated joint-space
+  direction.
+- It detects solver and numeric failure and holds measured position instead of
+  silently returning a partially converged target.
+
+Interlatent's historical implementation has two useful qualities:
+
+- A simple Cartesian workspace clamp makes obviously unreachable commands
+  benign before solving.
+- Best-effort iteration avoids a hard “no QP solution” event and may feel
+  continuous even when its result is inaccurate.
+
+Those qualities do not make it generally better. Silent nonconvergence, an
+unweighted stopping norm that mixes meters and radians, finite-difference
+Jacobians, partial orientation, and downstream component-wise velocity clipping
+are weaker foundations for general six-DoF control.
+
+The remaining uncertainty is tuning, not the overall architecture. A more
+principled next comparison would instrument full-pose tracking error, joint
+speed saturation, QP failures, and measured-command lag on the same six-DoF
+target traces. The private Interlatent YAM solver cannot be declared better or
+worse without either its code or equivalent trace data.

From f23d1a19c1bc638832b4e9df7c9554051ae81b47 Mon Sep 17 00:00:00 2001
From: cc <cc7@duck.com>
Date: Tue, 28 Jul 2026 00:21:14 -0700
Subject: [PATCH 3/8] spec: remove

---
 ...01-use-pink-control-ik-for-teleop-tasks.md |   3 -
 .../interlatent-historical-ik-comparison.md   | 160 ------------------
 .../.openspec.yaml                            |   2 -
 .../use-pink-control-ik-for-teleop/design.md  |  89 ----------
 .../proposal.md                               |  28 ---
 .../specs/teleop-ik-control/spec.md           | 110 ------------
 .../use-pink-control-ik-for-teleop/tasks.md   |  36 ----
 7 files changed, 428 deletions(-)
 delete mode 100644 docs/adr/0001-use-pink-control-ik-for-teleop-tasks.md
 delete mode 100644 docs/research/interlatent-historical-ik-comparison.md
 delete mode 100644 openspec/changes/use-pink-control-ik-for-teleop/.openspec.yaml
 delete mode 100644 openspec/changes/use-pink-control-ik-for-teleop/design.md
 delete mode 100644 openspec/changes/use-pink-control-ik-for-teleop/proposal.md
 delete mode 100644 openspec/changes/use-pink-control-ik-for-teleop/specs/teleop-ik-control/spec.md
 delete mode 100644 openspec/changes/use-pink-control-ik-for-teleop/tasks.md

diff --git a/docs/adr/0001-use-pink-control-ik-for-teleop-tasks.md b/docs/adr/0001-use-pink-control-ik-for-teleop-tasks.md
deleted file mode 100644
index 18d412aaf8..0000000000
--- a/docs/adr/0001-use-pink-control-ik-for-teleop-tasks.md
+++ /dev/null
@@ -1,3 +0,0 @@
-# Use Pink control IK for teleop tasks
-
-Teleop IK tasks will specialize the Cartesian IK task so every shipped manipulator reuses its measured-state Pink solve and safety pipeline while retaining engage-relative target preparation, E-STOP behavior, and gripper control. The migration is atomic: Piper, xArm6, xArm7, and mixed-arm blueprints will replace the legacy model-path and numeric end-effector-joint configuration with an authoritative `RobotModelConfig`; commands received during E-STOP are discarded, clearing E-STOP requires a fresh engagement baseline, and each model's named end-effector frame is used even where this intentionally changes legacy operator feel. Composition may replace the inheritance seam later if multiple specializations demonstrate a concrete need for a separate control-IK module.
diff --git a/docs/research/interlatent-historical-ik-comparison.md b/docs/research/interlatent-historical-ik-comparison.md
deleted file mode 100644
index d074b72aa5..0000000000
--- a/docs/research/interlatent-historical-ik-comparison.md
+++ /dev/null
@@ -1,160 +0,0 @@
-# DimOS Pink teleop IK compared with Interlatent's historical IK
-
-## Scope and evidence boundary
-
-This comparison uses Interlatent's last public teleop implementation before the
-package was removed, plus the public data that remains for its YAM robot. It is
-important not to conflate the two:
-
-- Interlatent's last public IK implementation was an SO-101-specific solver for
-  five arm joints, not the six-DoF YAM solver. The source is preserved at commit
-  [`8695afe`](https://github.com/Interlatent/Interlatent/blob/8695afe552f5bde94b2fea69aac47c79be7beafe/packages/teleop/src/interlatent_teleop/laptop/kinematics.py#L172-L293).
-- Interlatent removed that public teleop package in
-  [commit `347e9d1`](https://github.com/Interlatent/Interlatent/commit/347e9d1e1fbdaede09b51f9a336be149a58423dc).
-- Interlatent later published YAM's URDF and tuning/specification data, including
-  `solver_type: "decoupled_6dof"`, in
-  [commit `97693e6`](https://github.com/Interlatent/Interlatent/blob/97693e635ba3792b81bf86572d0691b8f9d7164f/robots/yam/ik_config.json).
-  It did not publish the implementation of that solver. Its current ADR states
-  explicitly that the SDK ships kinematic data but the IK solver and
-  retargeting stay on the closed platform
-  ([ADR 0012](https://github.com/Interlatent/Interlatent/blob/31a5df280a7a6a53bdbc74aa850a8faac309dfe2/docs/adr/0012-teleop-receiver-stub-open-core-boundary.md)).
-
-Therefore, exact algorithm-to-algorithm comparison is possible only against the
-historical SO-101 solver. The public YAM files can inform a design comparison,
-but not prove the closed solver's equations, step ordering, convergence
-criteria, or failure behavior.
-
-## Algorithm comparison
-
-| Concern | Historical Interlatent SO-101 | Current DimOS |
-|---|---|---|
-| Robot/model scope | Hand-coded elementary-transform chain for SO-101 | URDF-driven Pinocchio model with validated coordinator-to-URDF joint mapping and named end-effector frame |
-| Controlled task | Analytic pan, direct wrist roll, and a numerical solve over lift/elbow/wrist-flex for `[radius, z, pitch]` | Full six-coordinate SE(3) frame task: three translation and three orientation errors |
-| Linearization | Finite-difference 3×3 Jacobian, perturbing each joint by 0.3° | Analytic Pinocchio frame Jacobian and SE(3) log/Jlog through Pink |
-| Solve | Up to 60 damped pseudoinverse iterations per target | One differential-IK QP per coordinator tick, re-anchored to measured joints |
-| Regularization | Adaptive scalar `lambda = 1e-6 * (trace(J J^T) + 1e-12)` | Frame-task Levenberg-Marquardt damping, global QP damping, plus a small joint-velocity damping task |
-| Translation/orientation trade-off | No general trade-off: the residual mixes `r`, `z`, and one pitch angle without weights | Explicit position and orientation task costs; teleop currently sets both to `1.0` |
-| Posture | None | Disabled for teleop (`posture_cost = 0`); no canonical-posture pull |
-| Step/velocity limiting | Uniformly scales each inner Newton update to at most 8° per joint, then runtime independently clips each joint toward the result using profile velocity × control period | Uniformly scales the complete QP velocity vector against `min(model velocity limit, 1.0 rad/s)` and integrates it for the measured tick duration |
-| Position limits | Cylindrical Cartesian target clamp before IK; joint targets clipped downstream | Robot position limits are constraints in the QP, followed by a tolerance-only boundary clamp/check |
-| Failure | A linear-algebra error stops iteration; iteration exhaustion also returns the current partial result silently | Invalid inputs/results and QP failure are explicit errors; the parent task catches them and commands a measured-state hold |
-
-### Historical Interlatent equation
-
-For the three numerically solved joints, Interlatent formed the mixed-unit error
-
-```text
-e = [target_radius - radius(q),
-     target_z      - z(q),
-     target_pitch  - pitch(q)]
-```
-
-and applied the damped right pseudoinverse
-
-```text
-dq = J^T (J J^T + lambda I)^-1 e
-lambda = 1e-6 * (trace(J J^T) + 1e-12)
-```
-
-The implementation then uniformly scaled `dq` when its largest absolute
-component exceeded 8° and accumulated it into the iterative seed. These steps
-are directly visible in
-[`kinematics.py` lines 222–269](https://github.com/Interlatent/Interlatent/blob/8695afe552f5bde94b2fea69aac47c79be7beafe/packages/teleop/src/interlatent_teleop/laptop/kinematics.py#L222-L269).
-That 8° value is an inner iteration bound, not a time-based robot velocity
-limit: a target can consume as many as 60 such iterations before the result is
-sent downstream.
-
-The downstream safety gate first clips the finished joint target to profile
-position bounds. It then independently clips each joint displacement to
-`max_velocity[i] * control_dt`, anchored to the last commanded vector rather
-than the current measured vector
-([`safety.py` lines 147–169](https://github.com/Interlatent/Interlatent/blob/8695afe552f5bde94b2fea69aac47c79be7beafe/packages/teleop/src/interlatent_teleop/pi/safety.py#L147-L169)).
-This gives each joint its full allowed progress but changes the joint-space
-direction whenever only some axes saturate.
-
-### Current DimOS equation and policy
-
-Pink's task objective has the form
-
-```text
-min_Delta_q  1/2 ||J Delta_q + alpha e||_W^2
-```
-
-with the configured task costs forming `W`, plus regularization and position
-constraints. Pink returns `v = Delta_q / dt`; see Pink 4.2's
-[`Task.compute_qp_objective`](https://github.com/stephane-caron/pink/blob/v4.2.0/pink/tasks/task.py)
-and
-[`solve_ik`](https://github.com/stephane-caron/pink/blob/v4.2.0/pink/solve_ik.py).
-
-DimOS updates Pink's configuration from the current measured joint snapshot
-before every solve, sets the full pose target, solves once, uniformly scales
-the returned velocity if any effective joint-speed bound is exceeded, and then
-integrates for the current bounded `dt`. The implementation is in
-[`pink_control_ik.py`](../../dimos/control/tasks/cartesian_ik_task/pink_control_ik.py).
-The teleop policy uses equal numeric position/orientation costs, no posture task,
-a small damping task, and a global 1.0 rad/s ceiling in
-[`teleop_task.py`](../../dimos/control/tasks/teleop_task/teleop_task.py).
-
-Equal numeric costs do not make meters and radians physically identical.
-They mean Pink normalizes each translational coordinate by `1 cost/m` and each
-rotational coordinate by `1 cost/rad`. With a nonredundant six-DoF arm, both can
-normally be tracked together; the relative cost matters when the pose is
-unreachable, constrained, or poorly conditioned.
-
-## Orientation and posture details
-
-The historical SO-101 code did not solve full orientation. Pan was derived from
-target position, roll was passed directly, and pitch was the only orientation
-coordinate in the numerical residual. There is also a source-level mismatch
-worth preserving:
-
-- The retargeter comment says it drops the explicit pitch constraint and relies
-  on a minimum-norm solution
-  ([`retargeting.py` lines 172–179](https://github.com/Interlatent/Interlatent/blob/8695afe552f5bde94b2fea69aac47c79be7beafe/packages/teleop/src/interlatent_teleop/laptop/retargeting.py#L172-L179)).
-- The executed call passes `target_pitch_rad=None`; `ik_jacobian` replaces that
-  with `gripper_pitch(current_joints)` and passes it to the three-residual solve
-  ([`kinematics.py` lines 276–293](https://github.com/Interlatent/Interlatent/blob/8695afe552f5bde94b2fea69aac47c79be7beafe/packages/teleop/src/interlatent_teleop/laptop/kinematics.py#L276-L293)).
-
-The code therefore does preserve the warm-start pitch as an explicit residual,
-despite the comment. Wrist roll remains at the calibration/home value in this
-retargeting path.
-
-For public YAM data, `w_rot: 0.1`, separate position/rotation damping fields,
-and `solver_type: "decoupled_6dof"` strongly suggest a translation-favoring,
-decoupled design. The public data does not establish how the closed solver uses
-those values, so it cannot support a stronger algorithmic claim. The spec also
-contains per-joint `max_dq` values of 0.05 rad for the first three joints and
-0.1 rad for the wrist joints
-([YAM `kinematic_spec.json`](https://github.com/Interlatent/Interlatent/blob/31a5df280a7a6a53bdbc74aa850a8faac309dfe2/packages/sdk/src/interlatent_robots/yam/kinematic_spec.json)).
-
-## Is the current DimOS implementation better?
-
-For the intended six-DoF, multi-robot teleop use case, **yes, structurally**:
-
-- It solves the actual full-pose problem rather than a robot-specific planar
-  reduction.
-- It uses analytic model Jacobians and proper SE(3) orientation error.
-- It treats joint position bounds as part of optimization.
-- It starts every tick from measured state, preventing command-state drift.
-- Its time-based speed cap uniformly preserves the QP's coordinated joint-space
-  direction.
-- It detects solver and numeric failure and holds measured position instead of
-  silently returning a partially converged target.
-
-Interlatent's historical implementation has two useful qualities:
-
-- A simple Cartesian workspace clamp makes obviously unreachable commands
-  benign before solving.
-- Best-effort iteration avoids a hard “no QP solution” event and may feel
-  continuous even when its result is inaccurate.
-
-Those qualities do not make it generally better. Silent nonconvergence, an
-unweighted stopping norm that mixes meters and radians, finite-difference
-Jacobians, partial orientation, and downstream component-wise velocity clipping
-are weaker foundations for general six-DoF control.
-
-The remaining uncertainty is tuning, not the overall architecture. A more
-principled next comparison would instrument full-pose tracking error, joint
-speed saturation, QP failures, and measured-command lag on the same six-DoF
-target traces. The private Interlatent YAM solver cannot be declared better or
-worse without either its code or equivalent trace data.
diff --git a/openspec/changes/use-pink-control-ik-for-teleop/.openspec.yaml b/openspec/changes/use-pink-control-ik-for-teleop/.openspec.yaml
deleted file mode 100644
index e8209ffaac..0000000000
--- a/openspec/changes/use-pink-control-ik-for-teleop/.openspec.yaml
+++ /dev/null
@@ -1,2 +0,0 @@
-schema: spec-driven
-created: 2026-07-28
diff --git a/openspec/changes/use-pink-control-ik-for-teleop/design.md b/openspec/changes/use-pink-control-ik-for-teleop/design.md
deleted file mode 100644
index e17e78f0a1..0000000000
--- a/openspec/changes/use-pink-control-ik-for-teleop/design.md
+++ /dev/null
@@ -1,89 +0,0 @@
-## Context
-
-`TeleopIKTask` currently duplicates Cartesian task concerns around measured joint access, forward kinematics, inverse kinematics, output construction, and joint-delta safety. It constructs a legacy iterative `PinocchioIK` solver from a model file and numeric joint ID, while `CartesianIKTask` and `EEFTwistTask` use `PinkControlIK` configured by the authoritative `RobotModelConfig`. `EEFTwistTask` already demonstrates a target-source specialization of `CartesianIKTask`.
-
-The teleop task must continue interpreting Quest/hosted-controller messages as pose deltas relative to the robot pose at engagement, must remain a declarative coordinator task, and must retain gripper and E-STOP behavior. The migration affects Piper, xArm6, xArm7, and their mixed-arm composition.
-
-## Goals / Non-Goals
-
-**Goals:**
-
-- Give every shipped teleop IK task the same measured-state Pink solve and safety pipeline as Cartesian IK.
-- Preserve engage-relative pose semantics, gripper behavior, task-name routing, arbitration, and preemption.
-- Make `RobotModelConfig` the sole authority for controlled joints and the named end-effector frame.
-- Fail closed across E-STOP and runtime solver failures.
-- Migrate every in-repository teleop IK configuration atomically.
-
-**Non-Goals:**
-
-- Refactor Cartesian, twist, and teleop tasks around a new composed control engine.
-- Change controller-side delta generation, task registry routing, coordinator arbitration, or gripper units.
-- Add collision avoidance to the real-time Pink control task.
-- Preserve the legacy model-path and numeric end-effector-joint configuration.
-- Tune final hardware-specific control gains without simulation and hardware evidence.
-
-## Decisions
-
-### Teleop is a focused Cartesian IK specialization
-
-`TeleopIKTaskConfig` will extend `CartesianIKTaskConfig` with hand and gripper fields, and `TeleopIKTask` will extend `CartesianIKTask`. The parent will own joint/model validation, Pink construction, measured-state reads, bounded coordinator `dt`, IK execution, result validation, joint-delta checks, measured-state holds, timeout bookkeeping, and the arm resource claim.
-
-The child will own engagement-baseline state, delta-to-absolute target preparation, E-STOP gating, controller buttons, gripper interpolation, the extended gripper claim, and appending the gripper command to the parent output.
-
-Composition was considered because repository guidance generally prefers it. It is deferred because the current parent already exposes target-preparation and timeout hooks, `EEFTwistTask` proves the specialization pattern, and extracting a new engine would broaden this safety migration. A composed module can replace the inheritance seam later if concrete strain appears across multiple specializations.
-
-### The child prepares an absolute target from an engagement-relative delta
-
-On the first compute after a new engaged delta arrives, the task will capture forward kinematics from the current measured coordinator joints as the engagement baseline. It will preserve the established transform:
-
-- target translation = baseline translation + delta translation
-- target rotation = delta rotation × baseline rotation
-
-The resulting normalized, finite `SE3` target is passed through the parent's Pink compute pipeline. Disengage, timeout, stop, clear, or E-STOP discards the baseline so the next engagement starts from the then-current measured pose.
-
-### E-STOP discards rather than defers commands
-
-Latching E-STOP will make the task inert and clear the pose target, engagement baseline, and transient engagement state. Pose and gripper commands received while latched will return rejection without changing cached state. Clearing E-STOP will not restore any prior command; a fresh post-clear engagement and baseline are required.
-
-This avoids replaying in-flight or stale commands after a safety discontinuity.
-
-### RobotModelConfig is the only model authority
-
-The teleop factory and blueprint helper will accept the same nested Pink control configuration used by Cartesian and EEF-twist tasks. `model_path` and `ee_joint_id` will be removed from teleop parameters without a compatibility branch.
-
-Pink will target `RobotModelConfig.end_effector_link` and map the ordered coordinator joints through `joint_name_mapping`. Mixed-arm blueprints will construct robot models whose names match their hardware namespaces. Piper will intentionally move from legacy joint 6 to the model's `gripper_base` frame.
-
-### Existing declarative routing remains stable
-
-The task type remains `teleop_ik`. Its task card continues consuming task-name-routed Cartesian commands and broadcast teleop buttons. No coordinator stream or transport changes are required. The blueprint helper changes only how task parameters are assembled.
-
-### Verification is behavior-focused
-
-Task tests will exercise behavior through the task interface with a surgical fake Pink backend. They will cover exact delta composition, measured-state baseline capture and reseeding, bounded `dt`, valid output, measured holds, joint-delta rejection, E-STOP rejection, timeout, and gripper claim/output. Blueprint tests will verify that every shipped teleop task carries an authoritative reconstructable Pink configuration and no legacy model fields.
-
-Simulation smoke tests will validate named frames and operator motion before real Piper or xArm hardware rollout.
-
-## Risks / Trade-offs
-
-- **Piper's controlled point changes from joint 6 to `gripper_base`** → Treat this as intentional, validate translation and rotation behavior in simulation, then perform a low-speed hardware check.
-- **Pink's one-step differential response differs from the legacy multi-iteration solver** → Start with conservative existing Pink limits, retain the outer joint-delta guard, and tune gains only from measured simulation/hardware behavior.
-- **Inheritance couples teleop to protected Cartesian task state** → Keep overrides limited to documented hooks and teleop policy; defer a composition refactor until concrete additional variation justifies a new seam.
-- **Pink becomes mandatory for teleop task construction** → Keep imports actionable when the manipulation extra is absent and cover the failure path in tests.
-- **Atomic configuration removal is breaking** → Update all known in-repository callers and blueprint assertions in the same change so no shipped mixed solver configuration remains.
-- **Pose and button streams can arrive concurrently** → Guard all task-owned transient state consistently and test E-STOP/engagement transitions rather than relying on stream ordering.
-
-## Migration Plan
-
-1. Refactor the teleop configuration and class onto the Cartesian/Pink pipeline while preserving its registry type and streams.
-2. Change the shared teleop blueprint helper to resolve `RobotModelConfig` into Pink control configuration.
-3. Update Piper, xArm6, xArm7, and mixed-arm call sites atomically, including hardware-namespace-specific robot models.
-4. Replace legacy teleop solver tests with behavior tests at the task interface and extend blueprint configuration coverage.
-5. Update manipulation documentation and remove teleop references to model paths or numeric end-effector joint IDs.
-6. Run focused unit and blueprint tests, then the relevant broader test suite and type/style checks.
-7. Exercise each shipped teleop blueprint in simulation; perform low-speed hardware validation after simulation succeeds.
-
-Rollback is a source-level revert of the task, helper, call-site, test, and documentation changes as one unit. There is no persisted data migration.
-
-## Open Questions
-
-None. Hardware-specific Pink gains remain rollout tuning rather than an unresolved architecture decision.
diff --git a/openspec/changes/use-pink-control-ik-for-teleop/proposal.md b/openspec/changes/use-pink-control-ik-for-teleop/proposal.md
deleted file mode 100644
index e6b201515e..0000000000
--- a/openspec/changes/use-pink-control-ik-for-teleop/proposal.md
+++ /dev/null
@@ -1,28 +0,0 @@
-## Why
-
-Declarative teleop IK still uses a separate legacy Pinocchio solver and identifies robot models with file paths and numeric end-effector joint IDs. Moving teleop onto the measured-state Pink control pipeline gives Cartesian, twist, and teleop control one model authority and one safety-critical IK implementation.
-
-## What Changes
-
-- Make the teleop IK task a focused specialization of the Cartesian IK task, reusing Pink solving, measured-state anchoring, bounded tick timing, output validation, joint-limit handling, and measured-state holds.
-- Preserve teleop-specific engage-relative pose interpretation, controller-button behavior, gripper control, task-name routing, arbitration, and preemption.
-- Discard cached targets and engagement baselines on disengage, timeout, stop, clear, or E-STOP; reject pose and gripper commands while E-STOP is latched.
-- Use each authoritative `RobotModelConfig` named end-effector frame, including Piper's `gripper_base`.
-- Migrate Piper, xArm6, xArm7, and mixed xArm/Piper teleop blueprints together.
-- **BREAKING**: Replace teleop IK's `model_path` and numeric `ee_joint_id` parameters with `RobotModelConfig`-backed Pink control configuration; the legacy configuration is not supported concurrently.
-
-## Capabilities
-
-### New Capabilities
-
-- `teleop-ik-control`: Engage-relative manipulator teleoperation through measured-state Pink control IK, including model authority, safety, E-STOP, gripper, and lifecycle behavior.
-
-### Modified Capabilities
-
-None.
-
-## Impact
-
-- Affects the teleop control task and factory, shared manipulator blueprint helpers, Piper and xArm teleop blueprints, the mixed-manipulator coordinator, and their tests and manipulation documentation.
-- Makes the existing optional Pink/manipulation dependencies required when constructing a teleop IK task.
-- Intentionally changes Piper's controlled frame from legacy joint 6 to the model's named `gripper_base` frame and may require simulation and hardware tuning.
diff --git a/openspec/changes/use-pink-control-ik-for-teleop/specs/teleop-ik-control/spec.md b/openspec/changes/use-pink-control-ik-for-teleop/specs/teleop-ik-control/spec.md
deleted file mode 100644
index 5696d2d630..0000000000
--- a/openspec/changes/use-pink-control-ik-for-teleop/specs/teleop-ik-control/spec.md
+++ /dev/null
@@ -1,110 +0,0 @@
-## ADDED Requirements
-
-### Requirement: Authoritative teleop robot model
-The system SHALL configure every teleop IK task with Pink control IK backed by an authoritative `RobotModelConfig`. The task's ordered controlled joints SHALL match the model's ordered coordinator joint names, and the target frame SHALL be the model's named end-effector frame.
-
-#### Scenario: Construct teleop for a named robot model
-- **WHEN** a teleop IK task is constructed with a valid robot model whose coordinator joints match the task joints
-- **THEN** the task uses Pink control IK and targets the model's named end-effector frame
-
-#### Scenario: Reject mismatched controlled joints
-- **WHEN** a teleop IK task's ordered joints do not match the robot model's ordered coordinator joints
-- **THEN** task construction fails with an actionable configuration error
-
-#### Scenario: Reject legacy model parameters
-- **WHEN** a teleop IK task is configured only with a model path and numeric end-effector joint ID
-- **THEN** task construction rejects the legacy configuration instead of selecting the legacy solver
-
-### Requirement: Engagement-relative Cartesian target
-The system SHALL interpret a teleop pose command as an end-effector delta relative to an engagement baseline captured by forward kinematics from the current measured coordinator joints. Translation SHALL be added to the baseline translation, and delta rotation SHALL left-multiply the baseline rotation.
-
-#### Scenario: First command captures the measured baseline
-- **WHEN** the first pose delta of an engagement is computed with a complete measured joint state
-- **THEN** the task captures the corresponding measured end-effector pose as its engagement baseline and solves for the composed absolute target
-
-#### Scenario: Subsequent commands retain the engagement baseline
-- **WHEN** additional pose deltas arrive during the same engagement while measured joints change
-- **THEN** each target is composed from the same engagement baseline rather than recapturing it from the latest joint state
-
-#### Scenario: Missing measured joints defer baseline capture
-- **WHEN** the task receives an engaged pose delta but the coordinator state lacks any controlled joint position
-- **THEN** the task emits no joint command and does not capture a partial engagement baseline
-
-### Requirement: Measured-state Pink control safety
-For each active tick, the system SHALL seed Pink control IK from the current finite measured joint state, bound the coordinator timestep to configured limits, validate solver output, and enforce the configured per-tick joint-delta limit. An expected target-preparation or IK runtime failure after a complete measured state is available SHALL produce a measured-state servo-position hold.
-
-#### Scenario: Valid target produces an arbitrated joint command
-- **WHEN** Pink returns a finite, correctly shaped solution within configured limits
-- **THEN** the task emits a servo-position command for the controlled arm joints
-
-#### Scenario: Coordinator timestep is bounded
-- **WHEN** the coordinator timestep is outside the configured positive minimum and maximum
-- **THEN** the task uses the nearest configured bound for target preparation and Pink solving
-
-#### Scenario: Solver failure holds measured state
-- **WHEN** target preparation or Pink solving raises an expected runtime failure after measured joints are available
-- **THEN** the task emits a servo-position hold at the measured arm positions
-
-#### Scenario: Invalid or excessive solution holds measured state
-- **WHEN** Pink returns a non-finite, incorrectly shaped, or excessive joint-delta solution
-- **THEN** the task rejects the candidate and emits a servo-position hold at the measured arm positions
-
-### Requirement: Fresh baseline after lifecycle discontinuity
-The system SHALL discard the current pose target and engagement baseline on disengage, timeout, stop, clear, or E-STOP. The next accepted engagement SHALL capture a fresh baseline from the then-current measured robot state.
-
-#### Scenario: Disengage and re-engage
-- **WHEN** an operator disengages after commanding motion and later re-engages
-- **THEN** the next computed delta is based on a newly measured engagement baseline
-
-#### Scenario: Command stream times out
-- **WHEN** no teleop pose update arrives within the configured nonzero timeout
-- **THEN** the task becomes inactive, discards its target and baseline, and emits no command until a new engagement
-
-#### Scenario: Task is stopped or cleared
-- **WHEN** the task is stopped or cleared
-- **THEN** its target and engagement baseline are discarded and it no longer participates in arbitration
-
-### Requirement: Fail-closed E-STOP behavior
-The system SHALL make a teleop IK task inert while E-STOP is latched, SHALL reject pose and gripper commands received while latched, and SHALL NOT replay pre-latch or latched commands when E-STOP is cleared.
-
-#### Scenario: E-STOP while teleop is active
-- **WHEN** E-STOP is latched during an active engagement
-- **THEN** the task immediately clears its target and baseline and becomes inactive
-
-#### Scenario: Commands arrive during E-STOP
-- **WHEN** pose or gripper commands arrive while E-STOP is latched
-- **THEN** the commands are rejected without changing the cached target, baseline, or gripper target
-
-#### Scenario: E-STOP is cleared
-- **WHEN** E-STOP is cleared after one or more commands were rejected
-- **THEN** the task remains free of replayable commands and requires a fresh engagement baseline before arm motion resumes
-
-### Requirement: Teleop gripper and arbitration behavior
-The system SHALL preserve teleop gripper interpolation, resource claims, task-name-routed pose delivery, broadcast controller-button delivery, and joint-level arbitration while using Pink control IK.
-
-#### Scenario: Analog trigger commands the gripper
-- **WHEN** a configured hand supplies an analog trigger value from zero through one
-- **THEN** the task clamps the value, interpolates between configured open and closed positions, and appends the gripper target to an active arm output
-
-#### Scenario: Task claims the gripper
-- **WHEN** a teleop task is configured with a gripper joint
-- **THEN** its resource claim includes both the controlled arm joints and the gripper joint at the task priority
-
-#### Scenario: Cartesian command is routed by task name
-- **WHEN** a pose delta names a registered teleop task
-- **THEN** the coordinator delivers it only to that named task
-
-#### Scenario: Higher-priority task wins arbitration
-- **WHEN** a higher-priority task claims any of the same joints as an active teleop task
-- **THEN** coordinator arbitration gives those joints to the higher-priority task and reports preemption to teleop
-
-### Requirement: Atomic shipped-blueprint migration
-The system SHALL configure every shipped Piper, xArm6, xArm7, and mixed xArm/Piper teleop IK task through the authoritative Pink robot-model interface, with no shipped teleop task retaining legacy model-path or numeric end-effector-joint parameters.
-
-#### Scenario: Inspect shipped teleop task configurations
-- **WHEN** the shipped teleop blueprints are resolved
-- **THEN** each teleop IK task contains a reconstructable Pink control configuration whose robot-model coordinator joints match its hardware joints
-
-#### Scenario: Resolve mixed-arm teleop
-- **WHEN** the mixed xArm/Piper teleop coordinator is resolved
-- **THEN** each teleop task uses a robot model mapped to its own hardware namespace
diff --git a/openspec/changes/use-pink-control-ik-for-teleop/tasks.md b/openspec/changes/use-pink-control-ik-for-teleop/tasks.md
deleted file mode 100644
index f4300f6b99..0000000000
--- a/openspec/changes/use-pink-control-ik-for-teleop/tasks.md
+++ /dev/null
@@ -1,36 +0,0 @@
-## 1. Teleop Pink Task
-
-- [x] 1.1 Add focused teleop task tests with a surgical fake Pink backend covering exact engagement-relative translation/rotation composition, first-tick measured FK capture, retained baseline, missing joint state, bounded `dt`, valid output, measured-state holds, and joint-delta rejection.
-- [x] 1.2 Refactor `TeleopIKTaskConfig` and `TeleopIKTask` to specialize the Cartesian IK task and remove direct construction or use of legacy `PinocchioIK`, `model_path`, and `ee_joint_id`.
-- [x] 1.3 Implement and test lifecycle baseline resets for disengage, timeout, stop, and clear while preserving task activation and preemption behavior.
-- [x] 1.4 Implement and test fail-closed E-STOP handling that clears transient state, rejects pose and gripper commands while latched, and prevents command replay after clear.
-- [x] 1.5 Preserve and test analog gripper interpolation, combined arm/gripper resource claims, and appending gripper positions to both valid Pink outputs and measured-state holds.
-- [x] 1.6 Update teleop parameter validation and factory tests so authoritative Pink configuration is required, joint/model mismatches fail actionably, and missing optional Pink dependencies report the manipulation-extra installation path.
-
-## 2. Atomic Blueprint Migration
-
-- [x] 2.1 Change the shared `teleop_ik_task` blueprint helper to accept `RobotModelConfig`, resolve the nested Pink control configuration, and reject the removed legacy model parameters.
-- [x] 2.2 Migrate Piper teleop to its authoritative robot model and named `gripper_base` end-effector frame while preserving hand selection, routing, priority, and gripper units.
-- [x] 2.3 Migrate xArm6 and xArm7 teleop tasks to their authoritative no-gripper control models while preserving the lower-priority EEF-twist fallback and gripper behavior.
-- [x] 2.4 Migrate mixed xArm/Piper teleop to per-hardware-namespace robot models whose ordered coordinator joints match each hardware task.
-- [x] 2.5 Remove teleop-only legacy FK-model constants and imports that become unused after all call sites migrate.
-
-## 3. Routing and Configuration Verification
-
-- [x] 3.1 Extend blueprint tests to verify every shipped `teleop_ik` task has a reconstructable `PinkControlIKConfig`, matching hardware/model joints, the expected named end-effector frame, and no `model_path` or `ee_joint_id`.
-- [x] 3.2 Preserve and verify registry-card behavior for task-name-routed Cartesian pose deltas and broadcast teleop buttons.
-- [x] 3.3 Verify coordinator arbitration and preemption behavior remains unchanged for teleop versus lower-priority EEF-twist and higher-priority overlapping tasks.
-
-## 4. Documentation and Validation
-
-- [x] 4.1 Update manipulation and custom-arm documentation to describe Pink-based teleop configuration through `RobotModelConfig`, engagement-relative targets, named end-effector frames, and the removal of numeric joint IDs.
-- [x] 4.2 Run the focused teleop, Cartesian IK, EEF-twist, coordinator-routing, and manipulator-blueprint pytest suites and resolve failures.
-- [x] 4.3 Run repository formatting, lint, type checks, and `git diff --check` for all changed control and blueprint files.
-- [x] 4.4 Smoke-test Piper, xArm6, and xArm7 teleop in simulation at conservative settings, verifying fresh-baseline engagement, translation/rotation direction, E-STOP recovery, and gripper operation; record any hardware gain tuning as rollout follow-up rather than changing architecture.
-
-  Piper, xArm6, and xArm7 each passed the daemon startup health check with the
-  Quest server, MuJoCo adapter, Pink task, command routing, and gripper channel
-  active. Deterministic task tests verified fresh measured-state baselines,
-  translation/rotation composition, E-STOP clear without replay, and gripper
-  interpolation/output. No gain changes were needed; hardware-specific tuning
-  remains a rollout follow-up.

From 73bb0f415fa965e06ec097edf43ac8fa2159e7eb Mon Sep 17 00:00:00 2001
From: cc <cc7@duck.com>
Date: Tue, 4 Aug 2026 11:19:22 -0700
Subject: [PATCH 4/8] update

---
 .../cartesian_ik_task/test_pink_control_ik.py |  2 +-
 .../tasks/teleop_task/test_teleop_task.py     |  2 +-
 dimos/robot/all_blueprints.py                 |  2 +
 .../manipulators/a1z/blueprints/teleop.py     | 38 ++++++++++++++++++-
 .../manipulators/xarm/blueprints/teleop.py    | 10 +----
 dimos/teleop/quest/blueprints.py              |  8 ++++
 6 files changed, 51 insertions(+), 11 deletions(-)

diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py
index 5e1808770a..eb2bd2ab6d 100644
--- a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py
+++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py
@@ -225,7 +225,7 @@ def solve(
     assert kwargs["damping"] == 1e-4
     assert kwargs["eps"] == 1e-6
     assert isinstance(kwargs["limits"], list)
-    assert len(kwargs["limits"]) == 2
+    assert len(kwargs["limits"]) == 1
 
 
 def test_pink_solver_dependency_failure_is_translated_to_runtime_error(
diff --git a/dimos/control/tasks/teleop_task/test_teleop_task.py b/dimos/control/tasks/teleop_task/test_teleop_task.py
index 683cef4de2..a779afa877 100644
--- a/dimos/control/tasks/teleop_task/test_teleop_task.py
+++ b/dimos/control/tasks/teleop_task/test_teleop_task.py
@@ -125,7 +125,7 @@ def _delta(
 def fake_ik(mocker: MockerFixture) -> _FakePinkIK:
     backend = _FakePinkIK()
     mocker.patch(
-        "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.PinkControlIK",
+        "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik",
         return_value=backend,
     )
     return backend
diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py
index ecf75ac543..c67b10b549 100644
--- a/dimos/robot/all_blueprints.py
+++ b/dimos/robot/all_blueprints.py
@@ -39,6 +39,7 @@
     "coordinator-piper": "dimos.robot.manipulators.piper.blueprints.basic:coordinator_piper",
     "coordinator-piper-xarm": "dimos.robot.manipulators.common.mixed:coordinator_piper_xarm",
     "coordinator-servo-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_servo_xarm6",
+    "coordinator-teleop-a1z": "dimos.robot.manipulators.a1z.blueprints.teleop:coordinator_teleop_a1z",
     "coordinator-teleop-dual": "dimos.robot.manipulators.common.mixed:coordinator_teleop_dual",
     "coordinator-teleop-piper": "dimos.robot.manipulators.piper.blueprints.teleop:coordinator_teleop_piper",
     "coordinator-teleop-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_teleop_xarm6",
@@ -97,6 +98,7 @@
     "teleop-phone": "dimos.teleop.phone.blueprints:teleop_phone",
     "teleop-phone-go2": "dimos.teleop.phone.blueprints:teleop_phone_go2",
     "teleop-phone-go2-fleet": "dimos.teleop.phone.blueprints:teleop_phone_go2_fleet",
+    "teleop-quest-a1z": "dimos.teleop.quest.blueprints:teleop_quest_a1z",
     "teleop-quest-dual": "dimos.teleop.quest.blueprints:teleop_quest_dual",
     "teleop-quest-go2": "dimos.teleop.quest.blueprints:teleop_quest_go2",
     "teleop-quest-piper": "dimos.teleop.quest.blueprints:teleop_quest_piper",
diff --git a/dimos/robot/manipulators/a1z/blueprints/teleop.py b/dimos/robot/manipulators/a1z/blueprints/teleop.py
index 6207746b64..31f6d620cc 100644
--- a/dimos/robot/manipulators/a1z/blueprints/teleop.py
+++ b/dimos/robot/manipulators/a1z/blueprints/teleop.py
@@ -16,6 +16,8 @@
 
 from __future__ import annotations
 
+from dataclasses import replace
+
 from dimos.control.coordinator import ControlCoordinator, TaskConfig
 from dimos.core.coordination.blueprints import autoconnect
 from dimos.manipulation.manipulation_module import ManipulationModule
@@ -23,7 +25,11 @@
     a1z_hardware,
     make_a1z_model_config,
 )
-from dimos.robot.manipulators.common.blueprints import eef_twist_task, trajectory_task
+from dimos.robot.manipulators.common.blueprints import (
+    eef_twist_task,
+    teleop_ik_task,
+    trajectory_task,
+)
 from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule
 
 _a1z_keyboard_hw = a1z_hardware("arm")
@@ -53,3 +59,33 @@
         visualization={"backend": "viser"},
     ),
 )
+
+
+_a1z_quest_hw = replace(
+    a1z_hardware("arm"),
+    adapter_type="mock",
+    address=None,
+    adapter_kwargs={},
+)
+_a1z_quest_model = make_a1z_model_config()
+
+coordinator_teleop_a1z = autoconnect(
+    ControlCoordinator.blueprint(
+        hardware=[_a1z_quest_hw],
+        tasks=[
+            teleop_ik_task(
+                _a1z_quest_hw,
+                hand="left",
+                name="teleop_a1z",
+                robot_model=_a1z_quest_model,
+                control_ik={"max_velocity": 2.0},
+                priority=20,
+            ),
+            trajectory_task(_a1z_quest_hw),
+        ],
+    ),
+    ManipulationModule.blueprint(
+        robots=[_a1z_quest_model],
+        visualization={"backend": "viser"},
+    ),
+)
diff --git a/dimos/robot/manipulators/xarm/blueprints/teleop.py b/dimos/robot/manipulators/xarm/blueprints/teleop.py
index 84fa632786..57ae65de3e 100644
--- a/dimos/robot/manipulators/xarm/blueprints/teleop.py
+++ b/dimos/robot/manipulators/xarm/blueprints/teleop.py
@@ -185,10 +185,7 @@ class _XArm7TeleopCoordinator(ControlCoordinator):
                 priority=10,
                 params=_xarm_eef_params,
             ),
-            trajectory_task(
-                _xarm7_teleop_hw,
-                name=_xarm7_teleop_model.coordinator_task_name,
-            ),
+            trajectory_task(_xarm7_teleop_hw),
         ],
     ),
     ManipulationModule.blueprint(
@@ -216,10 +213,7 @@ class _XArm7TeleopCoordinator(ControlCoordinator):
                 priority=10,
                 params=_xarm_eef_params,
             ),
-            trajectory_task(
-                _xarm6_teleop_hw,
-                name=_xarm6_teleop_model.coordinator_task_name,
-            ),
+            trajectory_task(_xarm6_teleop_hw),
         ],
     ),
     ManipulationModule.blueprint(
diff --git a/dimos/teleop/quest/blueprints.py b/dimos/teleop/quest/blueprints.py
index 0b6644960a..d85b9f99b5 100644
--- a/dimos/teleop/quest/blueprints.py
+++ b/dimos/teleop/quest/blueprints.py
@@ -25,6 +25,7 @@
 from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
 from dimos.msgs.geometry_msgs.Twist import Twist
 from dimos.msgs.sensor_msgs.Image import Image
+from dimos.robot.manipulators.a1z.blueprints.teleop import coordinator_teleop_a1z
 from dimos.robot.manipulators.common.mixed import coordinator_teleop_dual
 from dimos.robot.manipulators.piper.blueprints.teleop import coordinator_teleop_piper
 from dimos.robot.manipulators.xarm.blueprints.teleop import (
@@ -82,6 +83,13 @@
 ).remappings([(ArmTeleopModule, "left_controller_output", "coordinator_cartesian_command")])
 
 
+# A1Z mock teleop: left controller -> A1Z arm
+teleop_quest_a1z = autoconnect(
+    ArmTeleopModule.blueprint(task_names={"left": "teleop_a1z"}),
+    coordinator_teleop_a1z,
+).remappings([(ArmTeleopModule, "left_controller_output", "coordinator_cartesian_command")])
+
+
 # XArm6 teleop (sim with --simulation, real otherwise): right controller -> xarm6
 teleop_quest_xarm6 = autoconnect(
     ArmTeleopModule.blueprint(task_names={"right": "teleop_xarm"}),

From e0ecd56da8a48eef817bf04c426ef2d663908e97 Mon Sep 17 00:00:00 2001
From: cc <cc7@duck.com>
Date: Tue, 4 Aug 2026 13:40:51 -0700
Subject: [PATCH 5/8] Improve A1Z teleop control

---
 .../cartesian_ik_task/cartesian_ik_task.py    | 37 ++++++--
 .../cartesian_ik_task/pink_control_ik.py      | 84 +++++++++++++----
 .../cartesian_ik_task/test_pink_control_ik.py | 90 ++++++++++++++++---
 .../tasks/eef_twist_task/eef_twist_task.py    | 39 +++++++-
 .../eef_twist_task/test_eef_twist_task.py     | 39 ++++++--
 .../control/tasks/teleop_task/teleop_task.py  | 35 +++++++-
 .../tasks/teleop_task/test_teleop_task.py     | 42 ++++++++-
 .../manipulators/galaxea_a1z/adapter.py       |  9 +-
 .../manipulators/galaxea_a1z/config.py        | 43 +++++++++
 .../manipulators/galaxea_a1z/test_adapter.py  | 34 ++++++-
 .../manipulators/a1z/blueprints/teleop.py     | 14 ++-
 .../a1z/blueprints/test_teleop.py             | 81 +++++++++++++++++
 dimos/robot/manipulators/a1z/config.py        |  6 +-
 dimos/teleop/quest/README.md                  |  7 ++
 docs/capabilities/manipulation/a1z.md         | 16 ++--
 stubs/a1z/robots/get_robot.pyi                |  6 ++
 16 files changed, 514 insertions(+), 68 deletions(-)
 create mode 100644 dimos/robot/manipulators/a1z/blueprints/test_teleop.py

diff --git a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py
index fbaa8e7360..686d7ac8d8 100644
--- a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py
+++ b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py
@@ -93,8 +93,9 @@ class CartesianIKTask(BaseControlTask):
     """Cartesian control task with Pink differential IK.
 
     Accepts streaming cartesian poses via on_cartesian_command() and computes IK
-    internally to output joint commands. Pink re-anchors each solve to the
-    current joint state from CoordinatorState.
+    internally to output joint commands. By default, Pink re-anchors each solve
+    to the current joint state from CoordinatorState. Specializations can retain
+    a validated command as the next solve seed.
 
     Unlike CartesianServoTask (which bypasses joint arbitration), this task
     outputs JointCommandOutput and participates in joint-level arbitration.
@@ -186,7 +187,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
             state: Current coordinator state (contains measured joint positions)
 
         Returns:
-            JointCommandOutput with positions or a measured-state hold after an
+            JointCommandOutput with positions or a solve-state hold after an
             expected runtime failure; None if inactive or timed out.
         """
         with self._lock:
@@ -205,13 +206,17 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
                     self._on_timeout()
                     return None
 
-        q_current = self._get_current_joints(state)
-        if q_current is None:
+        q_measured = self._get_current_joints(state)
+        if q_measured 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)):
+        if not np.all(np.isfinite(q_measured)):
             logger.error("CartesianIKTask %s: measured joint state is non-finite", self._name)
             return None
+        q_current = self._select_solve_joints(state, q_measured)
+        if q_current.shape != q_measured.shape or not np.all(np.isfinite(q_current)):
+            logger.error("CartesianIKTask %s: solve joint state is invalid", self._name)
+            return self._hold(q_measured)
         raw_dt = state.dt
         if not np.isfinite(raw_dt) or raw_dt <= 0.0:
             return self._hold(q_current)
@@ -245,6 +250,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
             )
             return self._hold(q_current)
 
+        self._on_solution_accepted(state, q_solution)
         return JointCommandOutput(
             joint_names=self._joint_names_list,
             positions=q_solution.flatten().tolist(),
@@ -252,7 +258,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
         )
 
     def _hold(self, q_current: NDArray[np.float64]) -> JointCommandOutput:
-        """Keep the measured configuration under the task's servo contract."""
+        """Keep the selected solve configuration under the task's servo contract."""
         return JointCommandOutput(
             joint_names=self._joint_names_list,
             positions=q_current.tolist(),
@@ -269,13 +275,28 @@ def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.float64] |
             positions.append(pos)
         return np.array(positions, dtype=np.float64)
 
+    def _select_solve_joints(
+        self,
+        state: CoordinatorState,
+        q_measured: NDArray[np.float64],
+    ) -> NDArray[np.float64]:
+        """Select the joint state used to warm-start this tick's IK solve."""
+        return q_measured
+
+    def _on_solution_accepted(
+        self,
+        state: CoordinatorState,
+        q_solution: NDArray[np.float64],
+    ) -> None:
+        """Handle a finite, shape-valid, joint-delta-checked IK solution."""
+
     def _prepare_target(
         self,
         state: CoordinatorState,
         q_current: NDArray[np.float64],
         dt: float,
     ) -> pinocchio.SE3 | None:
-        """Prepare one normalized target for the measured-state solve."""
+        """Prepare one normalized target for the selected solve configuration."""
         with self._lock:
             pose = self._target_pose
         if pose is None:
diff --git a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py
index d14fd59f6c..771dc5fe76 100644
--- a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py
+++ b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py
@@ -40,9 +40,6 @@
 from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake
 from dimos.protocol.service.spec import BaseConfig
 
-# Pink's integration/QP boundary tolerance is small but larger than machine epsilon.
-_POSITION_LIMIT_EPSILON_RAD = 1e-5
-
 
 class PinkControlIKConfig(BaseConfig):
     """Typed configuration for the control IK backend."""
@@ -55,7 +52,10 @@ class PinkControlIKConfig(BaseConfig):
     position_cost: FiniteFloat = Field(1.0, ge=0.0)
     orientation_cost: FiniteFloat = Field(1.0, ge=0.0)
     posture_cost: FiniteFloat = Field(1e-3, ge=0.0)
+    joint_centering_cost: FiniteFloat = Field(0.0, ge=0.0)
     damping_cost: FiniteFloat = Field(0.0, ge=0.0)
+    position_limit_margin: FiniteFloat = Field(1e-3, ge=0.0)
+    seed_limit_tolerance: FiniteFloat = Field(1e-2, ge=0.0)
     reference_q: list[float] | None = None
     qpsolver_options: dict[str, FiniteFloat] = Field(default_factory=dict)
 
@@ -90,6 +90,7 @@ class _PinkRuntime:
     configuration: Configuration
     frame_task: FrameTask
     posture_task: PostureTask | None
+    joint_centering_task: PostureTask | None
     damping_task: DampingTask | None
     tasks: list[object]
     limits: list[object]
@@ -153,10 +154,19 @@ def build(self) -> _PinkRuntime:
             gain=config.task_gain,
         )
         posture_task = PostureTask(cost=config.posture_cost) if config.posture_cost > 0.0 else None
+        joint_centering_task = (
+            PostureTask(cost=config.joint_centering_cost)
+            if config.joint_centering_cost > 0.0
+            else None
+        )
+        if joint_centering_task is not None:
+            joint_centering_task.set_target(self._build_joint_center_q(model, mapping, reference_q))
         damping_task = DampingTask(cost=config.damping_cost) if config.damping_cost > 0.0 else None
         tasks: list[object] = [frame_task]
         if posture_task is not None:
             tasks.append(posture_task)
+        if joint_centering_task is not None:
+            tasks.append(joint_centering_task)
         if damping_task is not None:
             tasks.append(damping_task)
 
@@ -178,6 +188,7 @@ def build(self) -> _PinkRuntime:
             configuration=configuration,
             frame_task=frame_task,
             posture_task=posture_task,
+            joint_centering_task=joint_centering_task,
             damping_task=damping_task,
             tasks=tasks,
             limits=limits,
@@ -271,6 +282,22 @@ def _uncontrolled_ee_chain(
             joint_id = int(model.parents[joint_id])
         return False
 
+    @staticmethod
+    def _build_joint_center_q(
+        model: pinocchio.Model,
+        mapping: _CoordinateMapping,
+        reference_q: NDArray[np.float64],
+    ) -> NDArray[np.float64]:
+        center_q = reference_q.copy()
+        for q_index, width in zip(mapping.q_indices, mapping.q_widths, strict=True):
+            if width != 1:
+                continue
+            lower = model.lowerPositionLimit[q_index]
+            upper = model.upperPositionLimit[q_index]
+            if np.isfinite(lower) and np.isfinite(upper):
+                center_q[q_index] = (lower + upper) / 2.0
+        return center_q
+
     @staticmethod
     def _validate_frame(model: pinocchio.Model, frame_name: str) -> int:
         if not model.existFrame(frame_name):
@@ -318,6 +345,14 @@ def _apply_limits(
                 model.velocityLimit[index] = limit
         for index in mapping.v_indices:
             model.velocityLimit[index] = min(model.velocityLimit[index], self._config.max_velocity)
+        margin = self._config.position_limit_margin
+        for q_index, width in zip(mapping.q_indices, mapping.q_widths, strict=True):
+            if width != 1:
+                continue
+            lower = model.lowerPositionLimit[q_index]
+            upper = model.upperPositionLimit[q_index]
+            if np.isfinite(lower) and np.isfinite(upper) and upper - lower <= 2.0 * margin:
+                raise ValueError("position limit margin leaves no valid joint range")
         # Keep position bounds in the QP, but apply velocity limits by uniformly
         # scaling the solution. Tiny per-tick velocity boxes can make ProxQP
         # misclassify feasible differential IK problems as primal-infeasible.
@@ -358,7 +393,8 @@ def solve(
         configuration = runtime.configuration
         frame_task = runtime.frame_task
         try:
-            configuration.update(self._full_q(measured))
+            solve_seed = self._project_position_limits(measured, "solve seed")
+            configuration.update(self._full_q(solve_seed))
             frame_task.set_target(target)
             if runtime.posture_task is not None:
                 runtime.posture_task.set_target(configuration.q.copy())
@@ -376,10 +412,10 @@ def solve(
                 raise IKControlRuntimeError("Pink produced an invalid velocity")
             velocity = self._scale_velocity(velocity)
             configuration.integrate_inplace(velocity, dt)
-            candidate = self._project_controlled_positions(configuration.q, measured)
-            if candidate.size != measured.size or not np.all(np.isfinite(candidate)):
+            candidate = self._project_controlled_positions(configuration.q, solve_seed)
+            if candidate.size != solve_seed.size or not np.all(np.isfinite(candidate)):
                 raise IKControlRuntimeError("Pink produced an invalid joint candidate")
-            candidate = self._clamp_position_limits(candidate)
+            candidate = self._project_position_limits(candidate, "candidate")
             return ControlIKResult(candidate, self._controlled_velocity(velocity))
         except IKControlRuntimeError:
             raise
@@ -432,10 +468,16 @@ def _scale_velocity(self, velocity: NDArray[np.float64]) -> NDArray[np.float64]:
             return velocity
         return velocity / max_ratio
 
-    def _clamp_position_limits(self, candidate: NDArray[np.float64]) -> NDArray[np.float64]:
+    def _project_position_limits(
+        self,
+        positions: NDArray[np.float64],
+        source: str,
+    ) -> NDArray[np.float64]:
         runtime = self._runtime
         mapping = runtime.mapping
-        bounded = candidate.copy()
+        bounded = positions.copy()
+        margin = runtime.config.position_limit_margin
+        tolerance = runtime.config.seed_limit_tolerance
         for index, width in enumerate(mapping.q_widths):
             if width != 1:
                 continue
@@ -443,16 +485,20 @@ def _clamp_position_limits(self, candidate: NDArray[np.float64]) -> NDArray[np.f
             lower = runtime.model.lowerPositionLimit[q_index]
             upper = runtime.model.upperPositionLimit[q_index]
             value = bounded[index]
-            if value < lower:
-                if lower - value <= _POSITION_LIMIT_EPSILON_RAD:
-                    bounded[index] = lower
-                else:
-                    raise IKControlRuntimeError("Pink produced an out-of-bounds joint candidate")
-            elif value > upper:
-                if value - upper <= _POSITION_LIMIT_EPSILON_RAD:
-                    bounded[index] = upper
-                else:
-                    raise IKControlRuntimeError("Pink produced an out-of-bounds joint candidate")
+            joint_name = mapping.joint_names[index]
+            if np.isfinite(lower) and value < lower - tolerance:
+                raise IKControlRuntimeError(
+                    f"Pink {source} for {joint_name} violates lower position limit: "
+                    f"{value} < {lower}"
+                )
+            if np.isfinite(upper) and value > upper + tolerance:
+                raise IKControlRuntimeError(
+                    f"Pink {source} for {joint_name} violates upper position limit: "
+                    f"{value} > {upper}"
+                )
+            safe_lower = lower + margin if np.isfinite(lower) else -np.inf
+            safe_upper = upper - margin if np.isfinite(upper) else np.inf
+            bounded[index] = np.clip(value, safe_lower, safe_upper)
         return bounded
 
 
diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py
index eb2bd2ab6d..14c289d861 100644
--- a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py
+++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py
@@ -122,6 +122,12 @@ def test_pink_settings_use_finite_declarative_validation(tmp_path: Path) -> None
         PinkControlIKConfig(robot_model=robot, damping_cost=-1e-3)
     with pytest.raises(ValueError, match="finite"):
         PinkControlIKConfig(robot_model=robot, qpsolver_options={"eps": np.nan})
+    with pytest.raises(ValueError, match="greater than or equal to 0"):
+        PinkControlIKConfig(robot_model=robot, joint_centering_cost=-1e-3)
+    with pytest.raises(ValueError, match="greater than or equal to 0"):
+        PinkControlIKConfig(robot_model=robot, position_limit_margin=-1e-3)
+    with pytest.raises(ValueError, match="greater than or equal to 0"):
+        PinkControlIKConfig(robot_model=robot, seed_limit_tolerance=-1e-3)
     with pytest.raises(ValueError, match="ordered"):
         CartesianIKTaskConfig(
             joint_names=["joint1", "joint2"],
@@ -194,6 +200,18 @@ def test_pink_validates_named_frame_and_exact_joint_mapping(tmp_path: Path) -> N
         )
 
 
+def test_pink_rejects_position_margin_that_eliminates_valid_range(tmp_path: Path) -> None:
+    model_path = _write_urdf(tmp_path)
+
+    with pytest.raises(ValueError, match="margin leaves no valid joint range"):
+        create_pink_control_ik(
+            PinkControlIKConfig(
+                robot_model=_robot(model_path),
+                position_limit_margin=2.0,
+            )
+        )
+
+
 def test_pink_reanchors_measured_state_and_runs_one_frame_task_step(
     tmp_path: Path, monkeypatch: pytest.MonkeyPatch
 ) -> None:
@@ -286,6 +304,41 @@ def solve(
     assert calls and len(calls[0]) == 1
 
 
+def test_pink_joint_centering_task_targets_position_limit_midpoints(
+    tmp_path: Path, monkeypatch: pytest.MonkeyPatch
+) -> None:
+    model_path = _write_urdf(tmp_path)
+    robot = _robot(model_path).model_copy(
+        update={
+            "joint_limits_lower": [-1.0, -0.25],
+            "joint_limits_upper": [0.5, 0.75],
+        }
+    )
+    backend = create_pink_control_ik(
+        PinkControlIKConfig(
+            robot_model=robot,
+            posture_cost=0.0,
+            joint_centering_cost=1e-3,
+        )
+    )
+    calls: list[list[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.1, 0.2])
+    backend.solve(backend.forward_kinematics(measured), measured, 0.01)
+
+    centering_task = backend._runtime.joint_centering_task
+    assert centering_task is not None
+    assert calls == [[backend._runtime.frame_task, centering_task]]
+    np.testing.assert_allclose(centering_task.target_q, [-0.25, 0.25])
+
+
 def test_pink_damping_task_replaces_posture_for_low_motion_policy(
     tmp_path: Path, monkeypatch: pytest.MonkeyPatch
 ) -> None:
@@ -413,7 +466,7 @@ def solve(
     assert len(backend._runtime.limits) == 1
 
 
-def test_pink_clamps_tiny_position_limit_overshoot(
+def test_pink_projects_seed_and_solution_to_inward_position_limit_margin(
     tmp_path: Path, monkeypatch: pytest.MonkeyPatch
 ) -> None:
     model_path = _write_urdf(tmp_path)
@@ -421,21 +474,36 @@ def test_pink_clamps_tiny_position_limit_overshoot(
         update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]}
     )
     backend = create_pink_control_ik(PinkControlIKConfig(robot_model=robot))
-    measured = np.array([1.22, 0.1])
+    measured = np.array([1.221940718699932, 0.1])
+    solver_seed: list[np.ndarray] = []
 
     def solve(
-        configuration: object, tasks: list[object], dt: float, **kwargs: object
+        configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
     ) -> np.ndarray:
-        return np.array([0.00013784674535, -0.2])
+        solver_seed.append(configuration.q.copy())
+        return np.array([0.5, -0.2])
 
     monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve)
     result = backend.solve(backend.forward_kinematics(measured), measured, 0.01)
 
-    assert np.array_equal(result.positions, np.array([1.22, 0.098]))
-    assert np.array_equal(result.velocity, np.array([0.00013784674535, -0.2]))
+    np.testing.assert_allclose(solver_seed, [[1.219, 0.1]])
+    np.testing.assert_allclose(result.positions, [1.219, 0.098])
+    np.testing.assert_allclose(result.velocity, [0.5, -0.2])
 
 
-def test_pink_rejects_material_position_limit_violation(
+def test_pink_rejects_seed_beyond_position_limit_tolerance(tmp_path: Path) -> None:
+    model_path = _write_urdf(tmp_path)
+    robot = _robot(model_path).model_copy(
+        update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]}
+    )
+    backend = create_pink_control_ik(PinkControlIKConfig(robot_model=robot))
+    measured = np.array([1.231, 0.1])
+
+    with pytest.raises(IKControlRuntimeError, match="solve seed.*joint1"):
+        backend.solve(backend.forward_kinematics(measured), measured, 0.01)
+
+
+def test_pink_rejects_candidate_beyond_position_limit_tolerance(
     tmp_path: Path, monkeypatch: pytest.MonkeyPatch
 ) -> None:
     model_path = _write_urdf(tmp_path)
@@ -443,16 +511,16 @@ def test_pink_rejects_material_position_limit_violation(
         update={"joint_limits_lower": [-1.22, -0.25], "joint_limits_upper": [1.22, 0.25]}
     )
     backend = create_pink_control_ik(PinkControlIKConfig(robot_model=robot))
-    measured = np.array([1.22, 0.1])
+    measured = np.array([1.2, 0.1])
 
     def solve(
         configuration: object, tasks: list[object], dt: float, **kwargs: object
     ) -> np.ndarray:
-        return np.array([0.01, -0.2])
+        return np.array([10.0, 0.0])
 
     monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve)
-    with pytest.raises(IKControlRuntimeError, match="out-of-bounds"):
-        backend.solve(backend.forward_kinematics(measured), measured, 0.01)
+    with pytest.raises(IKControlRuntimeError, match="candidate.*joint1"):
+        backend.solve(backend.forward_kinematics(measured), measured, 0.05)
 
 
 @pytest.mark.parametrize("legacy_field", ["backend", "ee_joint_id", "self_collision_enabled"])
diff --git a/dimos/control/tasks/eef_twist_task/eef_twist_task.py b/dimos/control/tasks/eef_twist_task/eef_twist_task.py
index 0de24cd968..e6e7785217 100644
--- a/dimos/control/tasks/eef_twist_task/eef_twist_task.py
+++ b/dimos/control/tasks/eef_twist_task/eef_twist_task.py
@@ -12,7 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-"""Measured-state end-effector twist control."""
+"""Command-integrating end-effector twist control."""
 
 from __future__ import annotations
 
@@ -36,6 +36,8 @@
 from dimos.utils.transform_utils import twist_to_numpy
 
 if TYPE_CHECKING:
+    from numpy.typing import NDArray
+
     from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped
     from dimos.msgs.std_msgs.Bool import Bool
 
@@ -44,7 +46,7 @@
 
 @dataclass
 class EEFTwistTaskConfig(CartesianIKTaskConfig):
-    """Configuration for measured-FK-relative EEF twist control."""
+    """Configuration for command-relative EEF twist control."""
 
     gripper_joint: str | None = None
     gripper_open_pos: float = 0.0
@@ -52,7 +54,7 @@ class EEFTwistTaskConfig(CartesianIKTaskConfig):
 
 
 class EEFTwistTask(CartesianIKTask):
-    """Cartesian task specialization whose target is prepared from a twist."""
+    """Integrate twists from the last accepted command while the stream is active."""
 
     _config: EEFTwistTaskConfig
 
@@ -60,6 +62,7 @@ def __init__(self, name: str, config: EEFTwistTaskConfig) -> None:
         super().__init__(name, config)
         self._twist_lock = threading.Lock()
         self._latest_twist: TwistStamped | None = None
+        self._last_commanded_joints: NDArray[np.float64] | None = None
         self._estopped = False
         self._gripper_target = config.gripper_open_pos
 
@@ -102,6 +105,7 @@ def on_ee_twist_command(self, twist: TwistStamped, t_now: float) -> bool:
                 return False
             if np.allclose(values, 0.0):
                 self._latest_twist = None
+                self._last_commanded_joints = None
                 cleared = True
             else:
                 self._latest_twist = twist
@@ -133,6 +137,7 @@ def set_estop(self, estopped: bool) -> None:
             self._estopped = estopped
             if estopped:
                 self._latest_twist = None
+                self._last_commanded_joints = None
 
     def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
         output = super().compute(state)
@@ -166,18 +171,46 @@ def _prepare_target(
             return None
         return pose
 
+    def _select_solve_joints(
+        self,
+        state: CoordinatorState,
+        q_measured: NDArray[np.float64],
+    ) -> NDArray[np.float64]:
+        with self._twist_lock:
+            if self._last_commanded_joints is None:
+                return q_measured
+            return self._last_commanded_joints.copy()
+
+    def _on_solution_accepted(
+        self,
+        state: CoordinatorState,
+        q_solution: NDArray[np.float64],
+    ) -> None:
+        with self._twist_lock:
+            if self._latest_twist is not None and not self._estopped:
+                self._last_commanded_joints = q_solution.copy()
+
+    def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:
+        if joints & self._joint_names:
+            with self._twist_lock:
+                self._last_commanded_joints = None
+        super().on_preempted(by_task, joints)
+
     def stop(self) -> None:
         with self._twist_lock:
             self._latest_twist = None
+            self._last_commanded_joints = None
         super().stop()
 
     def _on_timeout(self) -> None:
         with self._twist_lock:
             self._latest_twist = None
+            self._last_commanded_joints = None
 
     def clear(self) -> None:
         with self._twist_lock:
             self._latest_twist = None
+            self._last_commanded_joints = None
         super().clear()
 
 
diff --git a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py
index 051b01d65d..56d6040e4b 100644
--- a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py
+++ b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py
@@ -48,8 +48,10 @@ def __init__(self) -> None:
         self.nq = 3
         self.fk_calls: list[np.ndarray] = []
         self.solve_calls: list[FakePose] = []
+        self.q_calls: list[np.ndarray] = []
         self.dt_calls: list[float] = []
         self.solution = np.array([0.01, 0.02, 0.03], dtype=np.float64)
+        self.increment: np.ndarray | None = None
         self.raise_runtime = False
 
     def forward_kinematics(self, q_current: NDArray[np.float64]) -> FakePose:
@@ -60,8 +62,10 @@ def solve(self, pose: FakePose, q_current: NDArray[np.float64], dt: float) -> Co
         if self.raise_runtime:
             raise IKControlRuntimeError("synthetic solver failure")
         self.solve_calls.append(pose.copy())
+        self.q_calls.append(q_current.copy())
         self.dt_calls.append(dt)
-        return ControlIKResult(self.solution.copy(), self.solution - q_current)
+        solution = self.solution if self.increment is None else q_current + self.increment
+        return ControlIKResult(solution.copy(), solution - q_current)
 
 
 @pytest.fixture
@@ -158,17 +162,23 @@ def test_ik_runtime_error_is_a_bounded_hold(task: EEFTwistTask, fake_ik: FakeIK)
     assert hold.positions == [0.0, 0.0, 0.0]
 
 
-def test_integration_uses_current_fk_and_coordinator_dt(
+def test_integration_uses_last_command_and_coordinator_dt_when_feedback_lags(
     task: EEFTwistTask, fake_ik: FakeIK
 ) -> None:
     assert task.on_ee_twist_command(_twist(1.0), t_now=1.0)
+    fake_ik.increment = np.array([0.01, 0.0, 0.0], dtype=np.float64)
 
     first = task.compute(_state(1.01, dt=0.01))
-    fake_ik.solution = np.array([0.51, 0.0, 0.0], dtype=np.float64)
-    second = task.compute(_state(1.04, positions=[0.5, 0.0, 0.0], dt=0.01))
+    second = task.compute(_state(1.02, dt=0.01))
 
     assert first is not None
     assert second is not None
+    assert first.positions == pytest.approx([0.01, 0.0, 0.0])
+    assert second.positions == pytest.approx([0.02, 0.0, 0.0])
+    np.testing.assert_allclose(
+        fake_ik.q_calls,
+        np.array([[0.0, 0.0, 0.0], [0.01, 0.0, 0.0]]),
+    )
     assert fake_ik.dt_calls == [0.02, 0.02]
     assert fake_ik.solve_calls[1].translation[0] > fake_ik.solve_calls[0].translation[0]
 
@@ -225,7 +235,7 @@ def test_joint_delta_rejection_returns_a_hold(task: EEFTwistTask, fake_ik: FakeI
     assert rejected.positions == [0.0, 0.0, 0.0]
 
 
-def test_timeout_and_zero_command_clear_then_next_nonzero_reseeds(
+def test_timeout_and_zero_command_clear_then_next_nonzero_reseeds_from_measured(
     task: EEFTwistTask, fake_ik: FakeIK
 ) -> None:
     assert task.on_ee_twist_command(_twist(), t_now=1.0)
@@ -237,11 +247,30 @@ def test_timeout_and_zero_command_clear_then_next_nonzero_reseeds(
     fake_ik.solution = np.array([1.01, 0.0, 0.0], dtype=np.float64)
     assert task.on_ee_twist_command(_twist(), t_now=2.0)
     assert task.compute(_state(2.01, positions=[1.0, 0.0, 0.0])) is not None
+    np.testing.assert_allclose(fake_ik.q_calls[-1], [1.0, 0.0, 0.0])
     assert fake_ik.solve_calls[-1].translation[0] > 1.0
 
     assert task.on_ee_twist_command(_twist(0.0), t_now=2.02)
     assert not task.is_active()
 
+    fake_ik.solution = np.array([2.01, 0.0, 0.0], dtype=np.float64)
+    assert task.on_ee_twist_command(_twist(), t_now=3.0)
+    assert task.compute(_state(3.01, positions=[2.0, 0.0, 0.0])) is not None
+    np.testing.assert_allclose(fake_ik.q_calls[-1], [2.0, 0.0, 0.0])
+
+
+def test_preemption_discards_last_commanded_solve_seed(task: EEFTwistTask, fake_ik: FakeIK) -> None:
+    fake_ik.increment = np.array([0.01, 0.0, 0.0], dtype=np.float64)
+    assert task.on_ee_twist_command(_twist(), t_now=1.0)
+    assert task.compute(_state(1.01)) is not None
+
+    task.on_preempted("higher_priority", frozenset(["arm/joint1"]))
+    output = task.compute(_state(1.02, positions=[0.5, 0.0, 0.0]))
+
+    assert output is not None
+    assert output.positions == pytest.approx([0.51, 0.0, 0.0])
+    np.testing.assert_allclose(fake_ik.q_calls[-1], [0.5, 0.0, 0.0])
+
 
 @pytest.fixture
 def gripper_task(fake_ik: FakeIK) -> EEFTwistTask:
diff --git a/dimos/control/tasks/teleop_task/teleop_task.py b/dimos/control/tasks/teleop_task/teleop_task.py
index da6c00ba6a..7de544f377 100644
--- a/dimos/control/tasks/teleop_task/teleop_task.py
+++ b/dimos/control/tasks/teleop_task/teleop_task.py
@@ -12,7 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-"""Engagement-relative teleop control through measured-state Pink IK."""
+"""Engagement-relative teleop control through command-integrating Pink IK."""
 
 from __future__ import annotations
 
@@ -50,6 +50,7 @@ class TeleopControlIKConfig(PinkControlIKConfig):
     position_cost: FiniteFloat = Field(1.0, ge=0.0)
     orientation_cost: FiniteFloat = Field(1.0, ge=0.0)
     posture_cost: FiniteFloat = Field(0.0, ge=0.0)
+    joint_centering_cost: FiniteFloat = Field(1e-3, ge=0.0)
     damping_cost: FiniteFloat = Field(1e-3, ge=0.0)
 
 
@@ -74,6 +75,7 @@ def __init__(self, name: str, config: TeleopIKTaskConfig) -> None:
             raise ValueError(f"TeleopIKTask '{name}' requires hand='left' or 'right'")
         super().__init__(name, config)
         self._initial_ee_pose: pinocchio.SE3 | None = None
+        self._last_commanded_joints: NDArray[np.float64] | None = None
         self._prev_primary = False
         self._estopped = False
         self._gripper_target = config.gripper_open_pos
@@ -106,8 +108,28 @@ def set_estop(self, estopped: bool) -> None:
                 self._active = False
                 self._target_pose = None
                 self._initial_ee_pose = None
+                self._last_commanded_joints = None
                 self._prev_primary = False
 
+    def _select_solve_joints(
+        self,
+        state: CoordinatorState,
+        q_measured: NDArray[np.float64],
+    ) -> NDArray[np.float64]:
+        with self._lock:
+            if self._last_commanded_joints is None:
+                return q_measured
+            return self._last_commanded_joints.copy()
+
+    def _on_solution_accepted(
+        self,
+        state: CoordinatorState,
+        q_solution: NDArray[np.float64],
+    ) -> None:
+        with self._lock:
+            if not self._estopped and self._active and self._target_pose is not None:
+                self._last_commanded_joints = q_solution.copy()
+
     def _prepare_target(
         self,
         state: CoordinatorState,
@@ -171,10 +193,12 @@ def on_buttons(self, msg: Buttons) -> bool:
                 return False
             if primary and not self._prev_primary:
                 self._initial_ee_pose = None
+                self._last_commanded_joints = None
             elif not primary and self._prev_primary:
                 self._active = False
                 self._target_pose = None
                 self._initial_ee_pose = None
+                self._last_commanded_joints = None
             self._prev_primary = primary
 
         if self._config.gripper_joint is not None:
@@ -213,8 +237,15 @@ def on_gripper_trigger(self, value: float, _t_now: float = 0.0) -> bool:
     def _on_timeout(self) -> None:
         """Discard the baseline while the parent holds the task lock."""
         self._initial_ee_pose = None
+        self._last_commanded_joints = None
         self._prev_primary = False
 
+    def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:
+        if joints & self._joint_names:
+            with self._lock:
+                self._last_commanded_joints = None
+        super().on_preempted(by_task, joints)
+
     def stop(self) -> None:
         """Stop output and discard engagement-relative state."""
         super().stop()
@@ -222,6 +253,7 @@ def stop(self) -> None:
             self._active = False
             self._target_pose = None
             self._initial_ee_pose = None
+            self._last_commanded_joints = None
             self._prev_primary = False
 
     def clear(self) -> None:
@@ -231,6 +263,7 @@ def clear(self) -> None:
             self._active = False
             self._target_pose = None
             self._initial_ee_pose = None
+            self._last_commanded_joints = None
             self._prev_primary = False
 
 
diff --git a/dimos/control/tasks/teleop_task/test_teleop_task.py b/dimos/control/tasks/teleop_task/test_teleop_task.py
index a779afa877..56aed39aaf 100644
--- a/dimos/control/tasks/teleop_task/test_teleop_task.py
+++ b/dimos/control/tasks/teleop_task/test_teleop_task.py
@@ -49,6 +49,7 @@ def __post_init__(self) -> None:
         self.fk_calls: list[NDArray[np.float64]] = []
         self.solve_calls: list[tuple[pinocchio.SE3, NDArray[np.float64], float]] = []
         self.solution = np.array([0.01, 0.02], dtype=np.float64)
+        self.increment: NDArray[np.float64] | None = None
         self.raise_runtime = False
 
     def forward_kinematics(self, q: NDArray[np.float64]) -> pinocchio.SE3:
@@ -67,7 +68,8 @@ def solve(
         if self.raise_runtime:
             raise IKControlRuntimeError("synthetic Pink failure")
         self.solve_calls.append((target.copy(), measured.copy(), dt))
-        return ControlIKResult(self.solution.copy(), self.solution - measured)
+        solution = self.solution if self.increment is None else measured + self.increment
+        return ControlIKResult(solution.copy(), solution - measured)
 
 
 def _robot(path: Path) -> RobotModelConfig:
@@ -184,6 +186,25 @@ def test_delta_is_composed_with_one_measured_engagement_baseline(
     assert (first_dt, second_dt) == (0.03, 0.02)
 
 
+def test_teleop_ik_iterates_from_last_command_when_feedback_lags(
+    task: TeleopIKTask, fake_ik: _FakePinkIK
+) -> None:
+    fake_ik.increment = np.array([0.01, 0.0], dtype=np.float64)
+    assert task.on_cartesian_command(_delta(), t_now=1.0)
+
+    first = task.compute(_state(1.01))
+    second = task.compute(_state(1.02))
+
+    assert first is not None
+    assert second is not None
+    assert first.positions == pytest.approx([0.01, 0.0])
+    assert second.positions == pytest.approx([0.02, 0.0])
+    np.testing.assert_allclose(
+        [solve[1] for solve in fake_ik.solve_calls],
+        np.array([[0.0, 0.0], [0.01, 0.0]]),
+    )
+
+
 def test_missing_joint_state_defers_baseline_and_output(
     task: TeleopIKTask, fake_ik: _FakePinkIK
 ) -> None:
@@ -230,18 +251,36 @@ def test_release_timeout_stop_and_clear_force_fresh_baselines(
     assert task.on_teleop_buttons(pressed, 2.0)
     assert task.on_cartesian_command(_delta(), 2.0)
     assert task.compute(_state(2.01, (0.1, 0.2))) is not None
+    np.testing.assert_allclose(fake_ik.solve_calls[-1][1], [0.1, 0.2])
     assert task.compute(_state(3.0, (0.1, 0.2))) is None
 
     assert task.on_cartesian_command(_delta(), 4.0)
     assert task.compute(_state(4.01, (0.2, 0.3))) is not None
+    np.testing.assert_allclose(fake_ik.solve_calls[-1][1], [0.2, 0.3])
     task.stop()
     task.start()
     assert task.on_cartesian_command(_delta(), 5.0)
     assert task.compute(_state(5.01, (0.3, 0.4))) is not None
+    np.testing.assert_allclose(fake_ik.solve_calls[-1][1], [0.3, 0.4])
     task.clear()
     assert len(fake_ik.fk_calls) == 4
 
 
+def test_preemption_discards_teleop_commanded_solve_seed(
+    task: TeleopIKTask, fake_ik: _FakePinkIK
+) -> None:
+    fake_ik.increment = np.array([0.01, 0.0], dtype=np.float64)
+    assert task.on_cartesian_command(_delta(), t_now=1.0)
+    assert task.compute(_state(1.01)) is not None
+
+    task.on_preempted("higher_priority", frozenset(["arm/joint1"]))
+    output = task.compute(_state(1.02, (0.5, 0.5)))
+
+    assert output is not None
+    assert output.positions == pytest.approx([0.51, 0.5])
+    np.testing.assert_allclose(fake_ik.solve_calls[-1][1], [0.5, 0.5])
+
+
 def test_estop_rejects_commands_and_never_replays_them(
     gripper_task: TeleopIKTask, fake_ik: _FakePinkIK
 ) -> None:
@@ -317,6 +356,7 @@ def test_factory_applies_balanced_teleop_control_policy(
     assert task._config.control_ik.position_cost == 1.0
     assert task._config.control_ik.orientation_cost == 1.0
     assert task._config.control_ik.posture_cost == 0.0
+    assert task._config.control_ik.joint_centering_cost == 1e-3
     assert task._config.control_ik.damping_cost == 1e-3
     assert task._config.max_joint_delta_deg == 5.0
 
diff --git a/dimos/hardware/manipulators/galaxea_a1z/adapter.py b/dimos/hardware/manipulators/galaxea_a1z/adapter.py
index 00eb9f13c6..693e27273c 100644
--- a/dimos/hardware/manipulators/galaxea_a1z/adapter.py
+++ b/dimos/hardware/manipulators/galaxea_a1z/adapter.py
@@ -185,15 +185,22 @@ def connect(self) -> bool:
 
     def _create_robot(self) -> ArmRobot:
         gripper = self._config.gripper
-        return get_a1z_robot(
+        robot = get_a1z_robot(
             can_channel=self._can_channel,
             gravity_comp_factor=self._config.gravity_comp_factor,
             zero_gravity_mode=self._config.teaching is not None,
             control_freq_hz=_SDK_CONTROL_FREQ_HZ,
             urdf_path=self._config.urdf_path,
+            default_kp=np.asarray(self._config.default_kp, dtype=float),
+            default_kd=np.asarray(self._config.default_kd, dtype=float),
             with_gripper=gripper is not None,
             gripper_max_torque=gripper.max_torque if gripper else 0.5,
         )
+        if gripper is not None:
+            # The pinned SDK exposes the velocity on Gripper but not through
+            # get_a1z_robot(). Keep the override here until the factory does.
+            robot.gripper._max_vel = gripper.max_velocity_rad_s
+        return robot
 
     def disconnect(self) -> None:
         """Stop the control loop, disable motors, and close the CAN bus.
diff --git a/dimos/hardware/manipulators/galaxea_a1z/config.py b/dimos/hardware/manipulators/galaxea_a1z/config.py
index f3d52f62af..c18f3f434c 100644
--- a/dimos/hardware/manipulators/galaxea_a1z/config.py
+++ b/dimos/hardware/manipulators/galaxea_a1z/config.py
@@ -16,10 +16,37 @@
 
 from __future__ import annotations
 
+import math
 from pathlib import Path
 
 import attrs
 
+_A1Z_DOF = 6
+_A1Z_DEFAULT_KP = (80.0, 80.0, 80.0, 50.0, 10.0, 10.0)
+_A1Z_DEFAULT_KD = (3.0, 3.0, 3.0, 0.7, 0.2, 0.2)
+
+
+def _joint_gains(
+    value: tuple[float, ...] | list[float],
+    *,
+    name: str,
+    maximum: float,
+) -> tuple[float, ...]:
+    gains = tuple(float(gain) for gain in value)
+    if len(gains) != _A1Z_DOF:
+        raise ValueError(f"{name} must contain {_A1Z_DOF} values")
+    if any(not math.isfinite(gain) or not 0.0 <= gain <= maximum for gain in gains):
+        raise ValueError(f"{name} gains must be finite and within [0, {maximum}]")
+    return gains
+
+
+def _kp_gains(value: tuple[float, ...] | list[float]) -> tuple[float, ...]:
+    return _joint_gains(value, name="default_kp", maximum=200.0)
+
+
+def _kd_gains(value: tuple[float, ...] | list[float]) -> tuple[float, ...]:
+    return _joint_gains(value, name="default_kd", maximum=5.0)
+
 
 def _validate_optional_path(
     _instance: object,
@@ -47,6 +74,14 @@ class A1ZGripperConfig:
         converter=float,
         validator=attrs.validators.gt(0.0),
     )
+    max_velocity_rad_s: float = attrs.field(
+        default=10.0,
+        converter=float,
+        validator=attrs.validators.and_(
+            attrs.validators.gt(0.0),
+            attrs.validators.le(100.0),
+        ),
+    )
 
 
 @attrs.frozen(slots=False)
@@ -71,6 +106,14 @@ class A1ZConfig:
             attrs.validators.le(1.0),
         ),
     )
+    default_kp: tuple[float, ...] = attrs.field(
+        default=_A1Z_DEFAULT_KP,
+        converter=_kp_gains,
+    )
+    default_kd: tuple[float, ...] = attrs.field(
+        default=_A1Z_DEFAULT_KD,
+        converter=_kd_gains,
+    )
     urdf_path: str | Path | None = attrs.field(
         default=None,
         validator=_validate_optional_path,
diff --git a/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py b/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py
index 956469aaf8..81a9d681e2 100644
--- a/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py
+++ b/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py
@@ -52,6 +52,7 @@ def disable(self) -> None:
 class _FakeGripper:
     def __init__(self) -> None:
         self._motor = _FakeMotor()
+        self._max_vel = 10.0
         self.feedback_fraction = 0.0
 
     def get_feedback_norm(self) -> float:
@@ -79,8 +80,8 @@ def __init__(self, **factory_kwargs: Any) -> None:
         self._running = False
         self._estopped = False
         self._bus = _FakeBus()
-        self._default_kp = np.array([30.0, 30.0, 30.0, 20.0, 5.0, 5.0])
-        self._default_kd = np.array([1.0, 1.0, 1.0, 0.5, 0.5, 0.5])
+        self._default_kp = np.asarray(factory_kwargs["default_kp"], dtype=float)
+        self._default_kd = np.asarray(factory_kwargs["default_kd"], dtype=float)
         self.actions: list[Any] = []
         self.gravity_factor_history: list[float] = []
         self.gravity_comp_factor = float(factory_kwargs["gravity_comp_factor"])
@@ -240,6 +241,7 @@ def _connected_adapter(module: ModuleType, **kwargs: Any) -> tuple[Any, _FakeArm
         A1ZGripperConfig(
             max_torque=kwargs.pop("gripper_max_torque", 0.5),
             max_opening_m=kwargs.pop("gripper_max_opening_m", 0.1),
+            max_velocity_rad_s=kwargs.pop("gripper_max_velocity_rad_s", 10.0),
         )
         if gripper_enabled
         else None
@@ -250,6 +252,8 @@ def _connected_adapter(module: ModuleType, **kwargs: Any) -> tuple[Any, _FakeArm
     config = A1ZConfig(
         gravity_comp_factor=kwargs.pop("gravity_comp_factor", 1.0),
         urdf_path=kwargs.pop("urdf_path", None),
+        default_kp=kwargs.pop("default_kp", (80.0, 80.0, 80.0, 50.0, 10.0, 10.0)),
+        default_kd=kwargs.pop("default_kd", (3.0, 3.0, 3.0, 0.7, 0.2, 0.2)),
         gripper=gripper,
         teaching=teaching,
     )
@@ -269,6 +273,18 @@ def test_connect_opens_bus_without_powering_motors(
     assert not adapter.read_enabled()
 
 
+def test_connect_forwards_configured_arm_gains_to_sdk(
+    a1z_adapter_module: ModuleType,
+) -> None:
+    kp = (80.0, 80.0, 80.0, 50.0, 10.0, 10.0)
+    kd = (3.0, 3.0, 3.0, 0.7, 0.2, 0.2)
+
+    _, robot = _connected_adapter(a1z_adapter_module, default_kp=kp, default_kd=kd)
+
+    assert robot.factory_kwargs["default_kp"] == pytest.approx(kp)
+    assert robot.factory_kwargs["default_kd"] == pytest.approx(kd)
+
+
 def test_safe_start_stages_measured_hold_before_gravity_feedforward(
     a1z_adapter_module: ModuleType,
 ) -> None:
@@ -507,6 +523,20 @@ def test_gripper_round_trips_meters_to_normalized(
     assert robot.gripper_fraction == pytest.approx(1.0)
 
 
+def test_connect_applies_configured_gripper_velocity(
+    a1z_adapter_module: ModuleType,
+) -> None:
+    adapter, robot = _connected_adapter(
+        a1z_adapter_module,
+        gripper=True,
+        gripper_max_velocity_rad_s=24.0,
+    )
+
+    assert adapter.is_connected()
+    assert robot.factory_kwargs["gripper_max_torque"] == pytest.approx(0.5)
+    assert robot.gripper._max_vel == pytest.approx(24.0)
+
+
 def test_configured_gripper_free_drive_tracks_adapter_lifecycle(
     a1z_adapter_module: ModuleType,
 ) -> None:
diff --git a/dimos/robot/manipulators/a1z/blueprints/teleop.py b/dimos/robot/manipulators/a1z/blueprints/teleop.py
index 31f6d620cc..4cc2a1a8da 100644
--- a/dimos/robot/manipulators/a1z/blueprints/teleop.py
+++ b/dimos/robot/manipulators/a1z/blueprints/teleop.py
@@ -16,8 +16,6 @@
 
 from __future__ import annotations
 
-from dataclasses import replace
-
 from dimos.control.coordinator import ControlCoordinator, TaskConfig
 from dimos.core.coordination.blueprints import autoconnect
 from dimos.manipulation.manipulation_module import ManipulationModule
@@ -61,12 +59,7 @@
 )
 
 
-_a1z_quest_hw = replace(
-    a1z_hardware("arm"),
-    adapter_type="mock",
-    address=None,
-    adapter_kwargs={},
-)
+_a1z_quest_hw = a1z_hardware("arm")
 _a1z_quest_model = make_a1z_model_config()
 
 coordinator_teleop_a1z = autoconnect(
@@ -80,6 +73,11 @@
                 robot_model=_a1z_quest_model,
                 control_ik={"max_velocity": 2.0},
                 priority=20,
+                params={
+                    "gripper_joint": _a1z_quest_hw.gripper_joints[0],
+                    "gripper_open_pos": 1.0,
+                    "gripper_closed_pos": 0.0,
+                },
             ),
             trajectory_task(_a1z_quest_hw),
         ],
diff --git a/dimos/robot/manipulators/a1z/blueprints/test_teleop.py b/dimos/robot/manipulators/a1z/blueprints/test_teleop.py
new file mode 100644
index 0000000000..a7b8b51cc1
--- /dev/null
+++ b/dimos/robot/manipulators/a1z/blueprints/test_teleop.py
@@ -0,0 +1,81 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from typing import Any, cast
+
+import pytest
+
+from dimos.control.coordinator import ControlCoordinator, TaskConfig
+from dimos.core.coordination.blueprints import Blueprint
+from dimos.core.global_config import global_config
+from dimos.robot.manipulators.a1z.blueprints.teleop import coordinator_teleop_a1z
+from dimos.robot.manipulators.a1z.config import a1z_hardware
+from dimos.teleop.quest.blueprints import teleop_quest_a1z
+from dimos.teleop.quest.quest_extensions import ArmTeleopModule
+
+
+def _coordinator_kwargs(blueprint: Blueprint) -> dict[str, Any]:
+    return next(atom.kwargs for atom in blueprint.blueprints if atom.module is ControlCoordinator)
+
+
+def test_quest_teleop_uses_mock_a1z_hardware_and_gripper_by_default() -> None:
+    kwargs = _coordinator_kwargs(coordinator_teleop_a1z)
+    hardware = kwargs["hardware"][0]
+    tasks = cast("list[TaskConfig]", kwargs["tasks"])
+    teleop = next(task for task in tasks if task.name == "teleop_a1z")
+
+    assert hardware.adapter_type == "mock"
+    assert hardware.address is None
+    assert hardware.gripper_joints == ["arm/gripper"]
+    assert hardware.gripper_open_position == pytest.approx(0.1)
+    assert hardware.gripper_closed_position == pytest.approx(0.0)
+    assert teleop.params["gripper_joint"] == "arm/gripper"
+    assert teleop.params["gripper_open_pos"] == pytest.approx(1.0)
+    assert teleop.params["gripper_closed_pos"] == pytest.approx(0.0)
+
+
+def test_quest_left_controller_routes_to_a1z_teleop() -> None:
+    arm_kwargs = next(
+        atom.kwargs for atom in teleop_quest_a1z.blueprints if atom.module is ArmTeleopModule
+    )
+
+    assert arm_kwargs["task_names"] == {"left": "teleop_a1z"}
+    assert teleop_quest_a1z.remapping_map == {
+        ("armteleopmodule", "left_controller_output"): "coordinator_cartesian_command"
+    }
+
+
+def test_a1z_hardware_uses_mock_adapter_in_simulation(monkeypatch: pytest.MonkeyPatch) -> None:
+    monkeypatch.setattr(global_config, "can_port", "a1zcan")
+    monkeypatch.setattr(global_config, "simulation", "mujoco")
+
+    hardware = a1z_hardware("arm")
+
+    assert hardware.adapter_type == "mock"
+    assert hardware.address is None
+    assert hardware.adapter_kwargs == {}
+    assert hardware.gripper_joints == ["arm/gripper"]
+
+
+def test_a1z_hardware_uses_real_adapter_when_can_port_is_selected(
+    monkeypatch: pytest.MonkeyPatch,
+) -> None:
+    monkeypatch.setattr(global_config, "can_port", "a1zcan")
+    monkeypatch.setattr(global_config, "simulation", "")
+
+    hardware = a1z_hardware("arm")
+
+    assert hardware.adapter_type == "galaxea_a1z"
+    assert hardware.address == "a1zcan"
+    assert hardware.gripper_joints == ["arm/gripper"]
diff --git a/dimos/robot/manipulators/a1z/config.py b/dimos/robot/manipulators/a1z/config.py
index 30c40ef917..0c2b67f1c7 100644
--- a/dimos/robot/manipulators/a1z/config.py
+++ b/dimos/robot/manipulators/a1z/config.py
@@ -58,13 +58,13 @@ def a1z_hardware(
     dynamics_urdf_path: Path | None = None,
     adapter_config: A1ZConfig | None = None,
 ) -> HardwareComponent:
-    """Configure mock or real A1Z hardware from the resolved global settings."""
+    """Configure mock A1Z hardware unless an explicit CAN port selects the real adapter."""
     adapter_type = "mock"
     address = None
     adapter_kwargs: dict[str, object] = {}
-    if not global_config.simulation:
+    if not global_config.simulation and global_config.can_port:
         adapter_type = "galaxea_a1z"
-        address = global_config.can_port or "a1zcan"
+        address = global_config.can_port
         resolved_config = adapter_config or A1ZConfig(
             gripper=A1ZGripperConfig() if has_gripper else None,
         )
diff --git a/dimos/teleop/quest/README.md b/dimos/teleop/quest/README.md
index 4e8164ec9b..fc32904228 100644
--- a/dimos/teleop/quest/README.md
+++ b/dimos/teleop/quest/README.md
@@ -15,9 +15,16 @@ Quest Browser  ──WebSocket──→  Embedded HTTPS Server  ──→  Quest
 dimos run teleop-quest-rerun    # Quest teleop + Rerun viz
 dimos run teleop-quest-xarm7   # XArm7
 dimos run teleop-quest-piper   # Piper
+dimos run teleop-quest-a1z     # A1Z with mock hardware
 dimos run teleop-quest-dual    # Dual arm
 ```
 
+Select a CAN interface explicitly to control real A1Z hardware:
+
+```bash
+dimos --can-port a1zcan run teleop-quest-a1z
+```
+
 Open `https://<host-ip>:8443/teleop` on Quest browser. Accept cert, tap Connect.
 
 ## Subclassing
diff --git a/docs/capabilities/manipulation/a1z.md b/docs/capabilities/manipulation/a1z.md
index 1bc6941ee0..0cfeabb0e7 100644
--- a/docs/capabilities/manipulation/a1z.md
+++ b/docs/capabilities/manipulation/a1z.md
@@ -94,16 +94,20 @@ stopping DimOS. Disabling the motors makes the arm fall.
 dimos run keyboard-teleop-a1z
 ```
 
-This launches keyboard teleoperation, the control coordinator, trajectory
-execution, and `ManipulationModule`. Startup waits for feedback from all six arm
+This launches keyboard teleoperation with mock hardware, the control coordinator,
+trajectory execution, and `ManipulationModule`. Select a CAN interface explicitly
+to use the real arm. Real-hardware startup waits for feedback from all six arm
 motors, validates the measured state, holds the measured pose, and then ramps
-gravity compensation.
+gravity compensation:
 
-On Linux, the blueprint uses `a1zcan` by default. If you configured another
-verified SocketCAN interface, pass it explicitly:
+```bash
+dimos --can-port a1zcan run keyboard-teleop-a1z
+```
+
+On Linux, pass another verified SocketCAN interface instead if needed:
 
 ```bash
-dimos run keyboard-teleop-a1z --can-port can0
+dimos --can-port can0 run keyboard-teleop-a1z
 ```
 
 On macOS, the adapter selects the userspace USB transport automatically; omit
diff --git a/stubs/a1z/robots/get_robot.pyi b/stubs/a1z/robots/get_robot.pyi
index ec68cfd917..160a5ed390 100644
--- a/stubs/a1z/robots/get_robot.pyi
+++ b/stubs/a1z/robots/get_robot.pyi
@@ -1,4 +1,8 @@
 from pathlib import Path
+from typing import Any
+
+import numpy as np
+import numpy.typing as npt
 
 from .arm_robot import ArmRobot
 
@@ -8,6 +12,8 @@ def get_a1z_robot(
     zero_gravity_mode: bool = ...,
     control_freq_hz: int = ...,
     urdf_path: str | Path | None = ...,
+    default_kp: npt.NDArray[np.floating[Any]] | None = ...,
+    default_kd: npt.NDArray[np.floating[Any]] | None = ...,
     with_gripper: bool = ...,
     gripper_max_torque: float = ...,
 ) -> ArmRobot: ...

From e47931d54222495a61c17cd377616c0d0d889b52 Mon Sep 17 00:00:00 2001
From: cc <cc7@duck.com>
Date: Tue, 4 Aug 2026 14:50:02 -0700
Subject: [PATCH 6/8] Improve Cartesian IK command tracking

---
 .../cartesian_ik_task/cartesian_ik_task.py    | 103 ++++++++++++----
 .../cartesian_ik_task/pink_control_ik.py      |   7 ++
 .../test_cartesian_ik_task.py                 |  96 ++++++++++++++-
 .../tasks/eef_twist_task/eef_twist_task.py    |  79 ++++--------
 .../eef_twist_task/test_eef_twist_task.py     |   7 +-
 .../control/tasks/teleop_task/teleop_task.py  | 112 +++++++++---------
 .../tasks/teleop_task/test_teleop_task.py     |  34 ++++++
 .../manipulators/galaxea_a1z/adapter.py       |   7 +-
 .../manipulators/galaxea_a1z/config.py        |  14 +--
 .../manipulators/galaxea_a1z/test_adapter.py  |  26 ++--
 dimos/robot/manipulators/common/blueprints.py |  64 ++++++++--
 .../manipulators/xarm/blueprints/teleop.py    |  21 ++--
 12 files changed, 387 insertions(+), 183 deletions(-)

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 686d7ac8d8..8f62ed564a 100644
--- a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py
+++ b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py
@@ -58,6 +58,32 @@
 logger = setup_logger()
 
 
+def claim_optional_joint(claim: ResourceClaim, joint_name: str | None) -> ResourceClaim:
+    """Extend a task claim with an optional joint without changing its control policy."""
+    if joint_name is None:
+        return claim
+    return ResourceClaim(
+        joints=claim.joints | frozenset([joint_name]),
+        priority=claim.priority,
+        mode=claim.mode,
+    )
+
+
+def append_optional_joint(
+    output: JointCommandOutput | None,
+    joint_name: str | None,
+    position: float,
+) -> JointCommandOutput | None:
+    """Append an optional position-controlled joint to a Cartesian task output."""
+    if output is None or joint_name is None:
+        return output
+    return JointCommandOutput(
+        joint_names=[*output.joint_names, joint_name],
+        positions=[*(output.positions or []), position],
+        mode=output.mode,
+    )
+
+
 @dataclass
 class CartesianIKTaskConfig:
     """Configuration for cartesian IK task.
@@ -67,6 +93,7 @@ class CartesianIKTaskConfig:
         priority: Priority for arbitration (higher wins)
         timeout: If no command received for this many seconds, go inactive (0 = never)
         max_joint_delta_deg: Maximum allowed joint change per tick (safety limit)
+        max_tracking_error_deg: Maximum command-to-feedback error before rebasing
     """
 
     joint_names: list[str]
@@ -74,6 +101,7 @@ class CartesianIKTaskConfig:
     priority: int = 10
     timeout: float = 0.5
     max_joint_delta_deg: float = 15.0  # ~1500°/s at 100Hz
+    max_tracking_error_deg: float = 10.0
     min_dt: FiniteFloat = 1e-4
     max_dt: FiniteFloat = 0.05
 
@@ -93,9 +121,10 @@ class CartesianIKTask(BaseControlTask):
     """Cartesian control task with Pink differential IK.
 
     Accepts streaming cartesian poses via on_cartesian_command() and computes IK
-    internally to output joint commands. By default, Pink re-anchors each solve
-    to the current joint state from CoordinatorState. Specializations can retain
-    a validated command as the next solve seed.
+    internally to output joint commands. Each accepted differential IK result
+    seeds the next solve while measured hardware remains within the configured
+    tracking-error bound. Lagging or stalled hardware automatically rebases the
+    solve to measured state.
 
     Unlike CartesianServoTask (which bypasses joint arbitration), this task
     outputs JointCommandOutput and participates in joint-level arbitration.
@@ -133,6 +162,8 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None:
             raise ValueError("CartesianIKTask timeout must be finite and non-negative")
         if not np.isfinite(config.max_joint_delta_deg) or config.max_joint_delta_deg <= 0.0:
             raise ValueError("CartesianIKTask max_joint_delta_deg must be positive and finite")
+        if not np.isfinite(config.max_tracking_error_deg) or config.max_tracking_error_deg <= 0.0:
+            raise ValueError("CartesianIKTask max_tracking_error_deg must be positive and finite")
 
         self._name = name
         self._config = config
@@ -160,6 +191,7 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None:
         self._target_pose: Pose | PoseStamped | None = None
         self._last_update_time: float = 0.0
         self._active = False
+        self._last_commanded_joints: NDArray[np.float64] | None = None
 
         logger.info(
             f"CartesianIKTask {name} initialized with model: "
@@ -203,6 +235,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
                     )
                     self._active = False
                     self._target_pose = None
+                    self._last_commanded_joints = None
                     self._on_timeout()
                     return None
 
@@ -213,10 +246,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
         if not np.all(np.isfinite(q_measured)):
             logger.error("CartesianIKTask %s: measured joint state is non-finite", self._name)
             return None
-        q_current = self._select_solve_joints(state, q_measured)
-        if q_current.shape != q_measured.shape or not np.all(np.isfinite(q_current)):
-            logger.error("CartesianIKTask %s: solve joint state is invalid", self._name)
-            return self._hold(q_measured)
+        q_current = self._solve_seed(q_measured)
         raw_dt = state.dt
         if not np.isfinite(raw_dt) or raw_dt <= 0.0:
             return self._hold(q_current)
@@ -250,7 +280,9 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
             )
             return self._hold(q_current)
 
-        self._on_solution_accepted(state, q_solution)
+        with self._lock:
+            if self._active:
+                self._last_commanded_joints = q_solution.copy()
         return JointCommandOutput(
             joint_names=self._joint_names_list,
             positions=q_solution.flatten().tolist(),
@@ -275,20 +307,37 @@ def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.float64] |
             positions.append(pos)
         return np.array(positions, dtype=np.float64)
 
-    def _select_solve_joints(
-        self,
-        state: CoordinatorState,
-        q_measured: NDArray[np.float64],
-    ) -> NDArray[np.float64]:
-        """Select the joint state used to warm-start this tick's IK solve."""
-        return q_measured
+    def _solve_seed(self, q_measured: NDArray[np.float64]) -> NDArray[np.float64]:
+        """Return a bounded command seed, rebasing to feedback when tracking diverges."""
+        with self._lock:
+            cached = (
+                None if self._last_commanded_joints is None else self._last_commanded_joints.copy()
+            )
+        if cached is None:
+            return q_measured
+        if cached.shape != q_measured.shape or not np.all(np.isfinite(cached)):
+            logger.error("CartesianIKTask %s: cached joint command is invalid", self._name)
+            self._reset_command_state()
+            return q_measured
+        tracking_error_deg = np.rad2deg(np.abs(cached - q_measured))
+        if np.any(tracking_error_deg > self._config.max_tracking_error_deg):
+            worst_index = int(np.argmax(tracking_error_deg))
+            logger.warning(
+                "CartesianIKTask %s: rebasing solve to measured state; %s tracks %.1f° "
+                "behind command (limit %.1f°)",
+                self._name,
+                self._joint_names_list[worst_index],
+                tracking_error_deg[worst_index],
+                self._config.max_tracking_error_deg,
+            )
+            self._reset_command_state()
+            return q_measured
+        return cached
 
-    def _on_solution_accepted(
-        self,
-        state: CoordinatorState,
-        q_solution: NDArray[np.float64],
-    ) -> None:
-        """Handle a finite, shape-valid, joint-delta-checked IK solution."""
+    def _reset_command_state(self) -> None:
+        """Discard the retained differential-IK command seed."""
+        with self._lock:
+            self._last_commanded_joints = None
 
     def _prepare_target(
         self,
@@ -334,6 +383,7 @@ def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:
             joints: Joints that were preempted
         """
         if joints & self._joint_names:
+            self._reset_command_state()
             logger.warning(
                 f"CartesianIKTask {self._name} preempted by {by_task} on joints {joints}"
             )
@@ -349,6 +399,8 @@ def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool:
             True if accepted
         """
         with self._lock:
+            if not self._active:
+                self._last_commanded_joints = None
             self._target_pose = pose  # Store raw, convert to SE3 in compute()
             self._last_update_time = t_now
             self._active = True
@@ -358,6 +410,7 @@ def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool:
     def start(self) -> None:
         """Activate the task (start accepting and outputting commands)."""
         with self._lock:
+            self._last_commanded_joints = None
             self._active = True
         logger.info(f"CartesianIKTask {self._name} started")
 
@@ -366,6 +419,7 @@ def stop(self) -> None:
         with self._lock:
             self._active = False
             self._target_pose = None
+            self._last_commanded_joints = None
         logger.info(f"CartesianIKTask {self._name} stopped")
 
     def clear(self) -> None:
@@ -373,6 +427,7 @@ def clear(self) -> None:
         with self._lock:
             self._target_pose = None
             self._active = False
+            self._last_commanded_joints = None
         logger.info(f"CartesianIKTask {self._name} cleared")
 
     def is_tracking(self) -> bool:
@@ -411,6 +466,9 @@ def forward_kinematics(self, joint_positions: NDArray[np.float64]) -> pinocchio.
 
 class CartesianIKTaskParams(BaseConfig):
     control_ik: PinkControlIKConfig
+    timeout: float = 0.5
+    max_joint_delta_deg: float = 15.0
+    max_tracking_error_deg: float = 10.0
     min_dt: FiniteFloat = 1e-4
     max_dt: FiniteFloat = 0.05
 
@@ -422,6 +480,9 @@ def create_task(cfg: TaskConfig, hardware: object) -> CartesianIKTask:
         CartesianIKTaskConfig(
             joint_names=cfg.joint_names,
             priority=cfg.priority,
+            timeout=params.timeout,
+            max_joint_delta_deg=params.max_joint_delta_deg,
+            max_tracking_error_deg=params.max_tracking_error_deg,
             min_dt=params.min_dt,
             max_dt=params.max_dt,
             control_ik=params.control_ik,
diff --git a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py
index 771dc5fe76..ed028abc6d 100644
--- a/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py
+++ b/dimos/control/tasks/cartesian_ik_task/pink_control_ik.py
@@ -473,6 +473,13 @@ def _project_position_limits(
         positions: NDArray[np.float64],
         source: str,
     ) -> NDArray[np.float64]:
+        """Clamp small boundary drift but reject materially out-of-limit states.
+
+        Pink requires a valid configuration before it can solve. Floating-point
+        and one-tick integration drift within ``seed_limit_tolerance`` is
+        projected back inside the configured margin; larger violations remain
+        visible as runtime failures so model or feedback problems are not hidden.
+        """
         runtime = self._runtime
         mapping = runtime.mapping
         bounded = positions.copy()
diff --git a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py
index de7adada09..b610fb66c3 100644
--- a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py
+++ b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py
@@ -56,9 +56,9 @@ def _robot(path: Path) -> RobotModelConfig:
     )
 
 
-def _state(t_now: float, dt: float = 0.01) -> CoordinatorState:
+def _state(t_now: float, dt: float = 0.01, position: float = 0.0) -> CoordinatorState:
     return CoordinatorState(
-        joints=JointStateSnapshot(joint_positions={"joint1": 0.0}), t_now=t_now, dt=dt
+        joints=JointStateSnapshot(joint_positions={"joint1": position}), t_now=t_now, dt=dt
     )
 
 
@@ -68,11 +68,15 @@ class _FakeControlIK:
     def __init__(self) -> None:
         self.target: object | None = None
         self.dt: float | None = None
+        self.increment = 0.0
+        self.solve_seeds: list[np.ndarray] = []
 
     def solve(self, target: object, measured: np.ndarray, dt: float) -> ControlIKResult:
         self.target = target
         self.dt = dt
-        return ControlIKResult(measured.copy(), np.zeros(1))
+        self.solve_seeds.append(measured.copy())
+        positions = measured + self.increment
+        return ControlIKResult(positions, positions - measured)
 
 
 def test_cartesian_pipeline_passes_se3_target_and_bounded_dt(tmp_path: Path, mocker) -> None:
@@ -187,3 +191,89 @@ def fail(target: object, measured: np.ndarray, dt: float) -> ControlIKResult:
     hold = task.compute(_state(1.01))
     assert hold is not None
     assert hold.positions == [0.0]
+
+
+def test_cartesian_pipeline_accumulates_from_accepted_commands_while_feedback_tracks(
+    tmp_path: Path, mocker
+) -> None:
+    backend = _FakeControlIK()
+    backend.increment = 0.01
+    mocker.patch(
+        "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik",
+        return_value=backend,
+    )
+    task = CartesianIKTask(
+        "cartesian",
+        CartesianIKTaskConfig(
+            joint_names=["joint1"],
+            control_ik=PinkControlIKConfig(robot_model=_robot(tmp_path / "unused.urdf")),
+            max_tracking_error_deg=10.0,
+        ),
+    )
+    assert task.on_cartesian_command(PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), 1.0)
+
+    first = task.compute(_state(1.01))
+    second = task.compute(_state(1.02))
+
+    assert first is not None
+    assert second is not None
+    assert first.positions == pytest.approx([0.01])
+    assert second.positions == pytest.approx([0.02])
+    np.testing.assert_allclose(backend.solve_seeds, [[0.0], [0.01]])
+
+
+def test_cartesian_pipeline_rebases_when_command_outpaces_feedback(tmp_path: Path, mocker) -> None:
+    backend = _FakeControlIK()
+    backend.increment = 0.1
+    mocker.patch(
+        "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik",
+        return_value=backend,
+    )
+    task = CartesianIKTask(
+        "cartesian",
+        CartesianIKTaskConfig(
+            joint_names=["joint1"],
+            control_ik=PinkControlIKConfig(robot_model=_robot(tmp_path / "unused.urdf")),
+            max_tracking_error_deg=5.0,
+        ),
+    )
+    assert task.on_cartesian_command(PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), 1.0)
+
+    first = task.compute(_state(1.01))
+    second = task.compute(_state(1.02))
+
+    assert first is not None
+    assert second is not None
+    assert first.positions == pytest.approx([0.1])
+    assert second.positions == pytest.approx([0.1])
+    np.testing.assert_allclose(backend.solve_seeds, [[0.0], [0.0]])
+
+
+def test_cartesian_failure_holds_measured_after_tracking_error_rebase(
+    tmp_path: Path, monkeypatch: pytest.MonkeyPatch, mocker
+) -> None:
+    backend = _FakeControlIK()
+    backend.increment = 0.1
+    mocker.patch(
+        "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik",
+        return_value=backend,
+    )
+    task = CartesianIKTask(
+        "cartesian",
+        CartesianIKTaskConfig(
+            joint_names=["joint1"],
+            control_ik=PinkControlIKConfig(robot_model=_robot(tmp_path / "unused.urdf")),
+            max_tracking_error_deg=5.0,
+        ),
+    )
+    assert task.on_cartesian_command(PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), 1.0)
+    assert task.compute(_state(1.01)) is not None
+
+    def fail(target: object, measured: np.ndarray, dt: float) -> ControlIKResult:
+        raise IKControlRuntimeError("solver failed")
+
+    monkeypatch.setattr(backend, "solve", fail)
+    hold = task.compute(_state(1.02))
+
+    assert hold is not None
+    assert hold.positions == [0.0]
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 e6e7785217..9843574298 100644
--- a/dimos/control/tasks/eef_twist_task/eef_twist_task.py
+++ b/dimos/control/tasks/eef_twist_task/eef_twist_task.py
@@ -22,22 +22,20 @@
 
 import numpy as np
 import pinocchio
-from pydantic import FiniteFloat
 
 from dimos.control.coordinator import TaskConfig
 from dimos.control.task import CoordinatorState, JointCommandOutput, ResourceClaim
 from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import (
     CartesianIKTask,
     CartesianIKTaskConfig,
+    CartesianIKTaskParams,
+    append_optional_joint,
+    claim_optional_joint,
 )
-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
 
 if TYPE_CHECKING:
-    from numpy.typing import NDArray
-
     from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped
     from dimos.msgs.std_msgs.Bool import Bool
 
@@ -62,19 +60,12 @@ def __init__(self, name: str, config: EEFTwistTaskConfig) -> None:
         super().__init__(name, config)
         self._twist_lock = threading.Lock()
         self._latest_twist: TwistStamped | None = None
-        self._last_commanded_joints: NDArray[np.float64] | None = None
         self._estopped = False
         self._gripper_target = config.gripper_open_pos
+        self._gripper_active = config.gripper_joint is not None
 
     def claim(self) -> ResourceClaim:
-        claim = super().claim()
-        if self._config.gripper_joint is None:
-            return claim
-        return ResourceClaim(
-            joints=claim.joints | frozenset([self._config.gripper_joint]),
-            priority=claim.priority,
-            mode=claim.mode,
-        )
+        return claim_optional_joint(super().claim(), self._config.gripper_joint)
 
     def is_active(self) -> bool:
         with self._twist_lock:
@@ -105,15 +96,18 @@ def on_ee_twist_command(self, twist: TwistStamped, t_now: float) -> bool:
                 return False
             if np.allclose(values, 0.0):
                 self._latest_twist = None
-                self._last_commanded_joints = None
                 cleared = True
             else:
                 self._latest_twist = twist
                 cleared = False
+        if cleared:
+            self._reset_command_state()
         if cleared and self._config.gripper_joint is None:
             super().clear()
             return True
         with self._lock:
+            if not self._active:
+                self._last_commanded_joints = None
             self._last_update_time = t_now
             self._active = True
         return True
@@ -127,7 +121,10 @@ def on_gripper_command(self, msg: Bool, t_now: float) -> bool:
             self._gripper_target = (
                 self._config.gripper_closed_pos if msg.data else self._config.gripper_open_pos
             )
+            self._gripper_active = True
         with self._lock:
+            if not self._active:
+                self._last_commanded_joints = None
             self._last_update_time = t_now
             self._active = True
         return True
@@ -137,18 +134,19 @@ def set_estop(self, estopped: bool) -> None:
             self._estopped = estopped
             if estopped:
                 self._latest_twist = None
-                self._last_commanded_joints = None
+                self._gripper_active = False
+        if estopped:
+            super().clear()
 
     def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
         output = super().compute(state)
-        if output is None or self._config.gripper_joint is None:
-            return output
         with self._twist_lock:
             gripper_target = self._gripper_target
-        return JointCommandOutput(
-            joint_names=[*output.joint_names, self._config.gripper_joint],
-            positions=[*(output.positions or []), gripper_target],
-            mode=output.mode,
+            gripper_joint = self._config.gripper_joint if self._gripper_active else None
+        return append_optional_joint(
+            output,
+            gripper_joint,
+            gripper_target,
         )
 
     def _prepare_target(
@@ -171,55 +169,25 @@ def _prepare_target(
             return None
         return pose
 
-    def _select_solve_joints(
-        self,
-        state: CoordinatorState,
-        q_measured: NDArray[np.float64],
-    ) -> NDArray[np.float64]:
-        with self._twist_lock:
-            if self._last_commanded_joints is None:
-                return q_measured
-            return self._last_commanded_joints.copy()
-
-    def _on_solution_accepted(
-        self,
-        state: CoordinatorState,
-        q_solution: NDArray[np.float64],
-    ) -> None:
-        with self._twist_lock:
-            if self._latest_twist is not None and not self._estopped:
-                self._last_commanded_joints = q_solution.copy()
-
-    def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:
-        if joints & self._joint_names:
-            with self._twist_lock:
-                self._last_commanded_joints = None
-        super().on_preempted(by_task, joints)
-
     def stop(self) -> None:
         with self._twist_lock:
             self._latest_twist = None
-            self._last_commanded_joints = None
+            self._gripper_active = False
         super().stop()
 
     def _on_timeout(self) -> None:
         with self._twist_lock:
             self._latest_twist = None
-            self._last_commanded_joints = None
 
     def clear(self) -> None:
         with self._twist_lock:
             self._latest_twist = None
-            self._last_commanded_joints = None
+            self._gripper_active = False
         super().clear()
 
 
-class EEFTwistTaskParams(BaseConfig):
+class EEFTwistTaskParams(CartesianIKTaskParams):
     timeout: float = 0.3
-    max_joint_delta_deg: float = 15.0
-    min_dt: FiniteFloat = 1e-4
-    max_dt: FiniteFloat = 0.05
-    control_ik: PinkControlIKConfig
     gripper_joint: str | None = None
     gripper_open_pos: float = 0.0
     gripper_closed_pos: float = 0.0
@@ -234,6 +202,7 @@ def create_task(cfg: TaskConfig, hardware: object) -> EEFTwistTask:
             priority=cfg.priority,
             timeout=params.timeout,
             max_joint_delta_deg=params.max_joint_delta_deg,
+            max_tracking_error_deg=params.max_tracking_error_deg,
             min_dt=params.min_dt,
             max_dt=params.max_dt,
             control_ik=params.control_ik,
diff --git a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py
index 56d6040e4b..c07bb26ca6 100644
--- a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py
+++ b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py
@@ -317,6 +317,9 @@ def test_commands_during_estop_are_rejected(gripper_task: EEFTwistTask) -> None:
     assert not gripper_task.on_gripper_command(Bool(data=True), 1.0)
 
     gripper_task.set_estop(False)
-    output = gripper_task.compute(_state(2.0, positions=[0.1, 0.2, 0.3]))
+    assert gripper_task.compute(_state(2.0, positions=[0.1, 0.2, 0.3])) is None
+
+    assert gripper_task.on_gripper_command(Bool(data=True), 2.1)
+    output = gripper_task.compute(_state(2.2, positions=[0.1, 0.2, 0.3]))
     assert output is not None
-    assert output.positions[-1] == 0.85
+    assert output.positions[-1] == 0.0
diff --git a/dimos/control/tasks/teleop_task/teleop_task.py b/dimos/control/tasks/teleop_task/teleop_task.py
index 7de544f377..ba20c62749 100644
--- a/dimos/control/tasks/teleop_task/teleop_task.py
+++ b/dimos/control/tasks/teleop_task/teleop_task.py
@@ -17,6 +17,7 @@
 from __future__ import annotations
 
 from dataclasses import dataclass
+from enum import Enum, auto
 from typing import TYPE_CHECKING, Literal
 
 import numpy as np
@@ -28,9 +29,11 @@
 from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import (
     CartesianIKTask,
     CartesianIKTaskConfig,
+    CartesianIKTaskParams,
+    append_optional_joint,
+    claim_optional_joint,
 )
 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
 
 if TYPE_CHECKING:
@@ -43,6 +46,12 @@
 logger = setup_logger()
 
 
+class _EngagementState(Enum):
+    DISENGAGED = auto()
+    ENGAGED = auto()
+    WAITING_FOR_RELEASE = auto()
+
+
 class TeleopControlIKConfig(PinkControlIKConfig):
     """Pink control policy for engagement-relative arm teleoperation."""
 
@@ -75,21 +84,15 @@ def __init__(self, name: str, config: TeleopIKTaskConfig) -> None:
             raise ValueError(f"TeleopIKTask '{name}' requires hand='left' or 'right'")
         super().__init__(name, config)
         self._initial_ee_pose: pinocchio.SE3 | None = None
-        self._last_commanded_joints: NDArray[np.float64] | None = None
-        self._prev_primary = False
+        self._engagement = _EngagementState.DISENGAGED
+        self._primary_down = False
         self._estopped = False
         self._gripper_target = config.gripper_open_pos
+        self._gripper_active = config.gripper_joint is not None
 
     def claim(self) -> ResourceClaim:
         """Claim arm joints and the optional gripper joint."""
-        claim = super().claim()
-        if self._config.gripper_joint is None:
-            return claim
-        return ResourceClaim(
-            joints=claim.joints | frozenset([self._config.gripper_joint]),
-            priority=claim.priority,
-            mode=claim.mode,
-        )
+        return claim_optional_joint(super().claim(), self._config.gripper_joint)
 
     def is_active(self) -> bool:
         """Run only when a non-E-STOPped pose target is active."""
@@ -105,30 +108,18 @@ def set_estop(self, estopped: bool) -> None:
         with self._lock:
             self._estopped = estopped
             if estopped:
+                self._engagement = _EngagementState.WAITING_FOR_RELEASE
                 self._active = False
                 self._target_pose = None
                 self._initial_ee_pose = None
                 self._last_commanded_joints = None
-                self._prev_primary = False
-
-    def _select_solve_joints(
-        self,
-        state: CoordinatorState,
-        q_measured: NDArray[np.float64],
-    ) -> NDArray[np.float64]:
-        with self._lock:
-            if self._last_commanded_joints is None:
-                return q_measured
-            return self._last_commanded_joints.copy()
-
-    def _on_solution_accepted(
-        self,
-        state: CoordinatorState,
-        q_solution: NDArray[np.float64],
-    ) -> None:
-        with self._lock:
-            if not self._estopped and self._active and self._target_pose is not None:
-                self._last_commanded_joints = q_solution.copy()
+                self._gripper_active = False
+            else:
+                self._engagement = (
+                    _EngagementState.WAITING_FOR_RELEASE
+                    if self._primary_down
+                    else _EngagementState.DISENGAGED
+                )
 
     def _prepare_target(
         self,
@@ -170,17 +161,12 @@ def _prepare_target(
     def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
         """Run the inherited Pink solve and append the optional gripper target."""
         output = super().compute(state)
-        if output is None or self._config.gripper_joint is None:
-            return output
         with self._lock:
             if self._estopped:
                 return None
             gripper_target = self._gripper_target
-        return JointCommandOutput(
-            joint_names=[*output.joint_names, self._config.gripper_joint],
-            positions=[*(output.positions or []), gripper_target],
-            mode=output.mode,
-        )
+            gripper_joint = self._config.gripper_joint if self._gripper_active else None
+        return append_optional_joint(output, gripper_joint, gripper_target)
 
     def on_buttons(self, msg: Buttons) -> bool:
         """Use the configured primary button as press-and-hold engagement."""
@@ -189,17 +175,27 @@ def on_buttons(self, msg: Buttons) -> bool:
         trigger = msg.left_trigger_analog if is_left else msg.right_trigger_analog
 
         with self._lock:
+            was_primary_down = self._primary_down
+            self._primary_down = primary
             if self._estopped:
                 return False
-            if primary and not self._prev_primary:
+            if self._engagement is _EngagementState.WAITING_FOR_RELEASE:
+                if not primary:
+                    self._engagement = _EngagementState.DISENGAGED
+            elif (
+                self._engagement is _EngagementState.DISENGAGED and primary and not was_primary_down
+            ):
+                self._engagement = _EngagementState.ENGAGED
+                self._active = False
+                self._target_pose = None
                 self._initial_ee_pose = None
                 self._last_commanded_joints = None
-            elif not primary and self._prev_primary:
+            elif self._engagement is _EngagementState.ENGAGED and not primary:
+                self._engagement = _EngagementState.DISENGAGED
                 self._active = False
                 self._target_pose = None
                 self._initial_ee_pose = None
                 self._last_commanded_joints = None
-            self._prev_primary = primary
 
         if self._config.gripper_joint is not None:
             self.on_gripper_trigger(trigger)
@@ -210,10 +206,12 @@ def on_teleop_buttons(self, msg: Buttons, t_now: float) -> bool:
         return self.on_buttons(msg)
 
     def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool:
-        """Accept an engagement-relative pose delta unless E-STOP is latched."""
+        """Accept an engagement-relative pose delta only while its button is held."""
         with self._lock:
-            if self._estopped:
+            if self._estopped or self._engagement is not _EngagementState.ENGAGED:
                 return False
+            if not self._active:
+                self._last_commanded_joints = None
             self._target_pose = pose
             self._last_update_time = t_now
             self._active = True
@@ -232,19 +230,17 @@ def on_gripper_trigger(self, value: float, _t_now: float = 0.0) -> bool:
             if self._estopped:
                 return False
             self._gripper_target = position
+            self._gripper_active = True
         return True
 
     def _on_timeout(self) -> None:
         """Discard the baseline while the parent holds the task lock."""
         self._initial_ee_pose = None
-        self._last_commanded_joints = None
-        self._prev_primary = False
-
-    def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:
-        if joints & self._joint_names:
-            with self._lock:
-                self._last_commanded_joints = None
-        super().on_preempted(by_task, joints)
+        self._engagement = (
+            _EngagementState.WAITING_FOR_RELEASE
+            if self._primary_down
+            else _EngagementState.DISENGAGED
+        )
 
     def stop(self) -> None:
         """Stop output and discard engagement-relative state."""
@@ -254,7 +250,9 @@ def stop(self) -> None:
             self._target_pose = None
             self._initial_ee_pose = None
             self._last_commanded_joints = None
-            self._prev_primary = False
+            self._engagement = _EngagementState.DISENGAGED
+            self._primary_down = False
+            self._gripper_active = False
 
     def clear(self) -> None:
         """Clear output and discard engagement-relative state."""
@@ -264,16 +262,15 @@ def clear(self) -> None:
             self._target_pose = None
             self._initial_ee_pose = None
             self._last_commanded_joints = None
-            self._prev_primary = False
+            self._engagement = _EngagementState.DISENGAGED
+            self._primary_down = False
+            self._gripper_active = False
 
 
-class TeleopIKTaskParams(BaseConfig):
+class TeleopIKTaskParams(CartesianIKTaskParams):
     control_ik: TeleopControlIKConfig
     hand: Literal["left", "right"] | None = None
-    timeout: float = 0.5
     max_joint_delta_deg: float = 5.0
-    min_dt: FiniteFloat = 1e-4
-    max_dt: FiniteFloat = 0.05
     gripper_joint: str | None = None
     gripper_open_pos: float = 0.0
     gripper_closed_pos: float = 0.0
@@ -290,6 +287,7 @@ def create_task(cfg: TaskConfig, hardware: object) -> TeleopIKTask:
             priority=cfg.priority,
             timeout=params.timeout,
             max_joint_delta_deg=params.max_joint_delta_deg,
+            max_tracking_error_deg=params.max_tracking_error_deg,
             min_dt=params.min_dt,
             max_dt=params.max_dt,
             hand=params.hand,
diff --git a/dimos/control/tasks/teleop_task/test_teleop_task.py b/dimos/control/tasks/teleop_task/test_teleop_task.py
index 56aed39aaf..afd20b54fa 100644
--- a/dimos/control/tasks/teleop_task/test_teleop_task.py
+++ b/dimos/control/tasks/teleop_task/test_teleop_task.py
@@ -123,6 +123,16 @@ def _delta(
     )
 
 
+def _buttons(primary: bool) -> Buttons:
+    buttons = Buttons()
+    buttons.right_primary = primary
+    return buttons
+
+
+def _engage(task: TeleopIKTask, t_now: float = 0.0) -> None:
+    assert task.on_teleop_buttons(_buttons(True), t_now)
+
+
 @pytest.fixture
 def fake_ik(mocker: MockerFixture) -> _FakePinkIK:
     backend = _FakePinkIK()
@@ -166,6 +176,7 @@ def gripper_task(tmp_path: Path, fake_ik: _FakePinkIK) -> TeleopIKTask:
 def test_delta_is_composed_with_one_measured_engagement_baseline(
     task: TeleopIKTask, fake_ik: _FakePinkIK
 ) -> None:
+    _engage(task)
     assert task.on_cartesian_command(_delta(), t_now=1.0)
 
     first = task.compute(_state(1.01, (1.0, 2.0), dt=1.0))
@@ -190,6 +201,7 @@ def test_teleop_ik_iterates_from_last_command_when_feedback_lags(
     task: TeleopIKTask, fake_ik: _FakePinkIK
 ) -> None:
     fake_ik.increment = np.array([0.01, 0.0], dtype=np.float64)
+    _engage(task)
     assert task.on_cartesian_command(_delta(), t_now=1.0)
 
     first = task.compute(_state(1.01))
@@ -208,6 +220,7 @@ def test_teleop_ik_iterates_from_last_command_when_feedback_lags(
 def test_missing_joint_state_defers_baseline_and_output(
     task: TeleopIKTask, fake_ik: _FakePinkIK
 ) -> None:
+    _engage(task)
     assert task.on_cartesian_command(_delta(), t_now=1.0)
 
     output = task.compute(_state(1.01, (0.0,)))
@@ -220,6 +233,7 @@ def test_missing_joint_state_defers_baseline_and_output(
 def test_solver_failure_and_excessive_delta_return_measured_hold(
     task: TeleopIKTask, fake_ik: _FakePinkIK
 ) -> None:
+    _engage(task)
     assert task.on_cartesian_command(_delta(), t_now=1.0)
     fake_ik.raise_runtime = True
     failed = task.compute(_state(1.01, (0.4, 0.5)))
@@ -254,11 +268,15 @@ def test_release_timeout_stop_and_clear_force_fresh_baselines(
     np.testing.assert_allclose(fake_ik.solve_calls[-1][1], [0.1, 0.2])
     assert task.compute(_state(3.0, (0.1, 0.2))) is None
 
+    assert task.on_teleop_buttons(released, 3.1)
+    assert task.on_teleop_buttons(pressed, 3.2)
     assert task.on_cartesian_command(_delta(), 4.0)
     assert task.compute(_state(4.01, (0.2, 0.3))) is not None
     np.testing.assert_allclose(fake_ik.solve_calls[-1][1], [0.2, 0.3])
     task.stop()
     task.start()
+    assert task.on_teleop_buttons(released, 4.1)
+    assert task.on_teleop_buttons(pressed, 4.2)
     assert task.on_cartesian_command(_delta(), 5.0)
     assert task.compute(_state(5.01, (0.3, 0.4))) is not None
     np.testing.assert_allclose(fake_ik.solve_calls[-1][1], [0.3, 0.4])
@@ -270,6 +288,7 @@ def test_preemption_discards_teleop_commanded_solve_seed(
     task: TeleopIKTask, fake_ik: _FakePinkIK
 ) -> None:
     fake_ik.increment = np.array([0.01, 0.0], dtype=np.float64)
+    _engage(task)
     assert task.on_cartesian_command(_delta(), t_now=1.0)
     assert task.compute(_state(1.01)) is not None
 
@@ -284,6 +303,7 @@ def test_preemption_discards_teleop_commanded_solve_seed(
 def test_estop_rejects_commands_and_never_replays_them(
     gripper_task: TeleopIKTask, fake_ik: _FakePinkIK
 ) -> None:
+    _engage(gripper_task)
     assert gripper_task.on_cartesian_command(_delta(), 1.0)
     assert gripper_task.compute(_state(1.01)) is not None
 
@@ -294,6 +314,9 @@ def test_estop_rejects_commands_and_never_replays_them(
 
     gripper_task.set_estop(False)
     assert gripper_task.compute(_state(2.01)) is None
+    assert not gripper_task.on_cartesian_command(_delta(), 2.5)
+    assert gripper_task.on_teleop_buttons(_buttons(False), 2.6)
+    assert gripper_task.on_teleop_buttons(_buttons(True), 2.7)
     assert gripper_task.on_cartesian_command(_delta(), 3.0)
     assert gripper_task.compute(_state(3.01, (0.2, 0.3))) is not None
     assert len(fake_ik.fk_calls) == 2
@@ -302,6 +325,7 @@ def test_estop_rejects_commands_and_never_replays_them(
 def test_gripper_claim_interpolation_and_hold_output(
     gripper_task: TeleopIKTask, fake_ik: _FakePinkIK
 ) -> None:
+    _engage(gripper_task)
     assert gripper_task.on_gripper_trigger(0.25)
     assert gripper_task.on_cartesian_command(_delta(), 1.0)
     fake_ik.raise_runtime = True
@@ -314,6 +338,16 @@ def test_gripper_claim_interpolation_and_hold_output(
     assert output.positions == pytest.approx([0.4, 0.5, 0.6])
 
 
+def test_pose_is_rejected_before_engage_and_after_release(task: TeleopIKTask) -> None:
+    assert not task.on_cartesian_command(_delta(), 1.0)
+
+    _engage(task, 2.0)
+    assert task.on_cartesian_command(_delta(), 2.1)
+    assert task.on_teleop_buttons(_buttons(False), 2.2)
+
+    assert not task.on_cartesian_command(_delta(), 2.3)
+
+
 def test_factory_requires_pink_configuration_and_matching_model(tmp_path: Path) -> None:
     legacy = TaskConfig(
         name="teleop",
diff --git a/dimos/hardware/manipulators/galaxea_a1z/adapter.py b/dimos/hardware/manipulators/galaxea_a1z/adapter.py
index 693e27273c..1745b1692f 100644
--- a/dimos/hardware/manipulators/galaxea_a1z/adapter.py
+++ b/dimos/hardware/manipulators/galaxea_a1z/adapter.py
@@ -185,7 +185,7 @@ def connect(self) -> bool:
 
     def _create_robot(self) -> ArmRobot:
         gripper = self._config.gripper
-        robot = get_a1z_robot(
+        return get_a1z_robot(
             can_channel=self._can_channel,
             gravity_comp_factor=self._config.gravity_comp_factor,
             zero_gravity_mode=self._config.teaching is not None,
@@ -196,11 +196,6 @@ def _create_robot(self) -> ArmRobot:
             with_gripper=gripper is not None,
             gripper_max_torque=gripper.max_torque if gripper else 0.5,
         )
-        if gripper is not None:
-            # The pinned SDK exposes the velocity on Gripper but not through
-            # get_a1z_robot(). Keep the override here until the factory does.
-            robot.gripper._max_vel = gripper.max_velocity_rad_s
-        return robot
 
     def disconnect(self) -> None:
         """Stop the control loop, disable motors, and close the CAN bus.
diff --git a/dimos/hardware/manipulators/galaxea_a1z/config.py b/dimos/hardware/manipulators/galaxea_a1z/config.py
index c18f3f434c..9bcc85abe5 100644
--- a/dimos/hardware/manipulators/galaxea_a1z/config.py
+++ b/dimos/hardware/manipulators/galaxea_a1z/config.py
@@ -22,8 +22,8 @@
 import attrs
 
 _A1Z_DOF = 6
-_A1Z_DEFAULT_KP = (80.0, 80.0, 80.0, 50.0, 10.0, 10.0)
-_A1Z_DEFAULT_KD = (3.0, 3.0, 3.0, 0.7, 0.2, 0.2)
+_A1Z_DEFAULT_KP = (80.0, 80.0, 80.0, 50.0, 20.0, 20.0)
+_A1Z_DEFAULT_KD = (3.0, 3.0, 3.0, 0.7, 0.4, 0.4)
 
 
 def _joint_gains(
@@ -62,7 +62,7 @@ def _validate_optional_path(
 
 @attrs.frozen(slots=False)
 class A1ZGripperConfig:
-    """G1Z gripper configuration."""
+    """A1Z gripper configuration."""
 
     max_torque: float = attrs.field(
         default=0.5,
@@ -74,14 +74,6 @@ class A1ZGripperConfig:
         converter=float,
         validator=attrs.validators.gt(0.0),
     )
-    max_velocity_rad_s: float = attrs.field(
-        default=10.0,
-        converter=float,
-        validator=attrs.validators.and_(
-            attrs.validators.gt(0.0),
-            attrs.validators.le(100.0),
-        ),
-    )
 
 
 @attrs.frozen(slots=False)
diff --git a/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py b/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py
index 81a9d681e2..afc958d153 100644
--- a/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py
+++ b/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py
@@ -52,7 +52,6 @@ def disable(self) -> None:
 class _FakeGripper:
     def __init__(self) -> None:
         self._motor = _FakeMotor()
-        self._max_vel = 10.0
         self.feedback_fraction = 0.0
 
     def get_feedback_norm(self) -> float:
@@ -241,7 +240,6 @@ def _connected_adapter(module: ModuleType, **kwargs: Any) -> tuple[Any, _FakeArm
         A1ZGripperConfig(
             max_torque=kwargs.pop("gripper_max_torque", 0.5),
             max_opening_m=kwargs.pop("gripper_max_opening_m", 0.1),
-            max_velocity_rad_s=kwargs.pop("gripper_max_velocity_rad_s", 10.0),
         )
         if gripper_enabled
         else None
@@ -252,8 +250,8 @@ def _connected_adapter(module: ModuleType, **kwargs: Any) -> tuple[Any, _FakeArm
     config = A1ZConfig(
         gravity_comp_factor=kwargs.pop("gravity_comp_factor", 1.0),
         urdf_path=kwargs.pop("urdf_path", None),
-        default_kp=kwargs.pop("default_kp", (80.0, 80.0, 80.0, 50.0, 10.0, 10.0)),
-        default_kd=kwargs.pop("default_kd", (3.0, 3.0, 3.0, 0.7, 0.2, 0.2)),
+        default_kp=kwargs.pop("default_kp", (80.0, 80.0, 80.0, 50.0, 20.0, 20.0)),
+        default_kd=kwargs.pop("default_kd", (3.0, 3.0, 3.0, 0.7, 0.4, 0.4)),
         gripper=gripper,
         teaching=teaching,
     )
@@ -273,11 +271,18 @@ def test_connect_opens_bus_without_powering_motors(
     assert not adapter.read_enabled()
 
 
+def test_default_control_gains_use_stiffer_wrist_profile() -> None:
+    config = A1ZConfig()
+
+    assert config.default_kp == pytest.approx((80.0, 80.0, 80.0, 50.0, 20.0, 20.0))
+    assert config.default_kd == pytest.approx((3.0, 3.0, 3.0, 0.7, 0.4, 0.4))
+
+
 def test_connect_forwards_configured_arm_gains_to_sdk(
     a1z_adapter_module: ModuleType,
 ) -> None:
-    kp = (80.0, 80.0, 80.0, 50.0, 10.0, 10.0)
-    kd = (3.0, 3.0, 3.0, 0.7, 0.2, 0.2)
+    kp = (80.0, 80.0, 80.0, 50.0, 20.0, 20.0)
+    kd = (3.0, 3.0, 3.0, 0.7, 0.4, 0.4)
 
     _, robot = _connected_adapter(a1z_adapter_module, default_kp=kp, default_kd=kd)
 
@@ -523,18 +528,13 @@ def test_gripper_round_trips_meters_to_normalized(
     assert robot.gripper_fraction == pytest.approx(1.0)
 
 
-def test_connect_applies_configured_gripper_velocity(
+def test_connect_applies_configured_gripper_force(
     a1z_adapter_module: ModuleType,
 ) -> None:
-    adapter, robot = _connected_adapter(
-        a1z_adapter_module,
-        gripper=True,
-        gripper_max_velocity_rad_s=24.0,
-    )
+    adapter, robot = _connected_adapter(a1z_adapter_module, gripper=True)
 
     assert adapter.is_connected()
     assert robot.factory_kwargs["gripper_max_torque"] == pytest.approx(0.5)
-    assert robot.gripper._max_vel == pytest.approx(24.0)
 
 
 def test_configured_gripper_free_drive_tracks_adapter_lifecycle(
diff --git a/dimos/robot/manipulators/common/blueprints.py b/dimos/robot/manipulators/common/blueprints.py
index 66120cb279..d62c01c264 100644
--- a/dimos/robot/manipulators/common/blueprints.py
+++ b/dimos/robot/manipulators/common/blueprints.py
@@ -16,8 +16,8 @@
 
 from __future__ import annotations
 
-from collections.abc import Mapping, Sequence
-from typing import Any
+from collections.abc import Sequence
+from typing import Any, TypedDict
 
 from dimos.control.components import HardwareComponent
 from dimos.control.coordinator import ControlCoordinator, TaskConfig
@@ -33,6 +33,32 @@
 )
 
 
+class PinkControlIKOverrides(TypedDict, total=False):
+    """Pink tuning values that may be overridden by a manipulator blueprint."""
+
+    solver: str
+    max_velocity: float
+    lm_damping: float
+    task_gain: float
+    position_cost: float
+    orientation_cost: float
+    posture_cost: float
+    joint_centering_cost: float
+    damping_cost: float
+    position_limit_margin: float
+    seed_limit_tolerance: float
+    reference_q: list[float] | None
+    qpsolver_options: dict[str, float]
+
+
+class GripperTaskOverrides(TypedDict, total=False):
+    """Optional gripper fields shared by teleop and EEF-twist tasks."""
+
+    gripper_joint: str
+    gripper_open_pos: float
+    gripper_closed_pos: float
+
+
 def trajectory_task(
     hardware: HardwareComponent,
     *additional_hardware: HardwareComponent,
@@ -60,7 +86,7 @@ def trajectory_task(
 def _resolve_control_ik(
     hardware: HardwareComponent,
     robot_model: RobotModelConfig,
-    control_ik: Mapping[str, object] | None,
+    control_ik: PinkControlIKOverrides | None,
 ) -> dict[str, object]:
     coordinator_joints = robot_model.get_coordinator_joint_names()
     if hardware.joints != coordinator_joints:
@@ -75,9 +101,12 @@ def cartesian_ik_task(
     *,
     name: str = CARTESIAN_IK_TASK_NAME,
     priority: int = 10,
+    timeout: float = 0.5,
+    max_joint_delta_deg: float = 15.0,
+    max_tracking_error_deg: float = 10.0,
     min_dt: float = 1e-4,
     max_dt: float = 0.05,
-    control_ik: Mapping[str, object] | None = None,
+    control_ik: PinkControlIKOverrides | None = None,
     robot_model: RobotModelConfig,
 ) -> TaskConfig:
     resolved_control_ik = _resolve_control_ik(hardware, robot_model, control_ik)
@@ -88,6 +117,9 @@ def cartesian_ik_task(
         priority=priority,
         params={
             "control_ik": resolved_control_ik,
+            "timeout": timeout,
+            "max_joint_delta_deg": max_joint_delta_deg,
+            "max_tracking_error_deg": max_tracking_error_deg,
             "min_dt": min_dt,
             "max_dt": max_dt,
         },
@@ -99,15 +131,21 @@ def eef_twist_task(
     *,
     name: str = EEF_TWIST_TASK_NAME,
     priority: int = 10,
+    timeout: float = 0.3,
+    max_joint_delta_deg: float = 15.0,
+    max_tracking_error_deg: float = 10.0,
     min_dt: float = 1e-4,
     max_dt: float = 0.05,
-    control_ik: Mapping[str, object] | None = None,
+    control_ik: PinkControlIKOverrides | None = None,
     robot_model: RobotModelConfig,
-    params: Mapping[str, object] | None = None,
+    params: GripperTaskOverrides | None = None,
 ) -> TaskConfig:
     resolved_control_ik = _resolve_control_ik(hardware, robot_model, control_ik)
     task_params: dict[str, object] = {
         "control_ik": resolved_control_ik,
+        "timeout": timeout,
+        "max_joint_delta_deg": max_joint_delta_deg,
+        "max_tracking_error_deg": max_tracking_error_deg,
         "min_dt": min_dt,
         "max_dt": max_dt,
     }
@@ -129,13 +167,23 @@ def teleop_ik_task(
     name: str,
     robot_model: RobotModelConfig,
     priority: int = 10,
-    control_ik: Mapping[str, object] | None = None,
-    params: Mapping[str, object] | None = None,
+    timeout: float = 0.5,
+    max_joint_delta_deg: float = 5.0,
+    max_tracking_error_deg: float = 10.0,
+    min_dt: float = 1e-4,
+    max_dt: float = 0.05,
+    control_ik: PinkControlIKOverrides | None = None,
+    params: GripperTaskOverrides | None = None,
 ) -> TaskConfig:
     resolved_control_ik = _resolve_control_ik(hardware, robot_model, control_ik)
     task_params: dict[str, object] = {
         "control_ik": resolved_control_ik,
         "hand": hand,
+        "timeout": timeout,
+        "max_joint_delta_deg": max_joint_delta_deg,
+        "max_tracking_error_deg": max_tracking_error_deg,
+        "min_dt": min_dt,
+        "max_dt": max_dt,
     }
     if params:
         task_params.update(params)
diff --git a/dimos/robot/manipulators/xarm/blueprints/teleop.py b/dimos/robot/manipulators/xarm/blueprints/teleop.py
index 57ae65de3e..ef28d2274f 100644
--- a/dimos/robot/manipulators/xarm/blueprints/teleop.py
+++ b/dimos/robot/manipulators/xarm/blueprints/teleop.py
@@ -16,6 +16,8 @@
 
 from __future__ import annotations
 
+from typing import cast
+
 from dimos.control.coordinator import ControlCoordinator, TaskConfig
 from dimos.core.coordination.blueprints import autoconnect
 from dimos.core.global_config import global_config
@@ -23,6 +25,7 @@
 from dimos.manipulation.manipulation_module import ManipulationModule
 from dimos.msgs.sensor_msgs.JointState import JointState
 from dimos.robot.manipulators.common.blueprints import (
+    GripperTaskOverrides,
     eef_twist_task,
     teleop_ik_task,
     trajectory_task,
@@ -44,7 +47,7 @@
 _xarm7_hw = xarm7_hardware("arm", gripper=True, mock_without_address=True)
 _xarm6_control_model = make_xarm6_model_config(add_gripper=False)
 _xarm7_control_model = make_xarm7_model_config(add_gripper=False)
-_xarm_eef_params = {**XARM_GRIPPER_PARAMS, "timeout": 0.0}
+_xarm_gripper_params = cast("GripperTaskOverrides", XARM_GRIPPER_PARAMS)
 
 keyboard_teleop_xarm6 = autoconnect(
     KeyboardTeleopModule.blueprint(),
@@ -57,7 +60,8 @@
             eef_twist_task(
                 _xarm6_hw,
                 robot_model=_xarm6_control_model,
-                params=_xarm_eef_params,
+                timeout=0.0,
+                params=_xarm_gripper_params,
             )
         ],
     ),
@@ -78,7 +82,8 @@
             eef_twist_task(
                 _xarm7_hw,
                 robot_model=_xarm7_control_model,
-                params=_xarm_eef_params,
+                timeout=0.0,
+                params=_xarm_gripper_params,
             )
         ],
     ),
@@ -177,13 +182,14 @@ class _XArm7TeleopCoordinator(ControlCoordinator):
                 name="teleop_xarm",
                 robot_model=_xarm7_control_model,
                 priority=20,
-                params=XARM_GRIPPER_PARAMS,
+                params=_xarm_gripper_params,
             ),
             eef_twist_task(
                 _xarm7_teleop_hw,
                 robot_model=_xarm7_control_model,
                 priority=10,
-                params=_xarm_eef_params,
+                timeout=0.0,
+                params=_xarm_gripper_params,
             ),
             trajectory_task(_xarm7_teleop_hw),
         ],
@@ -205,13 +211,14 @@ class _XArm7TeleopCoordinator(ControlCoordinator):
                 name="teleop_xarm",
                 robot_model=_xarm6_control_model,
                 priority=20,
-                params=XARM_GRIPPER_PARAMS,
+                params=_xarm_gripper_params,
             ),
             eef_twist_task(
                 _xarm6_teleop_hw,
                 robot_model=_xarm6_control_model,
                 priority=10,
-                params=_xarm_eef_params,
+                timeout=0.0,
+                params=_xarm_gripper_params,
             ),
             trajectory_task(_xarm6_teleop_hw),
         ],

From bc3fd674fa460aafc6d9ff6a9fb544913b04dd76 Mon Sep 17 00:00:00 2001
From: cc <cc7@duck.com>
Date: Tue, 4 Aug 2026 14:57:21 -0700
Subject: [PATCH 7/8] chore: simplify a1z config

---
 .../manipulators/galaxea_a1z/config.py        | 37 +------------------
 1 file changed, 2 insertions(+), 35 deletions(-)

diff --git a/dimos/hardware/manipulators/galaxea_a1z/config.py b/dimos/hardware/manipulators/galaxea_a1z/config.py
index 9bcc85abe5..f73a410541 100644
--- a/dimos/hardware/manipulators/galaxea_a1z/config.py
+++ b/dimos/hardware/manipulators/galaxea_a1z/config.py
@@ -16,37 +16,10 @@
 
 from __future__ import annotations
 
-import math
 from pathlib import Path
 
 import attrs
 
-_A1Z_DOF = 6
-_A1Z_DEFAULT_KP = (80.0, 80.0, 80.0, 50.0, 20.0, 20.0)
-_A1Z_DEFAULT_KD = (3.0, 3.0, 3.0, 0.7, 0.4, 0.4)
-
-
-def _joint_gains(
-    value: tuple[float, ...] | list[float],
-    *,
-    name: str,
-    maximum: float,
-) -> tuple[float, ...]:
-    gains = tuple(float(gain) for gain in value)
-    if len(gains) != _A1Z_DOF:
-        raise ValueError(f"{name} must contain {_A1Z_DOF} values")
-    if any(not math.isfinite(gain) or not 0.0 <= gain <= maximum for gain in gains):
-        raise ValueError(f"{name} gains must be finite and within [0, {maximum}]")
-    return gains
-
-
-def _kp_gains(value: tuple[float, ...] | list[float]) -> tuple[float, ...]:
-    return _joint_gains(value, name="default_kp", maximum=200.0)
-
-
-def _kd_gains(value: tuple[float, ...] | list[float]) -> tuple[float, ...]:
-    return _joint_gains(value, name="default_kd", maximum=5.0)
-
 
 def _validate_optional_path(
     _instance: object,
@@ -98,14 +71,8 @@ class A1ZConfig:
             attrs.validators.le(1.0),
         ),
     )
-    default_kp: tuple[float, ...] = attrs.field(
-        default=_A1Z_DEFAULT_KP,
-        converter=_kp_gains,
-    )
-    default_kd: tuple[float, ...] = attrs.field(
-        default=_A1Z_DEFAULT_KD,
-        converter=_kd_gains,
-    )
+    default_kp: tuple[float, ...] = (80.0, 80.0, 80.0, 50.0, 20.0, 20.0)
+    default_kd: tuple[float, ...] = (3.0, 3.0, 3.0, 0.7, 0.4, 0.4)
     urdf_path: str | Path | None = attrs.field(
         default=None,
         validator=_validate_optional_path,

From 1f511e6d8b5a17a41ab7ad199ca3f5536aaf7a8a Mon Sep 17 00:00:00 2001
From: cc <cc7@duck.com>
Date: Tue, 4 Aug 2026 15:11:04 -0700
Subject: [PATCH 8/8] refactor: clean up Cartesian control tasks

---
 .../cartesian_ik_task/cartesian_ik_task.py    |  71 +++++-----
 .../test_cartesian_ik_task.py                 |  37 ++----
 .../cartesian_ik_task/test_pink_control_ik.py |  60 ++++-----
 .../tasks/eef_twist_task/eef_twist_task.py    |  18 +--
 .../control/tasks/teleop_task/teleop_task.py  |  24 ++--
 .../tasks/teleop_task/test_teleop_task.py     | 122 +-----------------
 .../manipulators/galaxea_a1z/test_adapter.py  |  11 +-
 dimos/robot/manipulators/common/blueprints.py |   6 +-
 8 files changed, 105 insertions(+), 244 deletions(-)

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 8f62ed564a..073e0fb38c 100644
--- a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py
+++ b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py
@@ -58,27 +58,27 @@
 logger = setup_logger()
 
 
-def claim_optional_joint(claim: ResourceClaim, joint_name: str | None) -> ResourceClaim:
-    """Extend a task claim with an optional joint without changing its control policy."""
-    if joint_name is None:
+def claim_with_gripper(claim: ResourceClaim, gripper_joint: str | None) -> ResourceClaim:
+    """Extend an arm task claim with its configured gripper joint."""
+    if gripper_joint is None:
         return claim
     return ResourceClaim(
-        joints=claim.joints | frozenset([joint_name]),
+        joints=claim.joints | frozenset([gripper_joint]),
         priority=claim.priority,
         mode=claim.mode,
     )
 
 
-def append_optional_joint(
+def append_gripper_position(
     output: JointCommandOutput | None,
-    joint_name: str | None,
+    gripper_joint: str | None,
     position: float,
 ) -> JointCommandOutput | None:
-    """Append an optional position-controlled joint to a Cartesian task output."""
-    if output is None or joint_name is None:
+    """Append a configured gripper position to an arm task output."""
+    if output is None or gripper_joint is None:
         return output
     return JointCommandOutput(
-        joint_names=[*output.joint_names, joint_name],
+        joint_names=[*output.joint_names, gripper_joint],
         positions=[*(output.positions or []), position],
         mode=output.mode,
     )
@@ -194,9 +194,10 @@ def __init__(self, name: str, config: CartesianIKTaskConfig) -> None:
         self._last_commanded_joints: NDArray[np.float64] | None = None
 
         logger.info(
-            f"CartesianIKTask {name} initialized with model: "
-            f"{config.control_ik.robot_model.model_path}, "
-            f"joints={config.joint_names}"
+            "Cartesian IK task initialized",
+            task=name,
+            model_path=str(config.control_ik.robot_model.model_path),
+            joints=config.joint_names,
         )
 
     def claim(self) -> ResourceClaim:
@@ -230,8 +231,9 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
                 time_since_update = state.t_now - self._last_update_time
                 if time_since_update > self._config.timeout:
                     logger.warning(
-                        f"CartesianIKTask {self._name} timed out "
-                        f"(no update for {time_since_update:.3f}s)"
+                        "Cartesian IK task timed out",
+                        task=self._name,
+                        seconds_since_update=time_since_update,
                     )
                     self._active = False
                     self._target_pose = None
@@ -241,10 +243,10 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
 
         q_measured = self._get_current_joints(state)
         if q_measured is None:
-            logger.debug(f"CartesianIKTask {self._name}: missing joint state for IK warm-start")
+            logger.debug("Missing joint state for IK warm-start", task=self._name)
             return None
         if not np.all(np.isfinite(q_measured)):
-            logger.error("CartesianIKTask %s: measured joint state is non-finite", self._name)
+            logger.error("Measured joint state is non-finite", task=self._name)
             return None
         q_current = self._solve_seed(q_measured)
         raw_dt = state.dt
@@ -254,7 +256,9 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
         try:
             target_pose = self._prepare_target(state, q_current, dt)
         except (FloatingPointError, RuntimeError, ValueError) as exc:
-            logger.warning("CartesianIKTask %s: target preparation failed: %s", self._name, exc)
+            logger.warning(
+                "Cartesian IK target preparation failed", task=self._name, error=str(exc)
+            )
             return self._hold(q_current)
         if target_pose is None:
             return self._hold(q_current)
@@ -263,20 +267,22 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
         try:
             result = self._ik.solve(target_pose, q_current, dt)
         except (FloatingPointError, RuntimeError, ValueError) as exc:
-            logger.warning("CartesianIKTask %s: IK solve failed: %s", self._name, exc)
+            logger.warning("Cartesian IK solve failed", task=self._name, error=str(exc))
             return self._hold(q_current)
         q_solution = np.asarray(result.positions, dtype=np.float64).reshape(-1)
         if not np.all(np.isfinite(q_solution)) or q_solution.shape != q_current.shape:
-            logger.warning("CartesianIKTask %s: rejecting invalid IK output", self._name)
+            logger.warning("Rejecting invalid Cartesian IK output", task=self._name)
             return self._hold(q_current)
 
         # Safety check: reject if any joint delta exceeds limit
         if not check_joint_delta(q_solution, q_current, self._config.max_joint_delta_deg):
             worst_idx, worst_deg = get_worst_joint_delta(q_solution, q_current)
             logger.warning(
-                f"CartesianIKTask {self._name}: rejecting motion - "
-                f"joint {self._joint_names_list[worst_idx]} delta "
-                f"{worst_deg:.1f}° exceeds limit {self._config.max_joint_delta_deg}°"
+                "Rejecting Cartesian IK motion exceeding joint delta limit",
+                task=self._name,
+                joint=self._joint_names_list[worst_idx],
+                joint_delta_deg=worst_deg,
+                max_joint_delta_deg=self._config.max_joint_delta_deg,
             )
             return self._hold(q_current)
 
@@ -316,19 +322,18 @@ def _solve_seed(self, q_measured: NDArray[np.float64]) -> NDArray[np.float64]:
         if cached is None:
             return q_measured
         if cached.shape != q_measured.shape or not np.all(np.isfinite(cached)):
-            logger.error("CartesianIKTask %s: cached joint command is invalid", self._name)
+            logger.error("Cached Cartesian IK joint command is invalid", task=self._name)
             self._reset_command_state()
             return q_measured
         tracking_error_deg = np.rad2deg(np.abs(cached - q_measured))
         if np.any(tracking_error_deg > self._config.max_tracking_error_deg):
             worst_index = int(np.argmax(tracking_error_deg))
             logger.warning(
-                "CartesianIKTask %s: rebasing solve to measured state; %s tracks %.1f° "
-                "behind command (limit %.1f°)",
-                self._name,
-                self._joint_names_list[worst_index],
-                tracking_error_deg[worst_index],
-                self._config.max_tracking_error_deg,
+                "Rebasing Cartesian IK solve to measured state",
+                task=self._name,
+                joint=self._joint_names_list[worst_index],
+                tracking_error_deg=tracking_error_deg[worst_index],
+                max_tracking_error_deg=self._config.max_tracking_error_deg,
             )
             self._reset_command_state()
             return q_measured
@@ -385,7 +390,10 @@ def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:
         if joints & self._joint_names:
             self._reset_command_state()
             logger.warning(
-                f"CartesianIKTask {self._name} preempted by {by_task} on joints {joints}"
+                "Cartesian IK task preempted",
+                task=self._name,
+                preempting_task=by_task,
+                joints=joints,
             )
 
     def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool:
@@ -412,7 +420,6 @@ def start(self) -> None:
         with self._lock:
             self._last_commanded_joints = None
             self._active = True
-        logger.info(f"CartesianIKTask {self._name} started")
 
     def stop(self) -> None:
         """Deactivate the task (stop outputting commands)."""
@@ -420,7 +427,6 @@ def stop(self) -> None:
             self._active = False
             self._target_pose = None
             self._last_commanded_joints = None
-        logger.info(f"CartesianIKTask {self._name} stopped")
 
     def clear(self) -> None:
         """Clear current target and deactivate."""
@@ -428,7 +434,6 @@ def clear(self) -> None:
             self._target_pose = None
             self._active = False
             self._last_commanded_joints = None
-        logger.info(f"CartesianIKTask {self._name} cleared")
 
     def is_tracking(self) -> bool:
         """Check if actively receiving and outputting commands."""
diff --git a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py
index b610fb66c3..08672e1fb6 100644
--- a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py
+++ b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py
@@ -15,7 +15,7 @@
 from pathlib import Path
 import subprocess
 import sys
-from typing import cast
+from typing import Any, cast
 
 import numpy as np
 import pinocchio
@@ -66,12 +66,12 @@ class _FakeControlIK:
     nq = 1
 
     def __init__(self) -> None:
-        self.target: object | None = None
+        self.target: Any | None = None
         self.dt: float | None = None
         self.increment = 0.0
         self.solve_seeds: list[np.ndarray] = []
 
-    def solve(self, target: object, measured: np.ndarray, dt: float) -> ControlIKResult:
+    def solve(self, target: Any, measured: np.ndarray, dt: float) -> ControlIKResult:
         self.target = target
         self.dt = dt
         self.solve_seeds.append(measured.copy())
@@ -171,7 +171,7 @@ def test_cartesian_runtime_error_is_a_measured_state_hold(
 ) -> None:
     backend = _FakeControlIK()
 
-    def fail(target: object, measured: np.ndarray, dt: float) -> ControlIKResult:
+    def fail(target: Any, measured: np.ndarray, dt: float) -> ControlIKResult:
         raise IKControlRuntimeError("solver failed")
 
     monkeypatch.setattr(backend, "solve", fail)
@@ -222,7 +222,9 @@ def test_cartesian_pipeline_accumulates_from_accepted_commands_while_feedback_tr
     np.testing.assert_allclose(backend.solve_seeds, [[0.0], [0.01]])
 
 
-def test_cartesian_pipeline_rebases_when_command_outpaces_feedback(tmp_path: Path, mocker) -> None:
+def test_cartesian_pipeline_rebases_when_command_outpaces_feedback(
+    tmp_path: Path, monkeypatch: pytest.MonkeyPatch, mocker
+) -> None:
     backend = _FakeControlIK()
     backend.increment = 0.1
     mocker.patch(
@@ -248,32 +250,11 @@ def test_cartesian_pipeline_rebases_when_command_outpaces_feedback(tmp_path: Pat
     assert second.positions == pytest.approx([0.1])
     np.testing.assert_allclose(backend.solve_seeds, [[0.0], [0.0]])
 
-
-def test_cartesian_failure_holds_measured_after_tracking_error_rebase(
-    tmp_path: Path, monkeypatch: pytest.MonkeyPatch, mocker
-) -> None:
-    backend = _FakeControlIK()
-    backend.increment = 0.1
-    mocker.patch(
-        "dimos.control.tasks.cartesian_ik_task.cartesian_ik_task.create_pink_control_ik",
-        return_value=backend,
-    )
-    task = CartesianIKTask(
-        "cartesian",
-        CartesianIKTaskConfig(
-            joint_names=["joint1"],
-            control_ik=PinkControlIKConfig(robot_model=_robot(tmp_path / "unused.urdf")),
-            max_tracking_error_deg=5.0,
-        ),
-    )
-    assert task.on_cartesian_command(PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), 1.0)
-    assert task.compute(_state(1.01)) is not None
-
-    def fail(target: object, measured: np.ndarray, dt: float) -> ControlIKResult:
+    def fail(target: Any, measured: np.ndarray, dt: float) -> ControlIKResult:
         raise IKControlRuntimeError("solver failed")
 
     monkeypatch.setattr(backend, "solve", fail)
-    hold = task.compute(_state(1.02))
+    hold = task.compute(_state(1.03))
 
     assert hold is not None
     assert hold.positions == [0.0]
diff --git a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py
index 14c289d861..32801fb1cb 100644
--- a/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py
+++ b/dimos/control/tasks/cartesian_ik_task/test_pink_control_ik.py
@@ -13,10 +13,11 @@
 # limitations under the License.
 
 from pathlib import Path
+from typing import Any
 
 import numpy as np
 from pink import Configuration
-from pink.tasks import PostureTask
+from pink.tasks import DampingTask, FrameTask, PostureTask
 import pytest
 
 from dimos.control.tasks.cartesian_ik_task.cartesian_ik_task import CartesianIKTaskConfig
@@ -150,7 +151,7 @@ def test_pink_prepares_xacro_with_package_paths_and_arguments(
             "xacro_args": {"dof": "2"},
         }
     )
-    prepared: dict[str, object] = {}
+    prepared: dict[str, Any] = {}
 
     def prepare(
         path: Path,
@@ -221,10 +222,10 @@ def test_pink_reanchors_measured_state_and_runs_one_frame_task_step(
     )
     measured = np.array([0.3, 0.1])
     target = backend.forward_kinematics(measured)
-    calls: list[tuple[Configuration, list[object], float, dict[str, object]]] = []
+    calls: list[tuple[Configuration, list[Any], float, dict[str, Any]]] = []
 
     def solve(
-        configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+        configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any
     ) -> np.ndarray:
         calls.append((configuration, tasks, dt, kwargs))
         return np.zeros(configuration.model.nv)
@@ -251,9 +252,7 @@ def test_pink_solver_dependency_failure_is_translated_to_runtime_error(
 ) -> None:
     backend = create_pink_control_ik(PinkControlIKConfig(robot_model=_robot(_write_urdf(tmp_path))))
 
-    def solve(
-        configuration: object, tasks: list[object], dt: float, **kwargs: object
-    ) -> np.ndarray:
+    def solve(configuration: Any, tasks: list[Any], dt: float, **kwargs: Any) -> np.ndarray:
         raise RuntimeError("solver dependency failed")
 
     monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve)
@@ -272,7 +271,7 @@ def test_pink_receives_pre_bounded_dt_unchanged(
     calls: list[float] = []
 
     def solve(
-        configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+        configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any
     ) -> np.ndarray:
         calls.append(dt)
         return np.zeros(configuration.model.nv)
@@ -289,10 +288,10 @@ def test_pink_posture_task_can_be_disabled(tmp_path: Path, monkeypatch: pytest.M
     backend = create_pink_control_ik(
         PinkControlIKConfig(robot_model=_robot(model_path), posture_cost=0.0)
     )
-    calls: list[list[object]] = []
+    calls: list[list[Any]] = []
 
     def solve(
-        configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+        configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any
     ) -> np.ndarray:
         calls.append(tasks)
         return np.zeros(configuration.model.nv)
@@ -321,10 +320,10 @@ def test_pink_joint_centering_task_targets_position_limit_midpoints(
             joint_centering_cost=1e-3,
         )
     )
-    calls: list[list[object]] = []
+    calls: list[list[Any]] = []
 
     def solve(
-        configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+        configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any
     ) -> np.ndarray:
         calls.append(tasks)
         return np.zeros(configuration.model.nv)
@@ -333,9 +332,11 @@ def solve(
     measured = np.array([0.1, 0.2])
     backend.solve(backend.forward_kinematics(measured), measured, 0.01)
 
-    centering_task = backend._runtime.joint_centering_task
-    assert centering_task is not None
-    assert calls == [[backend._runtime.frame_task, centering_task]]
+    assert len(calls) == 1
+    assert len(calls[0]) == 2
+    assert isinstance(calls[0][0], FrameTask)
+    centering_task = calls[0][1]
+    assert isinstance(centering_task, PostureTask)
     np.testing.assert_allclose(centering_task.target_q, [-0.25, 0.25])
 
 
@@ -350,10 +351,10 @@ def test_pink_damping_task_replaces_posture_for_low_motion_policy(
             damping_cost=1e-3,
         )
     )
-    calls: list[list[object]] = []
+    calls: list[list[Any]] = []
 
     def solve(
-        configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+        configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any
     ) -> np.ndarray:
         calls.append(tasks)
         return np.zeros(configuration.model.nv)
@@ -362,9 +363,10 @@ def solve(
     measured = np.array([0.3, 0.1])
     backend.solve(backend.forward_kinematics(measured), measured, 0.01)
 
-    assert backend._runtime.posture_task is None
-    assert backend._runtime.damping_task is not None
-    assert calls == [[backend._runtime.frame_task, backend._runtime.damping_task]]
+    assert len(calls) == 1
+    assert len(calls[0]) == 2
+    assert isinstance(calls[0][0], FrameTask)
+    assert isinstance(calls[0][1], DampingTask)
 
 
 def test_pink_rejects_uncontrolled_end_effector_chain_without_reference(
@@ -397,7 +399,7 @@ def test_continuous_joint_scalar_limits_fail_with_actionable_diagnostic(
     angle = np.array([3.0])
 
     def solve(
-        configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+        configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any
     ) -> np.ndarray:
         return np.zeros(configuration.model.nv)
 
@@ -424,7 +426,7 @@ def test_pink_applies_position_velocity_limits_and_finite_output(
     solver_inputs: dict[str, np.ndarray] = {}
 
     def solve(
-        configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+        configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any
     ) -> np.ndarray:
         solver_inputs["lower_position"] = configuration.model.lowerPositionLimit.copy()
         solver_inputs["velocity"] = configuration.model.velocityLimit.copy()
@@ -448,10 +450,10 @@ def test_pink_uniformly_scales_solver_velocity_before_integration(
     robot = _robot(model_path).model_copy(update={"velocity_limits": [0.1, 1.0]})
     backend = create_pink_control_ik(PinkControlIKConfig(robot_model=robot, max_velocity=0.2))
     measured = np.array([0.3, 0.1])
-    calls: list[dict[str, object]] = []
+    calls: list[dict[str, Any]] = []
 
     def solve(
-        configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+        configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any
     ) -> np.ndarray:
         calls.append(kwargs)
         return np.array([1.0, 0.5])
@@ -462,8 +464,8 @@ def solve(
 
     assert np.allclose(result.velocity, [0.1, 0.05])
     assert np.allclose(result.positions, [0.301, 0.1005])
-    assert calls[0]["limits"] == backend._runtime.limits
-    assert len(backend._runtime.limits) == 1
+    assert isinstance(calls[0]["limits"], list)
+    assert len(calls[0]["limits"]) == 1
 
 
 def test_pink_projects_seed_and_solution_to_inward_position_limit_margin(
@@ -478,7 +480,7 @@ def test_pink_projects_seed_and_solution_to_inward_position_limit_margin(
     solver_seed: list[np.ndarray] = []
 
     def solve(
-        configuration: Configuration, tasks: list[object], dt: float, **kwargs: object
+        configuration: Configuration, tasks: list[Any], dt: float, **kwargs: Any
     ) -> np.ndarray:
         solver_seed.append(configuration.q.copy())
         return np.array([0.5, -0.2])
@@ -513,9 +515,7 @@ def test_pink_rejects_candidate_beyond_position_limit_tolerance(
     backend = create_pink_control_ik(PinkControlIKConfig(robot_model=robot))
     measured = np.array([1.2, 0.1])
 
-    def solve(
-        configuration: object, tasks: list[object], dt: float, **kwargs: object
-    ) -> np.ndarray:
+    def solve(configuration: Any, tasks: list[Any], dt: float, **kwargs: Any) -> np.ndarray:
         return np.array([10.0, 0.0])
 
     monkeypatch.setattr("dimos.control.tasks.cartesian_ik_task.pink_control_ik.solve_ik", solve)
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 9843574298..104ab1aed3 100644
--- a/dimos/control/tasks/eef_twist_task/eef_twist_task.py
+++ b/dimos/control/tasks/eef_twist_task/eef_twist_task.py
@@ -29,8 +29,8 @@
     CartesianIKTask,
     CartesianIKTaskConfig,
     CartesianIKTaskParams,
-    append_optional_joint,
-    claim_optional_joint,
+    append_gripper_position,
+    claim_with_gripper,
 )
 from dimos.utils.logging_config import setup_logger
 from dimos.utils.transform_utils import twist_to_numpy
@@ -65,7 +65,7 @@ def __init__(self, name: str, config: EEFTwistTaskConfig) -> None:
         self._gripper_active = config.gripper_joint is not None
 
     def claim(self) -> ResourceClaim:
-        return claim_optional_joint(super().claim(), self._config.gripper_joint)
+        return claim_with_gripper(super().claim(), self._config.gripper_joint)
 
     def is_active(self) -> bool:
         with self._twist_lock:
@@ -143,7 +143,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
         with self._twist_lock:
             gripper_target = self._gripper_target
             gripper_joint = self._config.gripper_joint if self._gripper_active else None
-        return append_optional_joint(
+        return append_gripper_position(
             output,
             gripper_joint,
             gripper_target,
@@ -170,19 +170,21 @@ def _prepare_target(
         return pose
 
     def stop(self) -> None:
+        self._clear_inputs()
+        super().stop()
+
+    def _clear_inputs(self) -> None:
+        """Discard twist and gripper commands owned by this specialization."""
         with self._twist_lock:
             self._latest_twist = None
             self._gripper_active = False
-        super().stop()
 
     def _on_timeout(self) -> None:
         with self._twist_lock:
             self._latest_twist = None
 
     def clear(self) -> None:
-        with self._twist_lock:
-            self._latest_twist = None
-            self._gripper_active = False
+        self._clear_inputs()
         super().clear()
 
 
diff --git a/dimos/control/tasks/teleop_task/teleop_task.py b/dimos/control/tasks/teleop_task/teleop_task.py
index ba20c62749..e6bb46c4fb 100644
--- a/dimos/control/tasks/teleop_task/teleop_task.py
+++ b/dimos/control/tasks/teleop_task/teleop_task.py
@@ -30,8 +30,8 @@
     CartesianIKTask,
     CartesianIKTaskConfig,
     CartesianIKTaskParams,
-    append_optional_joint,
-    claim_optional_joint,
+    append_gripper_position,
+    claim_with_gripper,
 )
 from dimos.control.tasks.cartesian_ik_task.pink_control_ik import PinkControlIKConfig
 from dimos.utils.logging_config import setup_logger
@@ -92,7 +92,7 @@ def __init__(self, name: str, config: TeleopIKTaskConfig) -> None:
 
     def claim(self) -> ResourceClaim:
         """Claim arm joints and the optional gripper joint."""
-        return claim_optional_joint(super().claim(), self._config.gripper_joint)
+        return claim_with_gripper(super().claim(), self._config.gripper_joint)
 
     def is_active(self) -> bool:
         """Run only when a non-E-STOPped pose target is active."""
@@ -166,7 +166,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
                 return None
             gripper_target = self._gripper_target
             gripper_joint = self._config.gripper_joint if self._gripper_active else None
-        return append_optional_joint(output, gripper_joint, gripper_target)
+        return append_gripper_position(output, gripper_joint, gripper_target)
 
     def on_buttons(self, msg: Buttons) -> bool:
         """Use the configured primary button as press-and-hold engagement."""
@@ -245,11 +245,12 @@ def _on_timeout(self) -> None:
     def stop(self) -> None:
         """Stop output and discard engagement-relative state."""
         super().stop()
+        self._reset_engagement_state()
+
+    def _reset_engagement_state(self) -> None:
+        """Discard state owned specifically by engagement-relative teleop."""
         with self._lock:
-            self._active = False
-            self._target_pose = None
             self._initial_ee_pose = None
-            self._last_commanded_joints = None
             self._engagement = _EngagementState.DISENGAGED
             self._primary_down = False
             self._gripper_active = False
@@ -257,14 +258,7 @@ def stop(self) -> None:
     def clear(self) -> None:
         """Clear output and discard engagement-relative state."""
         super().clear()
-        with self._lock:
-            self._active = False
-            self._target_pose = None
-            self._initial_ee_pose = None
-            self._last_commanded_joints = None
-            self._engagement = _EngagementState.DISENGAGED
-            self._primary_down = False
-            self._gripper_active = False
+        self._reset_engagement_state()
 
 
 class TeleopIKTaskParams(CartesianIKTaskParams):
diff --git a/dimos/control/tasks/teleop_task/test_teleop_task.py b/dimos/control/tasks/teleop_task/test_teleop_task.py
index afd20b54fa..3d39730c74 100644
--- a/dimos/control/tasks/teleop_task/test_teleop_task.py
+++ b/dimos/control/tasks/teleop_task/test_teleop_task.py
@@ -24,7 +24,7 @@
 from pytest_mock import MockerFixture
 
 from dimos.control.coordinator import TaskConfig
-from dimos.control.task import ControlMode, CoordinatorState, JointStateSnapshot
+from dimos.control.task import CoordinatorState, JointStateSnapshot
 from dimos.control.tasks.cartesian_ik_task.pink_control_ik import (
     ControlIKResult,
     IKControlRuntimeError,
@@ -49,7 +49,6 @@ def __post_init__(self) -> None:
         self.fk_calls: list[NDArray[np.float64]] = []
         self.solve_calls: list[tuple[pinocchio.SE3, NDArray[np.float64], float]] = []
         self.solution = np.array([0.01, 0.02], dtype=np.float64)
-        self.increment: NDArray[np.float64] | None = None
         self.raise_runtime = False
 
     def forward_kinematics(self, q: NDArray[np.float64]) -> pinocchio.SE3:
@@ -68,8 +67,7 @@ def solve(
         if self.raise_runtime:
             raise IKControlRuntimeError("synthetic Pink failure")
         self.solve_calls.append((target.copy(), measured.copy(), dt))
-        solution = self.solution if self.increment is None else measured + self.increment
-        return ControlIKResult(solution.copy(), solution - measured)
+        return ControlIKResult(self.solution.copy(), self.solution - measured)
 
 
 def _robot(path: Path) -> RobotModelConfig:
@@ -197,58 +195,6 @@ def test_delta_is_composed_with_one_measured_engagement_baseline(
     assert (first_dt, second_dt) == (0.03, 0.02)
 
 
-def test_teleop_ik_iterates_from_last_command_when_feedback_lags(
-    task: TeleopIKTask, fake_ik: _FakePinkIK
-) -> None:
-    fake_ik.increment = np.array([0.01, 0.0], dtype=np.float64)
-    _engage(task)
-    assert task.on_cartesian_command(_delta(), t_now=1.0)
-
-    first = task.compute(_state(1.01))
-    second = task.compute(_state(1.02))
-
-    assert first is not None
-    assert second is not None
-    assert first.positions == pytest.approx([0.01, 0.0])
-    assert second.positions == pytest.approx([0.02, 0.0])
-    np.testing.assert_allclose(
-        [solve[1] for solve in fake_ik.solve_calls],
-        np.array([[0.0, 0.0], [0.01, 0.0]]),
-    )
-
-
-def test_missing_joint_state_defers_baseline_and_output(
-    task: TeleopIKTask, fake_ik: _FakePinkIK
-) -> None:
-    _engage(task)
-    assert task.on_cartesian_command(_delta(), t_now=1.0)
-
-    output = task.compute(_state(1.01, (0.0,)))
-
-    assert output is None
-    assert fake_ik.fk_calls == []
-    assert fake_ik.solve_calls == []
-
-
-def test_solver_failure_and_excessive_delta_return_measured_hold(
-    task: TeleopIKTask, fake_ik: _FakePinkIK
-) -> None:
-    _engage(task)
-    assert task.on_cartesian_command(_delta(), t_now=1.0)
-    fake_ik.raise_runtime = True
-    failed = task.compute(_state(1.01, (0.4, 0.5)))
-
-    fake_ik.raise_runtime = False
-    fake_ik.solution = np.array([2.0, 0.5], dtype=np.float64)
-    rejected = task.compute(_state(1.02, (0.4, 0.5)))
-
-    assert failed is not None
-    assert failed.positions == [0.4, 0.5]
-    assert rejected is not None
-    assert rejected.positions == [0.4, 0.5]
-    assert rejected.mode == ControlMode.SERVO_POSITION
-
-
 def test_release_timeout_stop_and_clear_force_fresh_baselines(
     task: TeleopIKTask, fake_ik: _FakePinkIK
 ) -> None:
@@ -281,23 +227,8 @@ def test_release_timeout_stop_and_clear_force_fresh_baselines(
     assert task.compute(_state(5.01, (0.3, 0.4))) is not None
     np.testing.assert_allclose(fake_ik.solve_calls[-1][1], [0.3, 0.4])
     task.clear()
-    assert len(fake_ik.fk_calls) == 4
-
-
-def test_preemption_discards_teleop_commanded_solve_seed(
-    task: TeleopIKTask, fake_ik: _FakePinkIK
-) -> None:
-    fake_ik.increment = np.array([0.01, 0.0], dtype=np.float64)
-    _engage(task)
-    assert task.on_cartesian_command(_delta(), t_now=1.0)
-    assert task.compute(_state(1.01)) is not None
-
-    task.on_preempted("higher_priority", frozenset(["arm/joint1"]))
-    output = task.compute(_state(1.02, (0.5, 0.5)))
-
-    assert output is not None
-    assert output.positions == pytest.approx([0.51, 0.5])
-    np.testing.assert_allclose(fake_ik.solve_calls[-1][1], [0.5, 0.5])
+    assert not task.is_active()
+    assert not task.on_cartesian_command(_delta(), 6.0)
 
 
 def test_estop_rejects_commands_and_never_replays_them(
@@ -369,48 +300,3 @@ def test_factory_requires_pink_configuration_and_matching_model(tmp_path: Path)
     )
     with pytest.raises(ValueError, match="task joints must match"):
         create_task(mismatched, {})
-
-
-def test_factory_applies_balanced_teleop_control_policy(
-    tmp_path: Path, fake_ik: _FakePinkIK
-) -> None:
-    configured = TaskConfig(
-        name="teleop",
-        type="teleop_ik",
-        joint_names=["arm/joint1", "arm/joint2"],
-        params={
-            "control_ik": {"robot_model": _robot(tmp_path / "unused.urdf")},
-            "hand": "right",
-        },
-    )
-
-    task = create_task(configured, {})
-
-    assert task._config.control_ik.max_velocity == 1.0
-    assert task._config.control_ik.position_cost == 1.0
-    assert task._config.control_ik.orientation_cost == 1.0
-    assert task._config.control_ik.posture_cost == 0.0
-    assert task._config.control_ik.joint_centering_cost == 1e-3
-    assert task._config.control_ik.damping_cost == 1e-3
-    assert task._config.max_joint_delta_deg == 5.0
-
-
-def test_factory_preserves_explicit_teleop_orientation_cost_override(
-    tmp_path: Path, fake_ik: _FakePinkIK
-) -> None:
-    configured = TaskConfig(
-        name="teleop",
-        type="teleop_ik",
-        joint_names=["arm/joint1", "arm/joint2"],
-        params={
-            "control_ik": {
-                "robot_model": _robot(tmp_path / "unused.urdf"),
-                "orientation_cost": 0.2,
-            },
-            "hand": "right",
-        },
-    )
-
-    task = create_task(configured, {})
-
-    assert task._config.control_ik.orientation_cost == 0.2
diff --git a/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py b/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py
index afc958d153..560ac1118e 100644
--- a/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py
+++ b/dimos/hardware/manipulators/galaxea_a1z/test_adapter.py
@@ -271,18 +271,11 @@ def test_connect_opens_bus_without_powering_motors(
     assert not adapter.read_enabled()
 
 
-def test_default_control_gains_use_stiffer_wrist_profile() -> None:
-    config = A1ZConfig()
-
-    assert config.default_kp == pytest.approx((80.0, 80.0, 80.0, 50.0, 20.0, 20.0))
-    assert config.default_kd == pytest.approx((3.0, 3.0, 3.0, 0.7, 0.4, 0.4))
-
-
 def test_connect_forwards_configured_arm_gains_to_sdk(
     a1z_adapter_module: ModuleType,
 ) -> None:
-    kp = (80.0, 80.0, 80.0, 50.0, 20.0, 20.0)
-    kd = (3.0, 3.0, 3.0, 0.7, 0.4, 0.4)
+    kp = (70.0, 70.0, 70.0, 40.0, 15.0, 15.0)
+    kd = (2.5, 2.5, 2.5, 0.6, 0.3, 0.3)
 
     _, robot = _connected_adapter(a1z_adapter_module, default_kp=kp, default_kd=kd)
 
diff --git a/dimos/robot/manipulators/common/blueprints.py b/dimos/robot/manipulators/common/blueprints.py
index d62c01c264..725b1255ff 100644
--- a/dimos/robot/manipulators/common/blueprints.py
+++ b/dimos/robot/manipulators/common/blueprints.py
@@ -87,7 +87,7 @@ def _resolve_control_ik(
     hardware: HardwareComponent,
     robot_model: RobotModelConfig,
     control_ik: PinkControlIKOverrides | None,
-) -> dict[str, object]:
+) -> dict[str, Any]:
     coordinator_joints = robot_model.get_coordinator_joint_names()
     if hardware.joints != coordinator_joints:
         raise ValueError("hardware joints must match RobotModelConfig coordinator joints")
@@ -141,7 +141,7 @@ def eef_twist_task(
     params: GripperTaskOverrides | None = None,
 ) -> TaskConfig:
     resolved_control_ik = _resolve_control_ik(hardware, robot_model, control_ik)
-    task_params: dict[str, object] = {
+    task_params: dict[str, Any] = {
         "control_ik": resolved_control_ik,
         "timeout": timeout,
         "max_joint_delta_deg": max_joint_delta_deg,
@@ -176,7 +176,7 @@ def teleop_ik_task(
     params: GripperTaskOverrides | None = None,
 ) -> TaskConfig:
     resolved_control_ik = _resolve_control_ik(hardware, robot_model, control_ik)
-    task_params: dict[str, object] = {
+    task_params: dict[str, Any] = {
         "control_ik": resolved_control_ik,
         "hand": hand,
         "timeout": timeout,