Control Coordinator Refactor: New Message Type

[mustafab0]

Goal

Every robot is a connection module in its own process. The control coordinator talks to all of them with one message type, decides which controller drives which part of which robot, and owns no hardware.

[mustafab0]

The wire: ControlValues

source, source_ts, epoch, sequence, and paired interface_names / values (float64). Two shared streams:

• control_state: each connection publishes everything it measures.
• control_command: the coordinator publishes a complete frame per connection, every tick.

Names are keys of the form source/resource/interface, e.g. arm/joint1/position, chassis/base/vx, g1/imu/qw. sequence orders frames. epoch stays 0 until the deferred safety work.

This is example of Control coordinator sending commands to the Go2

ControlValues(
    source="ControlCoordinator", sequence=1042,
    interface_names=["go2/base/vx", "go2/base/vy", "go2/base/wz"],
    values=         [0.5,           0.0,           0.2],
)

The following is example of Go2 sending its velocity and odom reading to the control coordinator

ControlValues(
    source="go2", sequence=88,
    interface_names=["go2/base/vx", "go2/base/vy", "go2/base/wz", "go2/base/x", "go2/base/y", "go2/base/yaw"],
    values=         [0.48,          0.01,          0.19,          3.2,          -1.1,         0.75],
)

And you would describe the contract with:

twist_base_description("go2", limits=..., odometry=True, measured_velocity=True)
# produces:
Resource(name="base", kind=BASE,
    state_interfaces   = ("vx", "vy", "wz", "x", "y", "yaw"),   # what it reports
    command_interfaces = ("vx", "vy", "wz"),                    # what it accepts
    units = {"vx": "m/s", "vy": "m/s", "wz": "rad/s", "x": "m", "y": "m", "yaw": "rad"})

It is quite useful for Manipulators that have different modes:

A xarm7 moving in position mode, and gripper moves in position mode:

ControlValues(
    source="ControlCoordinator",
    source_ts=1759550000.120,
    epoch=0,
    sequence=20417,
    interface_names=[
        "arm/joint1/position", "arm/joint2/position", "arm/joint3/position", "arm/joint4/position", "arm/joint5/position", arm/joint6/position", "arm/joint7/position", "arm/gripper/position",
    ],
    values=[0.10, -0.35, 0.02, 0.78, -0.05, 1.12, -0.42, 0.040],
)

A xarm7 moving in velocity mode, but gripper moves in position mode:

ControlValues(
    source="ControlCoordinator",
    source_ts=1759550000.130,
    epoch=0,
    sequence=20418,
    interface_names=[
        "arm/joint1/velocity", "arm/joint2/velocity", "arm/joint3/velocity", "arm/joint4/velocity", "arm/joint5/velocity", arm/joint6/velocity", "arm/joint7/velocity", "arm/gripper/position",
    ],
    values=[0.20, -0.10, 0.05, 0.0, 0.0, 0.0, 0.0, 0.040],
)

[mustafab0]

Claim 1:

Secondly, as mentioned earlier, muxing or arbitrating between different msg types is difficult. As we start doing complex motions with mobile manipulation we need to arbitrate Twist and JointState messages somehow.

@leshy's response:
controlcoordinator arbitrates internally, that's what it's for?

Claim 2:

You would have to break down the abstraction and rebuild it in some module, case by case basis

@leshy's response:
no, go2 understands twist, arm understands jointstates, there is never need internally to understand global coordination happening, the same way that go2 twist doesn't understand navigation or trajectory control

[mustafab0]

They are not on a single topic. Each robot has its own ports and respective topics topic.

[leshy]

can you show an example of a driver input topics using this new messaging system? my understanding from documentation so far is that it accepts a single topic for ControlValues

[mustafab0]

A go2 with a manipulator would look something like this:

class Go2ArmCoordinator(ControlCoordinator):
    go2_control_command: Out[ControlValues]
    arm_control_command: Out[ControlValues]

blueprint = autoconnect(
    Go2ArmCoordinator.blueprint(publish_robot_commands=True, tasks=[...]),
    Go2ConnectionModule.blueprint(),
    XArmConnectionModule.blueprint(),
).remappings([
  
    (Go2ConnectionModule, "control_command", "go2_control_command"),
    (XArmConnectionModule, "control_command", "arm_control_command"),
])

[leshy]

aha was asking for a driver I/O as my undesrtanding is that all control is going through the single topic that accepts ControlValues - that's what I meant by "I disagree that all drivers need to switch to a new custom message on a single topic" I understand that it's a single topic PER driver (which btw probably doesn't matter as they can share the topic given you pack all control messages into the same type)

[mustafab0]

I see! I misunderstood.

Here is what a Go2 connection module would look like

class Go2ConnectionModule(ConnectionModule):
     control_command: In[ControlValues]   # go2/base/vx, vy, wz
     control_state:   Out[ControlValues]  # go2/base/x, y, yaw
     odom: Out[PoseStamped]
     lidar: Out[PointCloud2]
     pointcloud: Out[PointCloud2]
     color_image: Out[Image]
     camera_info: Out[CameraInfo]
     tf: Out[TFMessage]

     def describe(self):  
     def connect(self):    
     def read_state(self): 
     def write(self, v):   
     def halt(self):       

And the arm module

class XArmConnectionModule(ConnectionModule):
    control_command: In[ControlValues]   # arm/joint1..7/position or /velocity, arm/gripper/position
    control_state:   Out[ControlValues]  # arm/joint1..7/position, /effort, arm/gripper/position

    def describe(self):   
    def connect(self):   
    def read_state(self): 
    def write(self, v):   
    def halt(self):       

[leshy]

#3390 (comment)

commented here but can continue in this topic

discussioncomment-18740734 below ⬇️

main issue, custom message, rewriting all drivers

I believe you are leaking an abstraction that belongs internally into a controlcoordinator, requiring a rewrite of all drivers, and causing incompatibility with any other system, to add a custom message that adds no new information, and removes individual topics, types etc that other systems understand. you are ignoring topics and frames, using a single one in dimos, then re-implementing topics and frames in your new custom message

I would say you can do all above that you'd like but keep those messages internal to controlcoordinator and translate into standards externally

your ros2_control example is actually good, it's a library used within ros nodes (this makes sense) but it still respects messages like twist etc for external system interaction. it never breaks any nodes external interface

answers/questions

from discord:

Firstly, standard messages lack all the information needed to command all robots.
For e.g I am already using a custom MotorCommand msg to interface with g1, go2 for low level control.

in a rare case something needs a custom control message, we can add it, but in most cases these are standard messages. custom messages don't justify removing all standard messages - but adding new custom messages

All your examples above carry the same (or less!) information then standard messages so I don't see why you are inventing a new type?

Secondly, as mentioned earlier, muxing or arbitrating between different msg types is difficult. As we start doing complex motions with mobile manipulation we need to arbitrate Twist and JointState messages somehow.

Yes but that's exactly why controlcoordinator exists? I send twist to controlcoordinator, and it controls driver modules with whatever language they understand, those SDKs and drivers don't have to understand global complex motion or msg type conversions

You would have to break down the abstraction and rebuild it in some module, case by case basis

Idea is not to do this at all. you keep your abstractions internal to controlcoordinator, and drivers do driver things. for example go2 understands twist, arm understands jointstates, there is never need internally for go2 driver to understand global coordination happening, the same way that go2 twist doesn't understand navigation or trajectory control but is able to navigate.

5. "Why we removed standardized abstraction, and we are rebuilding it internally only in this module?"
   Standard messages describe what kind of command something is (a velocity, a joint target).
   They don't say which motor it is for or who controls that motor right now, and arbitration needs both.

they do describe motor/entity by frame_id or topic (you actually invented new topic/frame abstraction in your message examples, just use topics or frame ids?)

One part, two kinds of command: a planner sends an arm positions while teleop sends it velocities. That's two ports into the same joints, and something has to know that to pick a winner.

yeah, controlcoordinator, above whole section is supposed to be answering my question on "why new topics" but is justifying existance of controlcoordinator, why? we agree there should be a controlcoordinator.

Moves that must happen together: an R1 Pro's base, torso and arms in one step. Separate topics arrive separately, and stopping one doesn't stop the others.

but you have external modules, so your topics are still separate, this doesn't help the problem but imo also it's not a problem, our pubsub is low latency, your proposal uses separate topics, my proposal uses separate topics, and that's ok

Safety checks repeated per message type in every driver.

depends on the safety layer and driver responsibilities, go2 twist should not do colision checks for example, that's a higher order operation, but it can make sure to stop if twist input stops, which is a low level driver responsibility and will stay there, low level driver should never blindly respect twist sent 5 minutes ago, even if controlcoordinator crashed. so there are different layers of safety on different layers of abstractions

general on topics/frames etc

it's possible that something is lacking and inconvinient here in terms of existing topics frames etc (we'll nail it down now) but then we should fix topics, frames etc instead of reimplement

[TomCC7]

I see your main concern here.

The drawback of this 'custom control message' is that we need to write all connection modules as dedicated to communicate with control coordinator (because of the custom type). but this is one of the design goal... (control coordinator should be the middle layer for all commands to hardware) I don't see a large issue for the goal but this is something we should discuss - do we want some upstream to command the connection module directly without control coordinator?

In the other way you proposed which also seems legit to me (using all standard typing for command and let control coordinator emit the corresponding message type), we need to figure out how to arbitrate between different message types for all connection modules. (like twist for moving base, velocity/torque/position for robot joints etc). My primitive idea without verification would be we reuse the pre-launch configuration design to create a subclass of control coordinator to expose all output types and wire correctly to the corresponding connection modules. I wonder what's the best way you can think of for this scenario.

CC @mustafab0

[leshy]

we need to figure out how to arbitrate between different message types for all connection modules.

you need to do this either way since that's what controlcoordinator does. you are just wrapping existing Twist, Pose into some new format, but hardware takes Twist or Pose and such, so you are still doing arbitration. Question is only if this arbitration is happening in drivers or in controlcoordinator and I say your abstractions should stay in your module.

The drawback of this 'custom control message' is that we need to write all connection modules as dedicated to communicate with control coordinator (because of the custom type).

This is one drawback but there are many deeper ones. Main thing is you are inventing a new message type that brings no new information and no upside, but breaks compatibility with ros, visualization systems, any alternative control metodologies, (mcap? CDR encoding? gone) even removes dimensional topics for different types (you have a new concept of topic or frame_id you call interface_names)

I don't see a large issue for the goal but this is something we should discuss - do we want some upstream to command the connection module directly without control coordinator?

This is imo important - new systems no matter how big their goals are, should always connect to the system using standard interfaces especially if those standard interfaces are adequate - as they are in this case. dimensional should be modular, systems removable, upgradable. I understand that your goal is to control everything, but that was the goal for control coordinator one, two and three, navigation was goal for navigation one two three up to five I guess, imagine each of these components changed our core messages, imagine they removed information or topics from dimos?

[TomCC7]

you need to do this either way since that's what controlcoordinator does

Sorry for reusing the term and caused the confusion. The only arbitration control coordinator currently does is in terms of which controller task wins the control over joints. It does not handle which action interface it should publish to (which is also kind of static after launch). So I guess we need a different term to define this part of the job (let's call it command routing maybe).

I agree with the other parts tho. maybe it's better to just see how to handle the command routing to different message types.

[mustafab0]

This is imo important rule of thumb - new systems no matter how big their goals are, should always and connect to the system using standard interfaces,

I will concede to this. I think you are right about keeping things consistent here.
Although I would disagree that ControlValues add no additional information or benefit.

Standard typed messages make sense, I will convert the messages at the edges, the arbitration logic remains the same.

I have revised my PR stack. Following is the proposed architecture.

Final architecture

flowchart LR
  IN["nav · teleop · nav"] -- "Twist" --> CC
  IN["manip · teleop · nav"] -- "JointState" --> CC

  subgraph CC["ControlCoordinator"]
    direction LR
    CIN["convert to named values<br/>go2/base/vx · arm/joint1/position"] --> AR["arbitrate<br/>per base / joint"] --> COUT["convert back"]
  end

  COUT -- "Twist" --> GO2["Go2"]
  COUT -- "JointState" --> ARM["xArm"]
  GO2 -- "PoseStamped" --> CC
  ARM -- "JointState" --> CC

Loading

Here is the PR stack for review: #4263

cc @leshy @TomCC7

[leshy]

@mustafab0 Great, without going into internals of your implementation which is your perview, above graph of external interfaces looks good to me.

I cannot comment on the actual spec though, PR itself or the stack since I don't have the full new spec but only the graph above.

IDK how for example go2 specifies the interface towards the control coordinator, there was mention of RPCs and dynamic discovery, so before aproving the full design this is what I'd discuss next. maybe go2 carrying pose controlled arm is a good example robot

[mustafab0]

IDK how for example go2 specifies the interface towards the control coordinator, there was mention of RPCs and dynamic discovery, so before aproving the full design this is what I'd discuss next. maybe go2 carrying pose controlled arm is a good example robot

Each driver describes itself in describe() built from its config:

class Go2ConnectionModule(ConnectionModule):
    base_command: In[Twist]
    odom:         Out[PoseStamped]

    def describe(self):
        return twist_base_description(
            "go2",
            limits={"go2/base/vx": Limits(-1.0, 1.0),
                    "go2/base/vy": Limits(-0.6, 0.6),
                    "go2/base/wz": Limits(-1.5, 1.5)},
            odometry=True,            # reports x, y, yaw
            state_rate_hz=50.0,
            deadman_timeout_s=0.2,
        )


class ArmConnectionModule(ConnectionModule):
    position_command: In[JointState]
    joint_state:      Out[JointState]

    def describe(self):
        # joint names, units and limits come from the arm's URDF
        return manipulator_description("arm", JOINTS, model=self.config.model)

On blueprint launch Control Coordinator calls describe_control() on all modules launched. And every time, a new module launches during runtime, the control coordinator will make an describe_control rpc call to the launched module.

This can help changing robot modes in runtime. For example switch go2 from the high-level (Twist) control and bring up a low-level control for running custom policy, without restarting the whole system.

Its describe() returns its joints instead of a base, and the coordinator picks it up on the next launch.

flowchart LR
  G["Go2"]
  A["Arm"]
  subgraph CC["ControlCoordinator"]
    P1["Go2: ports, limits, state rate"]
    P2["Arm: ports, limits, state rate"]
  end
  CC -- "@rpc describe_control()" --> G
  CC -- "@rpc describe_control()" --> A
  G -. "description" .-> CC
  A -. "description" .-> CC

Loading

code example

# coordinator, at start and after every load / restart
for module in connection_modules:
    info = module.describe_control()             # description + session ID
    for robot in info.descriptions:
        ports   = ports_for(robot)               # go2 → go2_base_command, go2_odom
        limits  = robot.limits                   # never send past these
        rate    = robot.state_rate_hz            # stop commanding if readings stop
        deadman = robot.deadman_timeout_s        # command at least this often
        if info.session_id != last_session[robot.source]:
            hold_where_it_is_now(robot)          # driver restarted

cc: @leshy @TomCC7

[mustafab0]

@leshy this makes sense. We can just package control description in the Config, which would be the best place for this.

I would then do the following:

class Go2Config(ConnectionConfig):
    control = twist_base(                    # a config field, so .env / CLI / blueprint can override it
        cmd_vel=TwistRange(Twist(1.0, 0.6, 1.5)),   # keyed by the port, limits in the message's own type
        odom=True, state_rate_hz=50, deadman_timeout_s=0.2)

class Go2ConnectionModule(ConnectionModule):
    default_config = Go2Config
    cmd_vel: In[Twist]             # the driver's own name, same as without a coordinator
    odom: Out[PoseStamped]

    def connect(self): ...         # the four hardware methods are unchanged
    def read_state(self): ...
    def write(self, values): ...
    def halt(self): ...

class Go2Config(ConnectionConfig):
  control = twist_base(                    # a config field, so .env / CLI / blueprint can override it
      cmd_vel=TwistRange(Twist(1.0, 0.6, 1.5)),   # keyed by the port, limits in the message's own type
      odom=True, state_rate_hz=50, deadman_timeout_s=0.2)

class Go2ConnectionModule(ConnectionModule):
  default_config = Go2Config
  cmd_vel: In[Twist]             # the driver's own name, same as without a coordinator
  odom: Out[PoseStamped]

  def connect(self): ...         # the four hardware methods are unchanged
  def read_state(self): ...
  def write(self, values): ...
  def halt(self): ...

And then for auto connect we have

    # without control coordinator
    autoconnect(nav, Go2ConnectionModule.blueprint()) 

    # with a control coordinator
    autoconnect(nav, Go2ConnectionModule.blueprint(), Go2Coordinator.blueprint(tasks=[...]))
    .remappings([(Go2ConnectionModule, "cmd_vel", "go2_cmd_vel")])

Regarding the typing vs RPC. The RPC method allows for detection of ConnectionModules that were started or restarted during runtime. This may be a feature that is valuable. I don't have a need for this right now. Happy to drop it now but will bring up if there is a valid use case for this.

[paul-nechifor]

def connect(self): ... # the four hardware methods are unchanged
def read_state(self): ...
def write(self, values): ...
def halt(self): ...

What are the 4 methods needed for?

Shouldn't at least read_state and write use streams?

[mustafab0]

The ports exist on the ConnectionModule and joint states are sent and received using topics, like JointState and Twist.

The read_state, write are the internal methods that call the robot SDK to executes the commands on the robot.

[paul-nechifor]

removed

I just realized I misunderstood the methods.

[mustafab0]

Happy to elaborate more if you have any more questions