Feat/lite biped deploy

robot_assets/workflow/urdf_to_mjcf.py

@@ -216,6 +216,130 @@ def add_freejoint(xml_file_path: Path) -> None:
     print("Added floating_base_joint to first body element")
 
 
+def add_imu_site_and_sensors(xml_file_path: Path, imu: dict) -> None:
+    """Add an IMU <site> on the root body plus the base-state <sensor>s.
+
+    A floating-base locomotion sim needs, at the base:
+      - orientation / angular velocity / linear acceleration -> surfaced to
+        ros2_control's MujocoSystem as the ``<prefix>_imu`` sensor (from MJCF
+        ``<prefix>_quat``/``_gyro``/``_accel``), republished by
+        imu_sensor_broadcaster as sensor_msgs/Imu on /imu/data;
+      - body-frame linear velocity -> ``<prefix>_vel`` velocimeter, read by the
+        base-velocity MuJoCo physics plugin (the RL policy's ``base_lin_vel``
+        obs term; a state estimator supplies it on hardware).
+    A ``framepos`` is emitted too so mujoco_ros2_control's framepos/framequat
+    Odometry publisher (keyed by site name) is well-formed.
+
+    A <site> is mandatory: mujoco_ros2_control resolves every sensor's site via
+    ``mj_id2name(mjOBJ_SITE, ...)`` and SIGABRTs on a siteless model (see
+    add_actuators). Emitted only for floating-base variants (config-gated), so
+    the fixed-base ros2_control variants stay sensor-free.
+    """
+    site_name = imu.get("site", "imu_site")
+    prefix = imu.get("prefix", "base")
+    pos = imu.get("pos", [0.0, 0.0, 0.0])
+
+    tree = ET.parse(xml_file_path)
+    root = tree.getroot()
+    worldbody = root.find("worldbody")
+    first_body = worldbody.find("body") if worldbody is not None else None
+    if first_body is None:
+        print("No body element found in worldbody; skipping IMU site")
+        return
+
+    site = ET.Element("site")
+    site.set("name", site_name)
+    site.set("pos", " ".join(str(v) for v in pos))
+    # Keep a leading freejoint (if any) as the body's first child.
+    lead = 1 if (len(first_body) and first_body[0].tag == "joint"
+                 and first_body[0].get("type") == "free") else 0
+    first_body.insert(lead, site)
+
+    sensor_section = ensure_section(root, "sensor", before_tag_name="actuator")
+    # IMU triad (consumed by MujocoSystem's <prefix>_imu sensor), then the
+    # velocimeter (base linear velocity) and a framepos (odom completeness).
+    quat = ET.SubElement(sensor_section, "framequat")
+    quat.set("name", f"{prefix}_quat")
+    quat.set("objtype", "site")
+    quat.set("objname", site_name)
+    gyro = ET.SubElement(sensor_section, "gyro")
+    gyro.set("name", f"{prefix}_gyro")
+    gyro.set("site", site_name)
+    accel = ET.SubElement(sensor_section, "accelerometer")
+    accel.set("name", f"{prefix}_accel")
+    accel.set("site", site_name)
+    vel = ET.SubElement(sensor_section, "velocimeter")
+    vel.set("name", f"{prefix}_vel")
+    vel.set("site", site_name)
+    fpos = ET.SubElement(sensor_section, "framepos")
+    fpos.set("name", f"{prefix}_pos")
+    fpos.set("objtype", "site")
+    fpos.set("objname", site_name)
+
+    tree.write(xml_file_path, encoding="utf-8", xml_declaration=True)
+    print(f"Added IMU site '{site_name}' + {prefix}_quat/gyro/accel/vel/pos sensors")
+
+
+def patch_contacts(xml_file_path: Path, contact: dict) -> None:
+    """Match mjlab's contact model for a legged robot.
+
+    mjlab's ``patch_spec`` defaults every collision geom to ``condim=1``
+    (frictionless point contact) and then re-promotes only the foot geoms to
+    ``condim=3`` + friction + priority. This means the feet grip the ground while
+    a grazing shin/thigh/self-contact SLIDES instead of catching -- important for
+    a floating-base gait. Our stock MJCF leaves every collision geom at MuJoCo's
+    default ``condim=3`` + friction, so a leg capsule that brushes the ground (or
+    the other leg) grabs and can trip the robot.
+
+    Config (physics.json ``contact``): ``foot_bodies`` (bodies whose collision
+    geoms keep friction), ``foot_friction`` (slide coefficient), ``default_condim``
+    (everything else; 1 = frictionless). Higher ``priority`` on the feet makes the
+    foot friction win over the floor's in the contact pair.
+    """
+    foot_bodies = set(contact.get("foot_bodies", ()))
+    foot_friction = contact.get("foot_friction", 1.0)
+    default_condim = str(int(contact.get("default_condim", 1)))
+    foot_friction_str = f"{foot_friction} 0.005 0.0001"  # slide torsional rolling
+
+    tree = ET.parse(xml_file_path)
+    root = tree.getroot()
+    worldbody = root.find("worldbody")
+    if worldbody is None:
+        return
+
+    def is_collision(geom: ET.Element) -> bool:
+        return geom.get("contype", "1") != "0" or geom.get("conaffinity", "1") != "0"
+
+    n_foot = 0
+    n_other = 0
+
+    def walk(body: ET.Element, in_foot: bool) -> None:
+        nonlocal n_foot, n_other
+        foot = in_foot or (body.get("name") in foot_bodies)
+        for geom in body.findall("geom"):
+            if not is_collision(geom):
+                continue
+            if foot:
+                geom.set("condim", "3")
+                geom.set("friction", foot_friction_str)
+                geom.set("priority", "1")
+                n_foot += 1
+            else:
+                geom.set("condim", default_condim)
+                n_other += 1
+        for child in body.findall("body"):
+            walk(child, foot)
+
+    for body in worldbody.findall("body"):
+        walk(body, False)
+
+    tree.write(xml_file_path, encoding="utf-8", xml_declaration=True)
+    print(
+        f"Patched contacts: {n_foot} foot geom(s) condim=3 friction={foot_friction} "
+        f"priority=1 (bodies {sorted(foot_bodies)}); {n_other} other geom(s) condim={default_condim}"
+    )
+
+
 def apply_joint_properties(xml_file_path: Path, joint_properties: dict) -> None:
     tree = ET.parse(xml_file_path)
     root = tree.getroot()
@@ -260,6 +384,8 @@ def convert(
     out_meshdir: str = "../meshes/visual/",
     physics_options: dict | None = None,
     freejoint: bool = False,
+    imu: dict | None = None,
+    contact: dict | None = None,
     notice: str | None = None,
 ) -> Path:
     urdf_path = Path(urdf_path)
@@ -286,6 +412,10 @@ def convert(
             print(f"Replaced {replaced} cylinder geom(s) with capsules")
         add_option_tag(temp_xml, physics_options or {})
         add_actuators(temp_xml, joint_properties)
+        if imu:
+            add_imu_site_and_sensors(temp_xml, imu)
+        if contact:
+            patch_contacts(temp_xml, contact)
         apply_joint_properties(temp_xml, joint_properties)
         set_compiler_meshdir(temp_xml, out_meshdir)
 
@@ -306,13 +436,20 @@ def generate(robot_dir: Path, *, freejoint: bool = False) -> list[Path]:
     joint_properties = json.loads((cad_dir / "joint_properties.json").read_text())
     physics_path = cad_dir / "physics.json"
     physics_options = json.loads(physics_path.read_text()) if physics_path.exists() else {}
+    # Floating-base + IMU are opt-in per variant via physics.json. Pop them out
+    # of physics_options so they don't leak into the MuJoCo <option> tag.
+    freejoint = bool(physics_options.pop("freejoint", False)) or freejoint
+    imu = physics_options.pop("imu", None)
+    contact = physics_options.pop("contact", None)
     mjcf_path = convert(
         hub_urdf,
         robot_dir / "mjcf" / f"{robot}.xml",
         joint_properties,
         meshes_dir=robot_dir / "meshes" / "visual",
         physics_options=physics_options,
         freejoint=freejoint,
+        imu=imu,
+        contact=contact,
         notice=robot_model.autogen_comment(robot),
     )
     return [mjcf_path]

robot_assets/workflow/urdf_to_xacro.py

@@ -129,12 +129,40 @@ def _group_macro(robot: str, group: str, joints: list[dict], limits: dict) -> st
   </xacro:macro>"""
 
 
-def _combined_macro(robot: str, name: str, active_groups: list[dict], backends: dict) -> str:
+_IMU_STATE_INTERFACES = (
+    "orientation.x", "orientation.y", "orientation.z", "orientation.w",
+    "angular_velocity.x", "angular_velocity.y", "angular_velocity.z",
+    "linear_acceleration.x", "linear_acceleration.y", "linear_acceleration.z",
+)
+
+
+def _imu_sensor_block(imu: dict) -> str:
+    """A ros2_control <sensor> for the base IMU, emitted only under use_sim.
+
+    MujocoSystem backs these state interfaces from the MJCF
+    ``<name-without-_imu>_quat``/``_gyro``/``_accel`` sensors, and
+    imu_sensor_broadcaster republishes them as sensor_msgs/Imu. Guarded by
+    use_sim so the mock backend (no such MuJoCo sensor) is unaffected; real
+    hardware gets its IMU from a dedicated driver node, not ros2_control.
+    """
+    name = imu["name"]
+    ifaces = "\n".join(f'          <state_interface name="{n}"/>' for n in _IMU_STATE_INTERFACES)
+    return f"""      <xacro:if value="${{use_sim}}">
+        <sensor name="{name}">
+{ifaces}
+        </sensor>
+      </xacro:if>"""
+
+
+def _combined_macro(
+    robot: str, name: str, active_groups: list[dict], backends: dict, imu: dict | None = None
+) -> str:
     group_calls = "\n".join(
         f'      <xacro:{robot}_{g["name"]}_joints '
         f'use_fake_hardware="${{use_fake_hardware}}" use_sim="${{use_sim}}"/>'
         for g in active_groups
     )
+    imu_block = ("\n" + _imu_sensor_block(imu)) if imu else ""
     return f"""  <!-- Combined single-block layout for the sim and mock backends. -->
   <xacro:macro name="{robot}_ros2_control_combined" params="name use_fake_hardware use_sim">
     <ros2_control name="${{name}}" type="system">
@@ -146,7 +174,7 @@ def _combined_macro(robot: str, name: str, active_groups: list[dict], backends:
           <plugin>{backends["mock"]}</plugin>
         </xacro:unless>
       </hardware>
-{group_calls}
+{group_calls}{imu_block}
     </ros2_control>
   </xacro:macro>"""
 
@@ -451,7 +479,7 @@ def build_ros2_control_xacro(robot: str, ros2_control: dict, limits: dict) -> st
     parts = [_joint_macro(robot, command, state)]
     for group in groups:
         parts.append(_group_macro(robot, group["name"], joints_by_group.get(group["name"], []), limits))
-    parts.append(_combined_macro(robot, combined_name, active_groups, backends))
+    parts.append(_combined_macro(robot, combined_name, active_groups, backends, ros2_control.get("imu")))
     parts.append(_real_macro(robot, groups, backends))
     parts.append(_top_macro(robot, combined_name))
 

robots/lite_biped/cad/physics.json

@@ -1,8 +1,13 @@
 {
-    "_comment": "MuJoCo <option> for the lite_biped sim -- same actively-damped tuning as the other Lite variants (stiff MIT-mode joints).",
-    "timestep": 0.001,
+    "_comment": "MuJoCo <option> for the lite_biped sim. Matched to mjlab's velocity-task training config (MujocoCfg defaults + the velocity override) so the RL policy sees the same contact/solver dynamics it trained against: timestep 0.005, Newton, implicitfast, PYRAMIDAL cone + impratio 1 (mjlab defaults; note high impratio only pairs with elliptic, so we keep impratio 1 here), iterations 10, ls_iterations 20. freejoint+imu are lite_biped-only (floating base + base-state sensors for the policy); the generator pops them before writing <option>.",
+    "timestep": 0.005,
     "integrator": "implicitfast",
     "solver": "Newton",
-    "cone": "elliptic",
-    "impratio": 10
+    "cone": "pyramidal",
+    "impratio": 1,
+    "iterations": 10,
+    "ls_iterations": 20,
+    "freejoint": true,
+    "imu": {"site": "imu_site", "prefix": "base", "pos": [0.0, 0.0, 0.08]},
+    "contact": {"foot_bodies": ["left_foot", "right_foot"], "foot_friction": 0.6, "default_condim": 1}
 }

robots/lite_biped/cad/ros2_control.json

@@ -0,0 +1,56 @@
+{
+    "_comment": "ROS 2 hardware mapping for the lite_biped (legs-only floating-base humanoid). Drives generation of xacro/lite_biped.ros2_control.xacro. Joint position limits come from the URDF (single source). The sim/mock combined block also advertises a base IMU <sensor> (use_sim only), backed by the MJCF base_quat/base_gyro/base_accel sensors via MujocoSystem and republished by imu_sensor_broadcaster. real-hardware can_id set from a live bus scan (left can2 ids 31-37, right can3 ids 41-47); direction/current_limit still pending commissioning + leg calibration.",
+
+    "interfaces": {
+        "command": ["position", "velocity", "effort", "stiffness", "damping"],
+        "state": ["position", "velocity", "effort"]
+    },
+
+    "backends": {
+        "sim": "mujoco_ros2_control/MujocoSystem",
+        "mock": "mock_components/GenericSystem",
+        "real": "humanoid_devices_robstride/RobstrideSystem"
+    },
+
+    "args": {
+        "use_fake_hardware": "true",
+        "use_sim": "false",
+        "mode": "legs",
+        "can_interface_left": "can2",
+        "can_interface_right": "can3",
+        "calibration_file": ""
+    },
+
+    "base_link": {"name": "base_link", "child": "pelvis"},
+
+    "combined_block_name": "LiteBipedHardware",
+
+    "imu": {"name": "base_imu"},
+
+    "groups": [
+        {"name": "left_leg",  "block_name": "LiteBipedLeftLeg",  "can_interface_arg": "can_interface_left"},
+        {"name": "right_leg", "block_name": "LiteBipedRightLeg", "can_interface_arg": "can_interface_right"}
+    ],
+
+    "modes": {
+        "legs": ["left_leg", "right_leg"]
+    },
+
+    "joints": [
+        {"name": "left_hip_pitch",   "group": "left_leg",  "can_id": 31, "model": "rs-06", "direction": -1, "torque_limit": 36.0, "current_limit": 43},
+        {"name": "left_hip_roll",    "group": "left_leg",  "can_id": 32, "model": "rs-06", "direction": 1, "torque_limit": 36.0, "current_limit": 43},
+        {"name": "left_hip_yaw",     "group": "left_leg",  "can_id": 33, "model": "rs-02", "direction": 1, "torque_limit": 17.0, "current_limit": 27},
+        {"name": "left_knee_pitch",  "group": "left_leg",  "can_id": 34, "model": "rs-06", "direction": -1, "torque_limit": 36.0, "current_limit": 43},
+        {"name": "left_ankle_yaw",   "group": "left_leg",  "can_id": 35, "model": "rs-00", "direction": 1, "torque_limit": 14.0, "current_limit": 16},
+        {"name": "left_ankle_pitch", "group": "left_leg",  "can_id": 36, "model": "rs-00", "direction": 1, "torque_limit": 14.0, "current_limit": 16},
+        {"name": "left_ankle_roll",  "group": "left_leg",  "can_id": 37, "model": "rs-05", "direction": -1, "torque_limit": 5.5,  "current_limit": 14},
+
+        {"name": "right_hip_pitch",   "group": "right_leg", "can_id": 41, "model": "rs-06", "direction": 1, "torque_limit": 36.0, "current_limit": 43},
+        {"name": "right_hip_roll",    "group": "right_leg", "can_id": 42, "model": "rs-06", "direction": 1, "torque_limit": 36.0, "current_limit": 43},
+        {"name": "right_hip_yaw",     "group": "right_leg", "can_id": 43, "model": "rs-02", "direction": 1, "torque_limit": 17.0, "current_limit": 27},
+        {"name": "right_knee_pitch",  "group": "right_leg", "can_id": 44, "model": "rs-06", "direction": 1, "torque_limit": 36.0, "current_limit": 43},
+        {"name": "right_ankle_yaw",   "group": "right_leg", "can_id": 45, "model": "rs-00", "direction": 1, "torque_limit": 14.0, "current_limit": 16},
+        {"name": "right_ankle_pitch", "group": "right_leg", "can_id": 46, "model": "rs-00", "direction": -1, "torque_limit": 14.0, "current_limit": 16},
+        {"name": "right_ankle_roll",  "group": "right_leg", "can_id": 47, "model": "rs-05", "direction": -1, "torque_limit": 5.5,  "current_limit": 14}
+    ]
+}