Robots: ROS 2, Gazebo, MuJoCo and plants

A robot's software as ROS 2 nodes, its world in Gazebo or MuJoCo, and the plants the simulation runs beside the model.

ROS 2#

A deployment that says packaging ros2 runs its Linux binaries as ROS 2 nodes: archec gen main.arche --target ros2 --deployment VehicleRos2 writes the messages as the interface package ros2/ (<project>_msgs) and, in ros2_nodes/, an ament_cmake package per binary on a Linux processor, with an rclcpp executable per process (or one for the binary) and a node per part it runs. The node wraps the part's C module (--target c: its state machine and functions) and gives it:

• a publisher per port and message it sends; its C send function converts the message and publishes it;
• a subscription per link that comes to it, which fires the state machine, calls the port's handler with the message's fields of the same names, then its on-receive;
• a service per operation of an interface the port provides, answered by the function of the same name, and a client for one it requires;
• a timer on ROS time (the simulator's clock with use_sim_time) that calls a periodic thread's step, and parameters from the definition's and the part's props.

Each model thread is a std::thread with its own executor, with its policy, priority and core on Linux. A topic is <part>/<port>, with /<message> after it when the port sends several, unless the link or the port gives a topic. The QoS comes from the link and the receiving port: buffer sample keeps 1, depth n keeps n, and reliability carries over. The message's period becomes the QoS deadline on both ends, so they stay compatible, and the link's deadline or stale-after becomes the lifespan. ROS 2 QoS has no latency budget, so latency is not carried. ros2_nodes/<deployment>_bringup launches every executable in the deployment's namespace. Your code in the nodes goes in their USER CODE regions (the includes, the end of each node's constructor, its members, main before the threads start, the CMakeLists), which archec gen keeps (regeneration). Binaries on an RTOS stay firmware: the generator lists the topics that need a bridge (CAN) and those no node publishes (sensors, or the simulator).

Gazebo#

archec gen main.arche --target gazebo puts the plant of that deployment in Gazebo (gz sim, Harmonic), so its nodes run against a simulated vehicle. The output folder gazebo/ is an ament package that holds:

• worlds/<system>.sdf, a skeleton you finish (written once). It has a world (physics at the parts' solver step, a ground, a sun) and a model of the system: a base_link and a link per part that is a sensor, an actuator the nodes command, or has a mass. Each link is named after its part and fixed to the base, with its pose a TODO. A sensor gets the Gazebo sensor it is, where one exists: a gpu_lidar (its channels prop as rings), a camera, an imu or a navsat, at the rate of what it sends.
• config/bridge.yaml for ros_gz_bridge: /clock, each sensor to its standard ROS type (PointCloud2, Image, Imu, NavSatFix), and each float field of a command to a gz.msgs.Double topic of its link, for a plugin to apply.
• scripts/sim_adapter.py (rclpy, written once). It turns Gazebo's messages into the model's on the topics the nodes subscribe to, with the deadline they ask for. It fills what it can (stamps, sequence numbers, image and cloud sizes, the nearest lidar return, latitude and longitude) and leaves a TODO for the rest. It also passes the nodes' commands on to Gazebo.
• launch/<system>_sim.launch.py, which starts gz sim with the world, the bridge, the adapter and the deployment's launch file with use_sim_time.

Shapes, poses, joints and the vehicle's dynamics are not in the model and stay yours. A sensor Gazebo does not simulate (a radar) is a note. CARLA is not generated: its ROS 2 bridge publishes the same standard types, so the adapter can take its topics instead of Gazebo's.

MuJoCo#

archec gen main.arche --target mujoco puts the plant of a system in MuJoCo, from what its parts say about their bodies:

component Link "Arm link" {
  physical {
    mass 1kg
    shape capsule 0.04m, 0.5m at 0, 0, -0.25m
  }
}

system Arm {
  part upper: Link {
    joint hinge y at 0, 0, 1.5m range -2..2 = 0.3
  }
}

• shape: box x, y, z (full sizes), sphere r, capsule or cylinder r, length (along z), and at x, y, z, where its centre is in the part's frame, rot, how it is turned there (rot y 90deg: a capsule along x).
• joint: how the part moves in the part it is in (the world for a top part): hinge or slide along x, y, z (or -x, or [x, y, z]), ball, free, or fixed (the default); at x, y, z, where its frame is in its parent's, rot, how it is turned there (roll, pitch, yaw rot 0, 0, 90deg, or rot z 90deg: the body's quat; its axis, shape and parts are in the turned frame); range lo..hi, its limits (rad, or m); = value, where it starts.

The output folder mujoco/ holds, written again each time:

• <system>.xml, the MJCF. A part with a mass or a shape is a body, inside the nearest body around it; its geom has the part's mass (MuJoCo derives the inertia), and a part with a mass and no shape is a 0.1 m box. A kind sensor part is a sensor of the body it is in, on a site at its joint at: its name says which (imu: an accelerometer and a gyro; encoder or angle: jointpos; velocity: jointvel; force, torque, touch, range, gps; attitude or ahrs: its orientation, a quaternion w, x, y, z) or else the unit of what it sends (rad, rad/s, m/s², N, N·m). A kind actuator part drives the hinge or slide of the body it is in: a motor, or a position (rad, m) or velocity (rad/s, m/s) servo from the unit of the field that commands it, with the field's range as its control range and prop kp/prop kv as its gains. On a joint free body (a drone) it is a thruster: a motor on a site at its joint at that pushes along z (or its joint slide axis), and turns the body back about that axis by prop reaction N·m per N (a propeller's drag). The timestep is the parts' solver step (1 ms by default); a keyframe holds the joints' starting values.
• plant_bridge.py, which runs it. A command message sets its actuator's data.ctrl from its arrival; each sensor's message is filled from data.sensordata (its float fields in order; a stamp field gets the time) at the message's period.
  • --stdio: lockstep with archec sim --plant mujoco/plant_bridge.py, which runs the rest of the model (its controllers) against the plant. The protocol is JSON lines with the simulator's own send events: every period, the plant's messages go to the parts linked to the sensors, the simulator runs a period, and what it sent to the actuators goes back. MUJOCO_PYTHON names the Python that has mujoco (else python3).
  • --csv out.csv [--control seq.csv] [--for 2s]: a fixed control sequence (a row per time in seconds, a column per actuator) and the readings, in CSV.
  • --viewer: MuJoCo's viewer, when there is one (on macOS, run it with mjpython).
  • --ros2 [--namespace NS] [--rate 1] [--for T]: a ROS 2 node (rclpy), for the nodes of archec gen --target ros2: each sensor's message published on its port's topic (cart/encoder/position, or the link's or port's topic) at its period (its QoS deadline, as the nodes ask), each actuator's command taken from its sender's topic as it comes, with the types of <project>_msgs; /clock is the plant's time, for the nodes' use_sim_time. The namespace is the deployment's (packaging ros2); --rate 0 runs as fast as it can, --rate 1 with the wall clock. Without --for it runs until stopped. It needs a sourced ROS 2 (rclpy, rosgraph_msgs) and the built <project>_msgs.

$ archec gen examples/arm/main.arche --target mujoco -o out
$ archec sim examples/arm/main.arche --for 3s --plant out/mujoco/plant_bridge.py

archec check says what does not read (body) and, in a system whose parts say their shape or joint, what MuJoCo cannot place (body-unmapped): a mass with no shape, a sensor it has no model of, an actuator with no joint to drive. The ground, colours and contacts beyond MuJoCo's defaults are not in the model.

How the sensors read and the actuators act is the model's (spec): a sensor's noise (its σ in the MJCF; the bridge draws it from --seed with the generator archec sim uses), its field's range (MuJoCo's cutoff) and its port's latency (delay); an actuator's command range (ctrlrange), force-range (forcerange), latency (delay), and with a gain or a lag a <general> actuator (gainprm, dyntype filterexact). Each keyframe of the system is a <key> — its hinges', slides' and balls' positions, its free bodies' places and turns, their velocities (qpos, qvel: see keyframes) — and plant_bridge.py --keyframe K (or archec sim --plant … --keyframe K) starts from it.

Plants#

Four plants ready to run, each a project to start from (archec new my-arm --template two-link-arm) and, in a store, the collection plants: a plant in MuJoCo (its parts' mass, shape and joint, its sensors and actuators), a controller in the action language, the system that wires them, requirements (with an EARS sentence or a formal property), a test plan and a card. The test plan runs against the plant:

archec gen main.arche --target mujoco -o out
archec sim main.arche --plant out/mujoco/plant_bridge.py --test

examples/cart-pole-mpc balances the same cart-pole with a model predictive controller (mpc.ts): every 5 ms, the forces of the next 0.1 s planned within ±3 N on the cart-pole that archec physics --linearize --lqr gives (A, B, and the LQR's cost to go P as the horizon's end), the first one sent. Its test plan passes against MuJoCo; with only a force limit, a clamped LQR does about as well there.