Go2 Precision based controller

dimos/control/blueprints/mobile.py

@@ -20,6 +20,7 @@
     dimos run coordinator-flowbase                       # FlowBase holonomic base (Portal RPC)
     dimos run coordinator-flowbase-keyboard-teleop       # FlowBase + WASD pygame teleop
     dimos run coordinator-flowbase-nav                   # FlowBase + FastLio2 + nav stack (click-to-drive)
+    dimos run coordinator-sim-fopdt                      # FOPDT sim plant on /go2/cmd_vel|odom (Go2-shaped)
 """
 
 from __future__ import annotations
@@ -36,13 +37,15 @@
 from dimos.core.transport import LCMTransport
 from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2
 from dimos.msgs.geometry_msgs.Pose import Pose
+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
 from dimos.msgs.geometry_msgs.Quaternion import Quaternion
 from dimos.msgs.geometry_msgs.Twist import Twist
 from dimos.msgs.geometry_msgs.Vector3 import Vector3
 from dimos.msgs.sensor_msgs.JointState import JointState
 from dimos.navigation.movement_manager.movement_manager import MovementManager
 from dimos.navigation.nav_stack.main import create_nav_stack, nav_stack_rerun_config
 from dimos.robot.catalog.ufactory import xarm7 as _catalog_xarm7
+from dimos.robot.sim.fopdt_plant_connection import FopdtPlantConnection
 from dimos.robot.unitree.g1.config import G1_LOCAL_PLANNER_PRECOMPUTED_PATHS
 from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop
 from dimos.visualization.rerun.bridge import RerunBridgeModule
@@ -87,6 +90,15 @@ def _flowbase_twist_base(
             type="velocity",
             joint_names=_base_joints,
             priority=10,
+            params={"zero_on_timeout": False},
+        ),
+        # Closed-loop path follower used by the benchmark tool.
+        # Inactive until the tool RPCs configure(...) + start_path(...).
+        TaskConfig(
+            name="path_follower",
+            type="path_follower",
+            joint_names=_base_joints,
+            priority=20,
         ),
     ],
 ).transports(
@@ -105,6 +117,15 @@ def _flowbase_twist_base(
             type="velocity",
             joint_names=_base_joints,
             priority=10,
+            params={"zero_on_timeout": False},
+        ),
+        # Closed-loop path follower used by the benchmark tool.
+        # Inactive until the tool RPCs configure(...) + start_path(...).
+        TaskConfig(
+            name="path_follower",
+            type="path_follower",
+            joint_names=_base_joints,
+            priority=20,
         ),
     ],
 ).transports(
@@ -119,15 +140,25 @@ def _flowbase_twist_base(
     ControlCoordinator.blueprint(
         hardware=[_flowbase_twist_base()],
         tasks=[
+            # task go inactive so control hands back to the path follower.
             TaskConfig(
                 name="vel_base",
                 type="velocity",
                 joint_names=_base_joints,
+                priority=20,
+                params={"zero_on_timeout": False},
+            ),
+            # Closed-loop path follower used by the benchmark tool. Inactive
+            # until the tool RPCs configure(...) + start_path(...).
+            TaskConfig(
+                name="path_follower",
+                type="path_follower",
+                joint_names=_base_joints,
                 priority=10,
             ),
         ],
     ),
-    KeyboardTeleop.blueprint(),
+    KeyboardTeleop.blueprint(publish_only_when_active=True),
 ).transports(
     {
         ("twist_command", Twist): LCMTransport("/cmd_vel", Twist),
@@ -229,6 +260,15 @@ def _flowbase_twist_base(
             type="velocity",
             joint_names=_base_joints,
             priority=10,
+            params={"zero_on_timeout": False},
+        ),
+        # Closed-loop path follower used by the benchmark tool.
+        # Inactive until the tool RPCs configure(...) + start_path(...).
+        TaskConfig(
+            name="path_follower",
+            type="path_follower",
+            joint_names=_base_joints,
+            priority=20,
         ),
     ],
 ).transports(
@@ -239,10 +279,66 @@ def _flowbase_twist_base(
 )
 
 
+# FOPDT in-process sim plant + a ControlCoordinator on top, so the
+# tuning tools see exactly the same /cmd_vel + /coordinator/joint_state
+# contract sim and hw. FopdtPlantConnection exposes /sim/cmd_vel (In)
+# and /sim/odom (Out); the coord drives /sim/cmd_vel via its
+# transport_lcm adapter (hardware_id="sim"), reads pose back via the
+# same adapter's /sim/odom subscription, and publishes JointState +
+# hosts the path_follower task. Drop-in stand-in for a real robot.
+_sim_joints = make_twist_base_joints("sim")
+
+coordinator_sim_fopdt = (
+    autoconnect(
+        FopdtPlantConnection.blueprint(),
+        ControlCoordinator.blueprint(
+            hardware=[
+                HardwareComponent(
+                    hardware_id="sim",
+                    hardware_type=HardwareType.BASE,
+                    joints=_sim_joints,
+                    adapter_type="transport_lcm",
+                ),
+            ],
+            tasks=[
+                TaskConfig(
+                    name="vel_sim",
+                    type="velocity",
+                    joint_names=_sim_joints,
+                    priority=10,
+                    params={"zero_on_timeout": False},
+                ),
+                TaskConfig(
+                    name="path_follower",
+                    type="path_follower",
+                    joint_names=_sim_joints,
+                    priority=20,
+                ),
+            ],
+        ),
+    )
+    .remappings(
+        [
+            (FopdtPlantConnection, "cmd_vel", "sim_cmd_vel"),
+            (FopdtPlantConnection, "odom", "sim_odom"),
+        ]
+    )
+    .transports(
+        {
+            ("twist_command", Twist): LCMTransport("/cmd_vel", Twist),
+            ("sim_cmd_vel", Twist): LCMTransport("/sim/cmd_vel", Twist),
+            ("sim_odom", PoseStamped): LCMTransport("/sim/odom", PoseStamped),
+            ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState),
+        }
+    )
+)
+
+
 __all__ = [
     "coordinator_flowbase",
     "coordinator_flowbase_keyboard_teleop",
     "coordinator_flowbase_nav",
     "coordinator_mobile_manip_mock",
     "coordinator_mock_twist_base",
+    "coordinator_sim_fopdt",
 ]

dimos/control/coordinator.py

@@ -45,6 +45,7 @@
     ConnectedWholeBody,
 )
 from dimos.control.task import ControlTask
+from dimos.control.tasks.registry import control_task_registry
 from dimos.control.tick_loop import TickLoop
 from dimos.core.core import rpc
 from dimos.core.module import Module, ModuleConfig
@@ -56,7 +57,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 +160,15 @@ class ControlCoordinator(Module):
     # Input: Teleop buttons for engage/disengage signaling
     buttons: In[Buttons]
 
+    # Input: Live RG corridor half-width (m) for tasks with set_e_max().
+    e_max: In[Float32]
+
+    # Input: Planned path for tasks with set_path(). Typical source is a
+    # nav-stack planner (e.g. ReplanningAStarPlanner). The coord just
+    # forwards the path; tasks read their own latest odom from internal
+    # state populated by compute() each tick.
+    path: In[Path]
+
     # Arming and dry-run are one-shot RPCs, not streams.
 
     def __init__(self, *args: Any, **kwargs: Any) -> None:
@@ -181,6 +193,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._e_max_unsub: Callable[[], None] | None = None
+        self._path_unsub: Callable[[], None] | None = None
 
         logger.info(f"ControlCoordinator initialized at {self.config.tick_rate}Hz")
 
@@ -271,8 +285,6 @@ def _create_whole_body_adapter(self, component: HardwareComponent) -> WholeBodyA
 
     def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask:
         """Create a control task from config via the task registry."""
-        from dimos.control.tasks.registry import control_task_registry
-
         return control_task_registry.create(cfg.type, cfg, hardware=self._hardware)
 
     @rpc
@@ -521,6 +533,62 @@ def _on_buttons(self, msg: Buttons) -> None:
             for task in self._tasks.values():
                 task.on_buttons(msg)
 
+    def _on_e_max(self, msg: Float32) -> None:
+        """Forward e_max (RG corridor half-width) to any task with set_e_max()."""
+        value = float(msg.data)
+        with self._task_lock:
+            for task in self._tasks.values():
+                set_e_max = getattr(task, "set_e_max", None)
+                if set_e_max is not None:
+                    set_e_max(value)
+
+    def _on_path(self, msg: Path) -> None:
+        """Forward a planned path + a fresh odom snapshot to any task with
+        ``set_path(path, odom)``.
+
+        TODO: upgrade to option C — add an always-called ``update_state(state)``
+        hook on ``BaseControlTask`` that ``TickLoop._compute_all_tasks``
+        invokes BEFORE the ``is_active()`` filter. Then tasks own their
+        own odom snapshot every tick regardless of active state, and the
+        ``set_path`` API can drop the ``odom`` arg. Cleaner architecturally
+        — current odom-via-set_path is a surgical fix for the immediate
+        race. See PR/issue [TODO link] for the framework-level change.
+        """
+        odom = 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.PoseStamped import PoseStamped
+                from dimos.msgs.geometry_msgs.Quaternion import Quaternion
+                from dimos.msgs.geometry_msgs.Vector3 import Vector3
+
+                odom = 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]))),
+                )
+                break
+
+        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
+
     @rpc
     def set_activated(self, engaged: bool) -> None:
         """Arm/disarm every task exposing ``arm()`` / ``disarm()``."""
@@ -680,6 +748,36 @@ def start(self) -> None:
             self._buttons_unsub = self.buttons.subscribe(self._on_buttons)
             logger.info("Subscribed to buttons for engage/disengage")
 
+        # Subscribe to e_max if any task implements set_e_max.
+        with self._task_lock:
+            has_e_max_task = any(
+                callable(getattr(task, "set_e_max", None)) for task in self._tasks.values()
+            )
+        if has_e_max_task:
+            try:
+                self._e_max_unsub = self.e_max.subscribe(self._on_e_max)
+                logger.info("Subscribed to e_max for precision-capable tasks")
+            except Exception:
+                logger.warning(
+                    "Precision-capable task configured but could not subscribe to e_max. "
+                    "Set transport via blueprint."
+                )
+
+        # Subscribe to path if any task implements set_path (nav-stack-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."
+                )
+
         # Arming + dry-run are RPC-only; no stream subscription here.
 
         logger.info(f"ControlCoordinator started at {self.config.tick_rate}Hz")
@@ -702,6 +800,12 @@ def stop(self) -> None:
         if self._buttons_unsub:
             self._buttons_unsub()
             self._buttons_unsub = None
+        if self._e_max_unsub:
+            self._e_max_unsub()
+            self._e_max_unsub = None
+        if self._path_unsub:
+            self._path_unsub()
+            self._path_unsub = None
 
         if self._tick_loop:
             self._tick_loop.stop()