Implement Mujoco Simulator

bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py

@@ -0,0 +1,67 @@
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo, TimerAction
+from launch.conditions import IfCondition
+from launch.launch_description_sources import AnyLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    """Launch MuJoCo simulation with domain bridge for multi-robot support."""
+
+    package_share = get_package_share_directory("bitbots_mujoco_sim")
+    bridge_config = Path(package_share) / "config" / "domain_bridges" / "multi_robot_bridge.yaml"
+    bringup_share = get_package_share_directory("bitbots_bringup")
+
+    # Motion standalone arguments
+    motion_arg = DeclareLaunchArgument("motion", default_value="false", description="Launch motion standalone stack")
+
+    return LaunchDescription(
+        [
+            motion_arg,
+            Node(
+                package="bitbots_mujoco_sim",
+                executable="sim",
+                name="sim_interface",
+                output="screen",
+                emulate_tty=True,
+            ),
+            # IMU complementary filter node and motion stack (delayed 5s, only when motion is enabled)
+            TimerAction(
+                period=5.0,
+                actions=[
+                    Node(
+                        package="imu_complementary_filter",
+                        executable="complementary_filter_node",
+                        name="complementary_filter_gain_node",
+                        output="screen",
+                        parameters=[{"use_sim_time": True}],
+                    ),
+                    IncludeLaunchDescription(
+                        AnyLaunchDescriptionSource(
+                            PathJoinSubstitution([bringup_share, "launch", "motion_standalone.launch"])
+                        ),
+                        launch_arguments={"sim": "true"}.items(),
+                    ),
+                ],
+                condition=IfCondition(LaunchConfiguration("motion")),
+            ),
+            TimerAction(
+                period=2.0,
+                actions=[
+                    LogInfo(msg=f"Starting domain bridge with config: {bridge_config}"),
+                    Node(
+                        package="domain_bridge",
+                        executable="domain_bridge",
+                        name="domain_bridge",
+                        arguments=[str(bridge_config)],
+                        output="screen",
+                        emulate_tty=True,
+                    ),
+                ],
+            ),
+        ]
+    )

bitbots_misc/bitbots_bringup/launch/simulator_teamplayer.launch

@@ -10,10 +10,18 @@
     <arg name="teamcom" default="false" description="Whether the team communication system should be started" />
     <arg name="vision" default="true" description="Whether the vision system should be started" />
     <arg name="world_model" default="true" description="Whether the world model should be started"/>
+    <arg name="mujoco" default="false" description="Whether to use the Mujoco simulator instead of Webots"/>
 
     <!-- load the general simulator -->
-    <include file="$(find-pkg-share bitbots_webots_sim)/launch/simulation.launch" />
-
+    <group if="$(var mujoco)">
+        <node pkg="bitbots_mujoco_sim" exec="sim" name="mujoco_simulation_node" output="screen"/>
+        <include file="$(find-pkg-share bitbots_webots_sim)/launch/imu_filter_sim.launch">
+            <arg name="sim" value="true"/>
+        </include>
+    </group>
+    <group unless="$(var mujoco)">
+        <include file="$(find-pkg-share bitbots_webots_sim)/launch/simulation.launch" />
+    </group>
     <!-- load teamplayer software stack -->
     <include file="$(find-pkg-share bitbots_bringup)/launch/teamplayer.launch">
         <arg name="behavior" value="$(var behavior)" />

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/domain_bridge_generator.py

@@ -0,0 +1,73 @@
+import os
+from pathlib import Path
+
+import yaml
+
+
+class DomainBridgeConfigGenerator:
+    """Generates ROS 2 domain bridge configuration for all robots."""
+
+    def __init__(self, robots: list):
+        self.main_domain = int(os.getenv("ROS_DOMAIN_ID", "0"))
+        self.robots = robots
+
+    def generate_config_file(self, output_dir: Path) -> Path:
+        """Generate a single config file for all robots with bidirectional bridging."""
+        output_dir.mkdir(parents=True, exist_ok=True)
+
+        output_path = output_dir / "multi_robot_bridge.yaml"
+        config = self.generate_config()
+        with open(output_path, "w") as f:
+            yaml.dump(config, f, default_flow_style=False, sort_keys=False)
+
+        return output_path
+
+    def generate_config(self) -> dict:
+        """Generate unified configuration for all robots."""
+        config = {
+            "name": "multi_robot_bridge",
+            "from_domain": self.main_domain,
+            "to_domain": self.main_domain,  # Default, overridden per-topic
+            "topics": {},
+        }
+
+        # Clock is shared - add it once for the first robot domain
+        # (only bridges to one domain, but multiple robots can subscribe in that domain)
+        if self.robots:
+            config["topics"]["clock"] = {
+                "type": "rosgraph_msgs/msg/Clock",
+                "to_domain": self.robots[0].domain,
+            }
+
+        for robot in self.robots:
+            # Sensor topics: main → robot (with remap to remove namespace)
+            sensor_topics = [
+                ("joint_states", "sensor_msgs/msg/JointState"),
+                ("imu/data_raw", "sensor_msgs/msg/Imu"),
+                ("camera/image_proc", "sensor_msgs/msg/Image"),
+                ("camera/camera_info", "sensor_msgs/msg/CameraInfo"),
+                ("foot_pressure_left/raw", "bitbots_msgs/msg/FootPressure"),
+                ("foot_pressure_right/raw", "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:
+                # YAML key is the SOURCE topic (what we subscribe to in main domain)
+                src_topic = f"{robot.namespace}/{topic_suffix}"
+                config["topics"][src_topic] = {
+                    "type": msg_type,
+                    "to_domain": robot.domain,
+                    "remap": topic_suffix,  # Remove namespace in robot domain
+                }
+
+            # Command topic: robot → main
+            # Subscribe to "DynamixelController/command" in robot domain, publish to "robot{N}/DynamixelController/command" in main
+            config["topics"][f"DynamixelController/command_{robot.namespace}"] = {
+                "type": "bitbots_msgs/msg/JointCommand",
+                "from_domain": robot.domain,
+                "to_domain": self.main_domain,
+                "remap": f"{robot.namespace}/DynamixelController/command",
+            }
+
+        return config

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]

bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/main.py

@@ -0,0 +1,18 @@
+import threading
+
+import rclpy
+from rclpy.experimental.events_executor import EventsExecutor
+
+from bitbots_mujoco_sim.simulation import Simulation
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    simulation = Simulation()
+    executor = EventsExecutor()
+    executor.add_node(simulation)
+    thread = threading.Thread(target=executor.spin, daemon=True)
+    thread.start()
+    simulation.run()
+    simulation.destroy_node()
+    rclpy.shutdown()