feat: use Pink IK for teleop tasks

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,14 +250,15 @@ 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(),
             mode=ControlMode.SERVO_POSITION,
         )
 
     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:

dimos/control/tasks/cartesian_ik_task/pink_control_ik.py

@@ -28,8 +28,8 @@
 
 try:
     from pink import Configuration, solve_ik
-    from pink.limits import ConfigurationLimit, VelocityLimit
-    from pink.tasks import FrameTask, PostureTask
+    from pink.limits import ConfigurationLimit
+    from pink.tasks import DampingTask, FrameTask, PostureTask
 except ModuleNotFoundError as exc:
     raise ModuleNotFoundError(
         f"{_PINK_INSTALL_ERROR} Missing module: {exc.name}",
@@ -40,9 +40,6 @@
 from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake
 from dimos.protocol.service.spec import BaseConfig
 
-# Pink's integration/QP boundary tolerance is small but larger than machine epsilon.
-_POSITION_LIMIT_EPSILON_RAD = 1e-5
-
 
 class PinkControlIKConfig(BaseConfig):
     """Typed configuration for the control IK backend."""
@@ -55,6 +52,10 @@ class PinkControlIKConfig(BaseConfig):
     position_cost: FiniteFloat = Field(1.0, ge=0.0)
     orientation_cost: FiniteFloat = Field(1.0, ge=0.0)
     posture_cost: FiniteFloat = Field(1e-3, ge=0.0)
+    joint_centering_cost: FiniteFloat = Field(0.0, ge=0.0)
+    damping_cost: FiniteFloat = Field(0.0, ge=0.0)
+    position_limit_margin: FiniteFloat = Field(1e-3, ge=0.0)
+    seed_limit_tolerance: FiniteFloat = Field(1e-2, ge=0.0)
     reference_q: list[float] | None = None
     qpsolver_options: dict[str, FiniteFloat] = Field(default_factory=dict)
 
@@ -89,8 +90,11 @@ class _PinkRuntime:
     configuration: Configuration
     frame_task: FrameTask
     posture_task: PostureTask | None
+    joint_centering_task: PostureTask | None
+    damping_task: DampingTask | None
     tasks: list[object]
     limits: list[object]
+    velocity_limits: NDArray[np.float64]
 
 
 class _PinkControlIKBuilder:
@@ -150,9 +154,29 @@ def build(self) -> _PinkRuntime:
             gain=config.task_gain,
         )
         posture_task = PostureTask(cost=config.posture_cost) if config.posture_cost > 0.0 else None
+        joint_centering_task = (
+            PostureTask(cost=config.joint_centering_cost)
+            if config.joint_centering_cost > 0.0
+            else None
+        )
+        if joint_centering_task is not None:
+            joint_centering_task.set_target(self._build_joint_center_q(model, mapping, reference_q))
+        damping_task = DampingTask(cost=config.damping_cost) if config.damping_cost > 0.0 else None
         tasks: list[object] = [frame_task]
         if posture_task is not None:
             tasks.append(posture_task)
+        if joint_centering_task is not None:
+            tasks.append(joint_centering_task)
+        if damping_task is not None:
+            tasks.append(damping_task)
+
+        velocity_limits = np.asarray(model.velocityLimit, dtype=np.float64).copy()
+        if (
+            velocity_limits.size != model.nv
+            or not np.all(np.isfinite(velocity_limits))
+            or np.any(velocity_limits <= 0.0)
+        ):
+            raise ValueError("effective Pink velocity limits are invalid")
 
         return _PinkRuntime(
             config=config,
@@ -164,8 +188,11 @@ def build(self) -> _PinkRuntime:
             configuration=configuration,
             frame_task=frame_task,
             posture_task=posture_task,
+            joint_centering_task=joint_centering_task,
+            damping_task=damping_task,
             tasks=tasks,
             limits=limits,
+            velocity_limits=velocity_limits,
         )
 
     @staticmethod
@@ -255,6 +282,22 @@ def _uncontrolled_ee_chain(
             joint_id = int(model.parents[joint_id])
         return False
 
+    @staticmethod
+    def _build_joint_center_q(
+        model: pinocchio.Model,
+        mapping: _CoordinateMapping,
+        reference_q: NDArray[np.float64],
+    ) -> NDArray[np.float64]:
+        center_q = reference_q.copy()
+        for q_index, width in zip(mapping.q_indices, mapping.q_widths, strict=True):
+            if width != 1:
+                continue
+            lower = model.lowerPositionLimit[q_index]
+            upper = model.upperPositionLimit[q_index]
+            if np.isfinite(lower) and np.isfinite(upper):
+                center_q[q_index] = (lower + upper) / 2.0
+        return center_q
+
     @staticmethod
     def _validate_frame(model: pinocchio.Model, frame_name: str) -> int:
         if not model.existFrame(frame_name):
@@ -302,7 +345,18 @@ def _apply_limits(
                 model.velocityLimit[index] = limit
         for index in mapping.v_indices:
             model.velocityLimit[index] = min(model.velocityLimit[index], self._config.max_velocity)
-        return [ConfigurationLimit(model), VelocityLimit(model)]
+        margin = self._config.position_limit_margin
+        for q_index, width in zip(mapping.q_indices, mapping.q_widths, strict=True):
+            if width != 1:
+                continue
+            lower = model.lowerPositionLimit[q_index]
+            upper = model.upperPositionLimit[q_index]
+            if np.isfinite(lower) and np.isfinite(upper) and upper - lower <= 2.0 * margin:
+                raise ValueError("position limit margin leaves no valid joint range")
+        # Keep position bounds in the QP, but apply velocity limits by uniformly
+        # scaling the solution. Tiny per-tick velocity boxes can make ProxQP
+        # misclassify feasible differential IK problems as primal-infeasible.
+        return [ConfigurationLimit(model)]
 
 
 class PinkControlIK:
@@ -339,7 +393,8 @@ def solve(
         configuration = runtime.configuration
         frame_task = runtime.frame_task
         try:
-            configuration.update(self._full_q(measured))
+            solve_seed = self._project_position_limits(measured, "solve seed")
+            configuration.update(self._full_q(solve_seed))
             frame_task.set_target(target)
             if runtime.posture_task is not None:
                 runtime.posture_task.set_target(configuration.q.copy())
@@ -355,11 +410,12 @@ def solve(
             velocity = np.asarray(velocity, dtype=np.float64).reshape(-1)
             if velocity.size != runtime.model.nv or not np.all(np.isfinite(velocity)):
                 raise IKControlRuntimeError("Pink produced an invalid velocity")
+            velocity = self._scale_velocity(velocity)
             configuration.integrate_inplace(velocity, dt)
-            candidate = self._project_controlled_positions(configuration.q, measured)
-            if candidate.size != measured.size or not np.all(np.isfinite(candidate)):
+            candidate = self._project_controlled_positions(configuration.q, solve_seed)
+            if candidate.size != solve_seed.size or not np.all(np.isfinite(candidate)):
                 raise IKControlRuntimeError("Pink produced an invalid joint candidate")
-            candidate = self._clamp_position_limits(candidate)
+            candidate = self._project_position_limits(candidate, "candidate")
             return ControlIKResult(candidate, self._controlled_velocity(velocity))
         except IKControlRuntimeError:
             raise
@@ -405,27 +461,44 @@ def _controlled_velocity(self, velocity: NDArray[np.float64]) -> NDArray[np.floa
             [velocity[index] for index in self._runtime.mapping.v_indices], dtype=np.float64
         )
 
-    def _clamp_position_limits(self, candidate: NDArray[np.float64]) -> NDArray[np.float64]:
+    def _scale_velocity(self, velocity: NDArray[np.float64]) -> NDArray[np.float64]:
+        """Uniformly scale a Pink solution to preserve its joint-space direction."""
+        max_ratio = float(np.max(np.abs(velocity) / self._runtime.velocity_limits))
+        if max_ratio <= 1.0:
+            return velocity
+        return velocity / max_ratio
+
+    def _project_position_limits(
+        self,
+        positions: NDArray[np.float64],
+        source: str,
+    ) -> NDArray[np.float64]:
         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
             q_index = mapping.q_indices[index]
             lower = runtime.model.lowerPositionLimit[q_index]
             upper = runtime.model.upperPositionLimit[q_index]
             value = bounded[index]
-            if value < lower:
-                if lower - value <= _POSITION_LIMIT_EPSILON_RAD:
-                    bounded[index] = lower
-                else:
-                    raise IKControlRuntimeError("Pink produced an out-of-bounds joint candidate")
-            elif value > upper:
-                if value - upper <= _POSITION_LIMIT_EPSILON_RAD:
-                    bounded[index] = upper
-                else:
-                    raise IKControlRuntimeError("Pink produced an out-of-bounds joint candidate")
+            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