Implement Mujoco Simulator

bitbots_misc/bitbots_bringup/launch/simulator_teamplayer.launch

@@ -10,10 +10,15 @@
     <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"/>
+    </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_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]

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()

bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/robot.py

@@ -0,0 +1,83 @@
+import mujoco
+
+from bitbots_mujoco_sim.camera import Camera
+from bitbots_mujoco_sim.joint import Joint
+from bitbots_mujoco_sim.sensor import Sensor
+
+
+class Robot:
+    """Represents the robot, holding all its components like the camera, joints, and sensors."""
+
+    def __init__(self, model: mujoco.MjModel, data: mujoco.MjData):
+        self.camera = Camera(model, data, name="head_camera")
+        self.joints = RobotJoints(
+            [
+                # --- Right Leg ---
+                Joint(model, data, ros_name="RHipYaw"),
+                Joint(model, data, ros_name="RHipRoll"),
+                Joint(model, data, ros_name="RHipPitch"),
+                Joint(model, data, ros_name="RKnee"),
+                Joint(model, data, ros_name="RAnklePitch"),
+                Joint(model, data, ros_name="RAnkleRoll"),
+                # --- Left Leg ---
+                Joint(model, data, ros_name="LHipYaw"),
+                Joint(model, data, ros_name="LHipRoll"),
+                Joint(model, data, ros_name="LHipPitch"),
+                Joint(model, data, ros_name="LKnee"),
+                Joint(model, data, ros_name="LAnklePitch"),
+                Joint(model, data, ros_name="LAnkleRoll"),
+                # --- Arms ---
+                Joint(model, data, ros_name="RShoulderPitch"),
+                Joint(model, data, ros_name="RShoulderRoll"),
+                Joint(model, data, ros_name="RElbow"),
+                Joint(model, data, ros_name="LShoulderPitch"),
+                Joint(model, data, ros_name="LShoulderRoll"),
+                Joint(model, data, ros_name="LElbow"),
+                # --- Head ---
+                Joint(model, data, ros_name="HeadPan"),
+                Joint(model, data, ros_name="HeadTilt"),
+            ]
+        )
+        self.sensors = RobotSensors(
+            [
+                # IMU Sensors
+                Sensor(model, data, name="gyro", ros_name="IMU_gyro"),
+                Sensor(model, data, name="accelerometer", ros_name="IMU_accelerometer"),
+                Sensor(
+                    model,
+                    data,
+                    name="orientation",
+                    ros_name="IMU_orientation",
+                ),  # Global orientation quaternion
+                Sensor(model, data, name="position", ros_name="IMU_position"),  # Global position vector
+                # Foot Sensors
+                Sensor(model, data, name="l_foot_pos", ros_name="left_foot_position"),
+                Sensor(model, data, name="r_foot_pos", ros_name="right_foot_position"),
+                Sensor(model, data, name="l_foot_global_linvel", ros_name="left_foot_velocity"),
+                Sensor(model, data, name="r_foot_global_linvel", ros_name="right_foot_velocity"),
+            ]
+        )
+
+
+class RobotSensors(list[Sensor]):
+    """A list of Robot Sensors with additional helper methods."""
+
+    def get(self, name: str) -> Joint:
+        """Finds and returns a specific joint by its ROS name."""
+        return next(filter(lambda joint: joint.ros_name == name, self))
+
+    @property
+    def gyro(self) -> Sensor:
+        return self.get("IMU_gyro")
+
+    @property
+    def accelerometer(self) -> Sensor:
+        return self.get("IMU_accelerometer")
+
+
+class RobotJoints(list[Joint]):
+    """A list of Robot Joints with additional helper methods."""
+
+    def get(self, name: str) -> Joint:
+        """Finds and returns a specific joint by its ROS name."""
+        return next(filter(lambda joint: joint.ros_name == name, self))

bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/sensor.py

@@ -0,0 +1,27 @@
+import mujoco
+import numpy as np
+
+
+class Sensor:
+    """Represents a single sensor, providing a clean interface to its value."""
+
+    def __init__(self, model: mujoco.MjModel, data: mujoco.MjData, name: str, ros_name: str):
+        self._data = data
+        self.ros_name: str = ros_name
+        self.instance: int = model.sensor(name)
+        self.id: int = self.instance.id
+
+    @property
+    def data(self) -> np.ndarray:
+        """Gets the current sensor reading from sensordata array."""
+        return self._data.sensordata[self.adr : self.adr + self.dim]
+
+    @property
+    def adr(self) -> int:
+        """Gets the address of the sensor data in sensordata array."""
+        return int(self.instance.adr)
+
+    @property
+    def dim(self) -> int:
+        """Gets the dimension of the sensor data."""
+        return int(self.instance.dim)