feat(go2): RPP Trajectory controller + decoupled pub/sub benchmark & scoring

dimos/control/coordinator.py

@@ -56,7 +56,9 @@
 from dimos.hardware.whole_body.spec import WholeBodyAdapter
 from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
 from dimos.msgs.geometry_msgs.Twist import Twist
+from dimos.msgs.nav_msgs.Path import Path
 from dimos.msgs.sensor_msgs.JointState import JointState
+from dimos.msgs.std_msgs.Float32 import Float32
 from dimos.teleop.quest.quest_types import (
     Buttons,
 )
@@ -157,6 +159,18 @@ class ControlCoordinator(Module):
     # Input: Teleop buttons for engage/disengage signaling
     teleop_buttons: In[Buttons]
 
+    # Input: Planned path for tasks exposing ``set_path()``. Typical source is a
+    # nav-stack planner or a benchmark publishing a reference path. The coord
+    # forwards the path (with a fresh odom snapshot) to every such task; the
+    # tasks read their own latest odom from internal state populated each tick.
+    path: In[Path]
+
+    # Input: Target/cruise speed (m/s) for tasks exposing ``set_speed()``. An
+    # optional runtime override — by default the follower's configured speed
+    # governs; a source (e.g. a benchmark sweeping speeds) wires this to retune
+    # the follower over the transport without RPC.
+    speed: In[Float32]
+
     # Arming and dry-run are one-shot RPCs, not streams.
 
     def __init__(self, *args: Any, **kwargs: Any) -> None:
@@ -181,6 +195,8 @@ def __init__(self, *args: Any, **kwargs: Any) -> None:
         self._cartesian_command_unsub: Callable[[], None] | None = None
         self._twist_command_unsub: Callable[[], None] | None = None
         self._buttons_unsub: Callable[[], None] | None = None
+        self._path_unsub: Callable[[], None] | None = None
+        self._speed_unsub: Callable[[], None] | None = None
 
         logger.info(f"ControlCoordinator initialized at {self.config.tick_rate}Hz")
 
@@ -526,6 +542,58 @@ def _on_buttons(self, msg: Buttons) -> None:
             for task in self._tasks.values():
                 task.on_buttons(msg)
 
+    def _read_base_odom(self) -> PoseStamped | None:
+        """Snapshot the BASE adapter's current odom as a PoseStamped, or None."""
+        with self._hardware_lock:
+            for hw in self._hardware.values():
+                if hw.component.hardware_type != HardwareType.BASE:
+                    continue
+                read_odometry = getattr(hw.adapter, "read_odometry", None)
+                if not callable(read_odometry):
+                    continue
+                try:
+                    xyt = read_odometry()
+                except Exception:
+                    continue
+                if xyt is None or len(xyt) < 3:
+                    continue
+                from dimos.msgs.geometry_msgs.Quaternion import Quaternion
+                from dimos.msgs.geometry_msgs.Vector3 import Vector3
+
+                return PoseStamped(
+                    ts=time.perf_counter(),
+                    position=Vector3(float(xyt[0]), float(xyt[1]), 0.0),
+                    orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(xyt[2]))),
+                )
+        return None
+
+    def _on_path(self, msg: Path) -> None:
+        """Forward a planned path + a fresh odom snapshot to any task exposing
+        ``set_path(path, odom)``. Single-arg ``set_path(path)`` callers still
+        work (TypeError fallback)."""
+        odom = self._read_base_odom()
+        with self._task_lock:
+            for task in self._tasks.values():
+                set_path = getattr(task, "set_path", None)
+                if set_path is None:
+                    continue
+                try:
+                    set_path(msg, odom)
+                except TypeError:
+                    set_path(msg)  # backwards compat with single-arg set_path
+
+    def _on_speed(self, msg: Float32) -> None:
+        """Forward a target/cruise speed to any task exposing ``set_speed()``."""
+        value = float(msg.data)
+        with self._task_lock:
+            for task in self._tasks.values():
+                set_speed = getattr(task, "set_speed", None)
+                if callable(set_speed):
+                    try:
+                        set_speed(value)
+                    except Exception:
+                        logger.exception(f"set_speed() raised on task {task.name!r}")
+
     @rpc
     def set_activated(self, engaged: bool) -> None:
         """Arm/disarm every task exposing ``arm()`` / ``disarm()``."""
@@ -687,6 +755,36 @@ def start(self) -> None:
             self._buttons_unsub = self.teleop_buttons.subscribe(self._on_buttons)
             logger.info("Subscribed to buttons for engage/disengage")
 
+        # Subscribe to path if any task implements set_path (nav/benchmark-driven).
+        with self._task_lock:
+            has_path_task = any(
+                callable(getattr(task, "set_path", None)) for task in self._tasks.values()
+            )
+        if has_path_task:
+            try:
+                self._path_unsub = self.path.subscribe(self._on_path)
+                logger.info("Subscribed to path for path-stream-capable tasks")
+            except Exception:
+                logger.warning(
+                    "Path-stream-capable task configured but could not subscribe to path. "
+                    "Set transport via blueprint."
+                )
+
+        # Subscribe to speed if any task implements set_speed (runtime override).
+        with self._task_lock:
+            has_speed_task = any(
+                callable(getattr(task, "set_speed", None)) for task in self._tasks.values()
+            )
+        if has_speed_task:
+            try:
+                self._speed_unsub = self.speed.subscribe(self._on_speed)
+                logger.info("Subscribed to speed for speed-capable tasks")
+            except Exception:
+                logger.warning(
+                    "Speed-capable task configured but could not subscribe to speed. "
+                    "Set transport via blueprint."
+                )
+
         # Arming + dry-run are RPC-only; no stream subscription here.
 
         logger.info(f"ControlCoordinator started at {self.config.tick_rate}Hz")
@@ -709,6 +807,12 @@ def stop(self) -> None:
         if self._buttons_unsub:
             self._buttons_unsub()
             self._buttons_unsub = None
+        if self._path_unsub:
+            self._path_unsub()
+            self._path_unsub = None
+        if self._speed_unsub:
+            self._speed_unsub()
+            self._speed_unsub = None
 
         if self._tick_loop:
             self._tick_loop.stop()

dimos/control/tasks/feedforward_gain_compensator.py

@@ -0,0 +1,93 @@
+# Copyright 2025-2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Static feedforward gain compensator (Strategy B).
+
+Sits between any path-following controller and the platform. Rather than
+closing a velocity loop with a PID (which requires actual_velocity feedback
+and is fragile when cascaded over a firmware that already tracks velocity),
+this compensator just **inverts the steady-state plant gain** so the
+controller's "I want vx=X" command actually produces vx=X at the wheels:
+
+    cmd_to_robot = controller_cmd / K_plant
+
+Stateless, no actual feedback needed, no phase-margin issues. Works as
+long as K is reasonably accurate. Trade: doesn't compensate for plant
+dynamics (tau, L) - controller's own outer loop handles those via pose
+feedback.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+
+def _clamp(v: float, lo: float, hi: float) -> float:
+    return max(lo, min(hi, v))
+
+
+@dataclass
+class FeedforwardGainConfig:
+    """Steady-state plant gains. Default = unity (passthrough).
+
+    For Go2, do not hardcode: read the vendored fit
+    ``dimos.utils.benchmarking.plant_models.GO2_PLANT_FITTED`` (currently
+    ``K_vx≈0.92``, ``K_wz≈2.45``). A stale hardcoded ``K_wz=2.175`` copy
+    silently mis-calibrated every FF controller; the single source of
+    truth is plant_models.
+    """
+
+    K_vx: float = 1.0
+    K_vy: float = 1.0
+    K_wz: float = 1.0
+    output_min_vx: float = -1.0
+    output_max_vx: float = 1.0
+    output_min_vy: float = -1.0
+    output_max_vy: float = 1.0
+    output_min_wz: float = -1.5
+    output_max_wz: float = 1.5
+
+
+class FeedforwardGainCompensator:
+    """Divide controller-output velocities by plant gains; clamp to limits.
+
+    API mirrors :class:`VelocityTrackingPID.compute` so it slots into the
+    same place in the path-follower task pipeline. ``actual_*`` arguments
+    are accepted but ignored - this is pure feedforward.
+    """
+
+    def __init__(self, config: FeedforwardGainConfig | None = None) -> None:
+        self.cfg = config or FeedforwardGainConfig()
+
+    def compute(
+        self,
+        desired_vx: float,
+        desired_vy: float,
+        desired_wz: float,
+        actual_vx: float = 0.0,
+        actual_vy: float = 0.0,
+        actual_wz: float = 0.0,
+    ) -> tuple[float, float, float]:
+        return (
+            _clamp(desired_vx / self.cfg.K_vx, self.cfg.output_min_vx, self.cfg.output_max_vx),
+            _clamp(desired_vy / self.cfg.K_vy, self.cfg.output_min_vy, self.cfg.output_max_vy),
+            _clamp(desired_wz / self.cfg.K_wz, self.cfg.output_min_wz, self.cfg.output_max_wz),
+        )
+
+    def reset(self) -> None:
+        # Stateless. Method exists so it's drop-in for VelocityTrackingPID.
+        pass
+
+
+__all__ = ["FeedforwardGainCompensator", "FeedforwardGainConfig"]

dimos/control/tasks/path_follower_task/__registry__.py

@@ -0,0 +1,17 @@
+# Copyright 2025-2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+TASK_FACTORIES = {
+    "path_follower": "dimos.control.tasks.path_follower_task.path_follower_task:create_task",
+}