Pim/feat/g1 groot wbc

dimos/control/components.py

@@ -18,6 +18,8 @@
 from enum import Enum
 from typing import Any
 
+from dimos.hardware.whole_body.spec import WholeBodyConfig
+
 HardwareId = str
 JointName = str
 TaskName = str
@@ -61,6 +63,16 @@ class HardwareComponent:
         address: Connection address - IP for TCP, port for CAN
         auto_enable: Whether to auto-enable servos
         gripper_joints: Joints that use adapter gripper methods (separate from joints).
+        domain_id: DDS domain ID for adapters that use DDS transport
+            (e.g. Unitree G1). Real robot uses 0; unitree_mujoco sim
+            defaults to 1. Ignored by non-DDS adapters.
+        adapter_kwargs: Generic untyped kwargs forwarded to the adapter
+            constructor — use for adapter-specific knobs that don't
+            belong in the spec.
+        wb_config: Whole-body-specific config (PD gains etc.).  Populate
+            on hardware_type=WHOLE_BODY components.  Keeps WB-only knobs
+            off the generic HardwareComponent shared by manipulators,
+            bases, and grippers.
     """
 
     hardware_id: HardwareId
@@ -70,7 +82,9 @@ class HardwareComponent:
     address: str | None = None
     auto_enable: bool = True
     gripper_joints: list[JointName] = field(default_factory=list)
+    domain_id: int = 0
     adapter_kwargs: dict[str, Any] = field(default_factory=dict)
+    wb_config: WholeBodyConfig | None = None
 
     @property
     def all_joints(self) -> list[JointName]:

dimos/control/hardware_interface.py

@@ -332,6 +332,34 @@ def __init__(
         self._component = component
         self._joint_names = component.joints
 
+        # Resolve per-joint PD gains once at wire-up time.  Gains live on
+        # the WB-specific sub-config; fall back to _DEFAULT_KP/_DEFAULT_KD
+        # if the blueprint didn't supply a wb_config.
+        n = len(self._joint_names)
+        wb = component.wb_config
+        kp_in = wb.kp if wb is not None else None
+        kd_in = wb.kd if wb is not None else None
+        if kp_in is not None:
+            if len(kp_in) != n:
+                raise ValueError(
+                    f"HardwareComponent '{component.hardware_id}': wb_config.kp length "
+                    f"{len(kp_in)} does not match joints length {n}"
+                )
+            self._kp = list(kp_in)
+        else:
+            self._kp = [_DEFAULT_KP] * n
+        if kd_in is not None:
+            if len(kd_in) != n:
+                raise ValueError(
+                    f"HardwareComponent '{component.hardware_id}': wb_config.kd length "
+                    f"{len(kd_in)} does not match joints length {n}"
+                )
+            self._kd = list(kd_in)
+        else:
+            self._kd = [_DEFAULT_KD] * n
+        self._kp_by_name = dict(zip(self._joint_names, self._kp, strict=False))
+        self._kd_by_name = dict(zip(self._joint_names, self._kd, strict=False))
+
         self._last_commanded: dict[str, float] = {}
         self._initialized = False
         self._warned_unknown_joints: set[str] = set()
@@ -361,10 +389,13 @@ def read_state(self) -> dict[JointName, JointState]:
         }
 
     def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool:
-        """Write position commands — converts to MotorCommand with PD gains.
+        """Write position commands — converts to MotorCommand with per-joint PD gains.
 
         Only POSITION / SERVO_POSITION are supported; other modes are warned
         and dropped (matches ConnectedHardware's warn-and-skip pattern).
+        Per-joint kp/kd come from ``component.wb_config`` (resolved in
+        ``__init__``); fall back to ``_DEFAULT_KP``/``_DEFAULT_KD`` when
+        the blueprint didn't supply gains.
         """
         from dimos.hardware.whole_body.spec import MotorCommand
 
@@ -392,8 +423,8 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool:
             MotorCommand(
                 q=self._last_commanded[name],
                 dq=0.0,
-                kp=_DEFAULT_KP,
-                kd=_DEFAULT_KD,
+                kp=self._kp_by_name[name],
+                kd=self._kd_by_name[name],
                 tau=0.0,
             )
             for name in self._joint_names