Implement Mujoco Simulator

bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py

@@ -0,0 +1,209 @@
+import os
+from pathlib import Path
+
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    LogInfo,
+    OpaqueFunction,
+    TimerAction,
+)
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+# Teamplayer arguments to expose (name, description)
+# Empty string default means "not set" - will use teamplayer's default
+# sim is always true for mujoco simulation
+TEAMPLAYER_ARGS = [
+    ("audio", "Whether the audio system should be started"),
+    ("behavior", "Whether the behavior control system should be started"),
+    ("behavior_dsd_file", "The behavior dsd file that should be used"),
+    ("game_controller", "Whether the Gamecontroller module should be started"),
+    ("ipm", "Whether the inverse perspective mapping should be started"),
+    ("localization", "Whether the localization system should be started"),
+    ("motion", "Whether the motion control system should be started"),
+    ("path_planning", "Whether the path planning should be started"),
+    ("teamcom", "Whether the team communication system should be started"),
+    ("vision", "Whether the vision system should be started"),
+    ("world_model", "Whether the world model should be started"),
+    ("monitoring", "Whether the system monitor and udp bridge should be started"),
+    ("record", "Whether the ros bag recording should be started"),
+    ("tts", "Whether to speak"),
+]
+
+
+def generate_domain_bridge_config(robot_domain: int, output_dir: Path) -> Path:
+    """Generate domain bridge config file for a single robot.
+
+    Returns the config file path.
+    """
+    main_domain = int(os.getenv("ROS_DOMAIN_ID", "0"))
+    output_dir.mkdir(parents=True, exist_ok=True)
+
+    namespace = f"robot{robot_domain}"
+
+    config = {
+        "name": f"robot{robot_domain}_bridge",
+        "from_domain": main_domain,
+        "to_domain": robot_domain,
+        "topics": {
+            # Clock: main → robot domain
+            "clock": {
+                "type": "rosgraph_msgs/msg/Clock",
+            },
+        },
+    }
+
+    # Sensor topics: main → robot domain (with remap to remove namespace)
+    sensor_topics = [
+        ("joint_states", "sensor_msgs/msg/JointState"),
+        ("imu/data", "sensor_msgs/msg/Imu"),
+        ("camera/image_proc", "sensor_msgs/msg/Image"),
+        ("camera/camera_info", "sensor_msgs/msg/CameraInfo"),
+        ("foot_pressure_left/filtered", "bitbots_msgs/msg/FootPressure"),
+        ("foot_pressure_right/filtered", "bitbots_msgs/msg/FootPressure"),
+        ("foot_center_of_pressure_left", "geometry_msgs/msg/PointStamped"),
+        ("foot_center_of_pressure_right", "geometry_msgs/msg/PointStamped"),
+    ]
+
+    for topic_suffix, msg_type in sensor_topics:
+        src_topic = f"{namespace}/{topic_suffix}"
+        config["topics"][src_topic] = {
+            "type": msg_type,
+            "remap": topic_suffix,
+        }
+
+    # Command topic: robot domain → main (reversed direction)
+    # Key is source topic in from_domain, remap is destination in to_domain
+    config["topics"]["DynamixelController/command"] = {
+        "type": "bitbots_msgs/msg/JointCommand",
+        "from_domain": robot_domain,
+        "to_domain": main_domain,
+        "remap": f"{namespace}/DynamixelController/command",
+    }
+
+    config_path = output_dir / f"robot{robot_domain}_bridge.yaml"
+    with open(config_path, "w") as f:
+        yaml.dump(config, f, default_flow_style=False, sort_keys=False)
+
+    return config_path
+
+
+def generate_world_xml(num_robots: int, package_share: str) -> Path:
+    """Generate MuJoCo world XML with the correct number of robots."""
+    template_path = Path(package_share) / "xml" / "adult_field.xml"
+    output_path = Path(package_share) / "xml" / "generated_world.xml"
+
+    with open(template_path) as f:
+        template = f.read()
+
+    # Replace placeholder with actual robot count
+    world_xml = template.replace("{{NUM_ROBOTS}}", str(num_robots))
+
+    with open(output_path, "w") as f:
+        f.write(world_xml)
+
+    return output_path
+
+
+def launch_setup(context):
+    """Dynamically set up launches based on num_robots."""
+    num_robots = int(LaunchConfiguration("num_robots").perform(context))
+    package_share = get_package_share_directory("bitbots_mujoco_sim")
+    bridge_config_dir = Path(package_share) / "config" / "domain_bridges"
+
+    # Get teamplayer argument values - only pass if explicitly set (not empty)
+    teamplayer_args = ["sim:=true"]  # sim is always true for mujoco simulation
+    for arg_name, _ in TEAMPLAYER_ARGS:
+        value = LaunchConfiguration(arg_name).perform(context)
+        if value:  # Only pass if not empty string
+            teamplayer_args.append(f"{arg_name}:={value}")
+
+    world_file = generate_world_xml(num_robots, package_share)
+
+    actions = []
+
+    actions.append(
+        LogInfo(msg=f"Starting MuJoCo simulation with {num_robots} robot(s)"),
+    )
+    actions.append(
+        Node(
+            package="bitbots_mujoco_sim",
+            executable="sim",
+            name="sim_interface",
+            output="screen",
+            emulate_tty=True,
+            parameters=[{"world_file": str(world_file)}],
+        ),
+    )
+
+    for robot_domain in range(num_robots):
+        config_file = generate_domain_bridge_config(robot_domain, bridge_config_dir)
+        actions.append(
+            LogInfo(msg=f"Starting domain bridge for robot{robot_domain} (domain {robot_domain})"),
+        )
+        actions.append(
+            Node(
+                package="domain_bridge",
+                executable="domain_bridge",
+                name=f"domain_bridge_robot{robot_domain}",
+                arguments=[str(config_file)],
+                output="screen",
+                emulate_tty=True,
+            ),
+        )
+
+        actions.append(
+            TimerAction(
+                period=3.0,
+                actions=[
+                    LogInfo(msg=f"Launching teamplayer stack for robot{robot_domain} in domain {robot_domain}"),
+                    ExecuteProcess(
+                        cmd=[
+                            "ros2",
+                            "launch",
+                            "bitbots_bringup",
+                            "teamplayer.launch",
+                        ]
+                        + teamplayer_args,
+                        output="screen",
+                        additional_env={"ROS_DOMAIN_ID": str(robot_domain)},
+                    ),
+                ],
+            )
+        )
+
+    return actions
+
+
+def generate_launch_description():
+    """Launch MuJoCo simulation with domain bridge for multi-robot support."""
+
+    declared_args = [
+        DeclareLaunchArgument(
+            "num_robots",
+            default_value="1",
+            description="Number of robots in the simulation",
+        ),
+    ]
+
+    # Add all teamplayer arguments with empty default (means use teamplayer's default)
+    for arg_name, description in TEAMPLAYER_ARGS:
+        declared_args.append(
+            DeclareLaunchArgument(
+                arg_name,
+                default_value="",
+                description=description,
+            )
+        )
+
+    return LaunchDescription(
+        declared_args
+        + [
+            # All setup happens in OpaqueFunction to ensure proper ordering
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml

@@ -21,7 +21,7 @@ walking:
       freq: 1.2
       trunk_height: 0.394780002666927
       trunk_phase: -0.151653984431689
-      trunk_pitch: 0.105566178884548
+      trunk_pitch: 0.45
       trunk_pitch_p_coef_forward: -0.186068274875133
       trunk_pitch_p_coef_turn: -0.457339940581988
       trunk_swing: 0.154856652745882

bitbots_robot/wolfgang_animations/animations/standup/stand_up_back.json

@@ -38,7 +38,7 @@
                 "LAnklePitch": -81.47,
                 "LAnkleRoll": -2.0,
                 "LElbow": -16.17,
-                "LHipPitch": -38.85,
+                "LHipPitch": -8.85,
                 "LHipRoll": -2.2,
                 "LHipYaw": 0.0,
                 "LKnee": 124.0,
@@ -47,7 +47,7 @@
                 "RAnklePitch": 78.7,
                 "RAnkleRoll": 2.0,
                 "RElbow": 13.37,
-                "RHipPitch": 36.84,
+                "RHipPitch": 6.84,
                 "RHipRoll": -2.2,
                 "RHipYaw": 0.0,
                 "RKnee": -124.0,
@@ -66,7 +66,7 @@
                 "LAnklePitch": -80.0,
                 "LAnkleRoll": -2.0,
                 "LElbow": -90.0,
-                "LHipPitch": 51.0,
+                "LHipPitch": 71.0,
                 "LHipRoll": -2.2,
                 "LHipYaw": 0.0,
                 "LKnee": 143.0,
@@ -75,15 +75,15 @@
                 "RAnklePitch": 80.0,
                 "RAnkleRoll": 2.0,
                 "RElbow": 90.0,
-                "RHipPitch": -51.0,
+                "RHipPitch": -71.0,
                 "RHipRoll": 2.2,
                 "RHipYaw": 0.0,
                 "RKnee": -143.0,
                 "RShoulderPitch": -54.84,
                 "RShoulderRoll": -0.0
             },
             "name": "push",
-            "pause": 0.2,
+            "pause": 0.1,
             "torque": {}
         },
         {
@@ -94,20 +94,20 @@
                 "LAnklePitch": -80.0,
                 "LAnkleRoll": -2.0,
                 "LElbow": -90.0,
-                "LHipPitch": 51.0,
+                "LHipPitch": 91.0,
                 "LHipRoll": -2.2,
                 "LHipYaw": 0.0,
                 "LKnee": 144.0,
-                "LShoulderPitch": -9.0,
+                "LShoulderPitch": -75.0,
                 "LShoulderRoll": 0.0,
                 "RAnklePitch": 80.0,
                 "RAnkleRoll": 2.0,
                 "RElbow": 90.0,
-                "RHipPitch": -51.0,
+                "RHipPitch": -91.0,
                 "RHipRoll": 2.2,
                 "RHipYaw": 0.0,
                 "RKnee": -144.0,
-                "RShoulderPitch": 9.0,
+                "RShoulderPitch": 75.0,
                 "RShoulderRoll": 0.0
             },
             "name": "squat",

bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/__init__.py

@@ -0,0 +1,5 @@
+"""BitBots MuJoCo Simulation Package"""
+
+__all__ = [
+    "simulation",
+]

bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/camera.py

@@ -0,0 +1,55 @@
+import mujoco
+import numpy as np
+
+
+class Camera:
+    """Represents a camera in the MuJoCo simulation, providing image rendering capabilities."""
+
+    def __init__(self, model: mujoco.MjModel, data: mujoco.MjData, name: str, width: int = 800, height: int = 600):
+        self.model: mujoco.MjModel = model
+        self.data: mujoco.MjData = data
+        self.name: str = name
+        self.instance = model.camera(name)
+        self.width: int = width
+        self.height: int = height
+        self.renderer = mujoco.Renderer(self.model, height=self.height, width=self.width)
+
+    @property
+    def fov(self) -> float:
+        """Returns the camera's horizontal field of view in radians."""
+        if hasattr(self, "_fov") and self._fov is not None:
+            return self._fov
+
+        # MuJoCo uses fovy (vertical field of view in degrees)
+        fovy_deg = self.instance.fovy[0] if hasattr(self.instance.fovy, "__iter__") else self.instance.fovy
+        fovy_rad = np.deg2rad(fovy_deg)
+
+        # Convert vertical FOV to horizontal FOV using aspect ratio
+        aspect_ratio = self.width / self.height
+        fovx_rad = 2 * np.arctan(np.tan(fovy_rad / 2) * aspect_ratio)
+
+        self._fov = fovx_rad
+        return self._fov
+
+    def render(self) -> bytes:
+        """
+        Renders and returns the camera image as raw bytes (BGRA format).
+        Returns Raw image data in BGRA8 format for ROS Image messages.
+        """
+        # Update renderer with current scene
+        self.renderer.update_scene(self.data, camera=self.name)
+
+        # Render the image - returns RGB by default
+        rgb_array = self.renderer.render()
+
+        # Convert RGB to BGRA (standard for ROS Image messages)
+        # MuJoCo returns RGB uint8 array of shape (height, width, 3)
+        # We need to add alpha channel and swap R and B
+        height, width, _ = rgb_array.shape
+        bgra_array = np.zeros((height, width, 4), dtype=np.uint8)
+        bgra_array[:, :, 0] = rgb_array[:, :, 2]  # B
+        bgra_array[:, :, 1] = rgb_array[:, :, 1]  # G
+        bgra_array[:, :, 2] = rgb_array[:, :, 0]  # R
+        bgra_array[:, :, 3] = 255  # A (fully opaque)
+
+        return bgra_array.tobytes()

bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/joint.py

@@ -0,0 +1,39 @@
+import mujoco
+
+
+class Joint:
+    """Represents a single controllable joint and associated actuator in the MuJoCo simulation."""
+
+    def __init__(
+        self,
+        model: mujoco.MjModel,
+        data: mujoco.MjData,
+        ros_name: str,
+        name: str | None = None,
+    ):
+        self.model: mujoco.MjModel = model
+        self.data: mujoco.MjData = data
+        self.ros_name: str = ros_name
+        self.name: str = name if name is not None else ros_name
+        self.joint_instance: int = model.joint(self.name)
+        self.actuator_instance: int = model.actuator(self.name)
+
+    @property
+    def position(self) -> float:
+        """Gets the current joint position (angle) in radians."""
+        return self.data.qpos[self.joint_instance.qposadr[0]]
+
+    @position.setter
+    def position(self, value: float) -> None:
+        """Sets the position target for the joint's position actuator."""
+        self.data.ctrl[self.actuator_instance.id] = value
+
+    @property
+    def velocity(self) -> float:
+        """Gets the current joint velocity in rad/s."""
+        return self.data.qvel[self.joint_instance.dofadr[0]]
+
+    @property
+    def effort(self) -> float:
+        """Gets the current effort (force/torque) applied by the position actuator."""
+        return self.data.actuator_force[self.actuator_instance.id]