Go2 Speed vs Precision testing

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,
+            velocity_zero_on_timeout=False,
+        ),
+        # Closed-loop path follower used by the benchmark tool.
+        # Inactive until the tool RPCs configure(...) + start_path(...).
+        TaskConfig(
+            name="baseline_follower",
+            type="baseline_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,
+            velocity_zero_on_timeout=False,
+        ),
+        # Closed-loop path follower used by the benchmark tool.
+        # Inactive until the tool RPCs configure(...) + start_path(...).
+        TaskConfig(
+            name="baseline_follower",
+            type="baseline_path_follower",
+            joint_names=_base_joints,
+            priority=20,
         ),
     ],
 ).transports(
@@ -229,6 +250,15 @@ def _flowbase_twist_base(
             type="velocity",
             joint_names=_base_joints,
             priority=10,
+            velocity_zero_on_timeout=False,
+        ),
+        # Closed-loop path follower used by the benchmark tool.
+        # Inactive until the tool RPCs configure(...) + start_path(...).
+        TaskConfig(
+            name="baseline_follower",
+            type="baseline_path_follower",
+            joint_names=_base_joints,
+            priority=20,
         ),
     ],
 ).transports(
@@ -239,10 +269,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 baseline_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,
+                    velocity_zero_on_timeout=False,
+                ),
+                TaskConfig(
+                    name="baseline_follower",
+                    type="baseline_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

@@ -90,6 +90,9 @@ class TaskConfig:
     type: str = "trajectory"
     joint_names: list[str] = field(default_factory=lambda: [])
     priority: int = 10
+    # Velocity-task specific. zero_on_timeout=True keeps the task streaming zeros
+    velocity_timeout: float = 0.2
+    velocity_zero_on_timeout: bool = True
     # Cartesian IK / Teleop IK specific
     model_path: str | Path | None = None
     ee_joint_id: int = 6
@@ -320,9 +323,29 @@ def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask:
                 JointVelocityTaskConfig(
                     joint_names=cfg.joint_names,
                     priority=cfg.priority,
+                    timeout=cfg.velocity_timeout,
+                    zero_on_timeout=cfg.velocity_zero_on_timeout,
                 ),
             )
 
+        elif task_type == "baseline_path_follower":
+            from dimos.control.tasks.baseline_path_follower_task import (
+                BaselinePathFollowerTask,
+                BaselinePathFollowerTaskConfig,
+            )
+            from dimos.core.global_config import global_config as _gc
+
+            # Idle defaults; per-run params come from the tool via the
+            # task's `configure()` RPC immediately before `start_path()`.
+            return BaselinePathFollowerTask(
+                cfg.name,
+                BaselinePathFollowerTaskConfig(
+                    joint_names=cfg.joint_names,
+                    priority=cfg.priority,
+                ),
+                global_config=_gc,
+            )
+
         elif task_type == "cartesian_ik":
             from dimos.control.tasks.cartesian_ik_task import CartesianIKTask, CartesianIKTaskConfig