How-to guides › Build robots

How-to guides

Robots: ROS 2, Gazebo, MuJoCo and plants

archec can turn a robot's model into running software and a simulated world. The software runs as ROS 2 nodes. The world runs in Gazebo or MuJoCo. A plant is the physical thing being controlled (an arm, a cart, a drone); the simulation runs it beside the model's controllers.

A first robot#

One arm link, hinged at 1.5 m above the ground:

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
  }
}
Diagram: Arm
What archec draws from this code: Arm
Try it

Generate its MuJoCo world:

archec gen main.arche --target mujoco -o out
  out/mujoco/arm.xml
  out/mujoco/plant_bridge.py

arm.xml is the MJCF, MuJoCo's model file:

<mujoco model="Arm">
  <compiler angle="radian" autolimits="true"/>
  <option timestep="0.001"/>
  <worldbody>
    <light pos="0 0 4" dir="0 0 -1"/>
    <!-- upper: Link -->
    <body name="upper" pos="0 0 1.5">
      <joint name="upper" type="hinge" axis="0 1 0" range="-2 2"/>
      <geom type="capsule" size="0.04 0.25" pos="0 0 -0.25" mass="1"/>
    </body>
  </worldbody>
  <keyframe>
    <key name="initial" qpos="0.3"/>
  </keyframe>
</mujoco>

plant_bridge.py runs it. It needs Python with the mujoco package. The MuJoCo section says what each word becomes, and Plants has four robots ready to run.

ROS 2#

A deployment that says packaging ros2 runs its Linux binaries as ROS 2 nodes:

archec gen main.arche --target ros2 --deployment VehicleRos2

It writes two things:

  • ros2/: the messages, as the interface package <project>_msgs;
  • ros2_nodes/: an ament_cmake package per binary on a Linux processor. It has an rclcpp executable per process (or one for the binary), and a node per part that the binary runs.

Each node wraps the part's C module (--target c: its state machine and functions). It gives the part:

  • 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. A message fires the state machine, calls the port's handler with the message's fields of the same names, then calls its on-receive.
  • a service per operation of an interface the port provides, answered by the function of the same name. A port that requires an interface gets a client.
  • a timer on ROS time that calls a periodic thread's step. With use_sim_time, ROS time is the simulator's clock.
  • parameters, from the props of the definition and of the part.

Threads. Each model thread is a std::thread with its own executor. On Linux it gets its policy, priority and core.

Topics. A topic is <part>/<port>. When the port sends several messages, /<message> follows. A topic on the link or the port replaces this name.

QoS (ROS 2's quality of service) comes from the link and the receiving port:

  • buffer sample keeps 1 message; depth n keeps n.
  • reliability carries over.
  • The message's period becomes the QoS deadline on both ends, so they stay compatible.
  • The link's deadline or stale-after becomes the lifespan.

Launch. ros2_nodes/<deployment>_bringup launches every executable in the deployment's namespace.

Your code. It goes in the nodes' USER CODE regions, which archec gen keeps (regeneration). The regions are: the includes, the end of each node's constructor, its members, main before the threads start, and the CMakeLists.

Binaries on an RTOS stay firmware. The generator lists the topics that need a bridge (CAN), and those that no node publishes (sensors, or the simulator).

Gazebo#

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

  • worlds/<system>.sdf: a skeleton that you finish. It is written once.
    • It has a world: physics at the parts' solver step, a ground and a sun.
    • It has 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. Its pose is a TODO.
    • A sensor gets the Gazebo sensor it is, where one exists: a gpu_lidar (its channels prop gives the rings), a camera, an imu or a navsat. It runs at the rate of what it sends.
  • config/bridge.yaml, for ros_gz_bridge. It bridges /clock, and each sensor to its standard ROS type (PointCloud2, Image, Imu, NavSatFix). Each float field of a command goes 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.
    • It leaves a TODO for the rest.
    • It also passes the nodes' commands on to Gazebo.
  • launch/<system>_sim.launch.py. It starts gz sim with the world, the bridge, the adapter, and the deployment's launch file with use_sim_time.

MuJoCo#

archec gen main.arche --target mujoco puts the plant of a system in MuJoCo. It reads what the parts say about their bodies, as in A first robot.

shape, inside physical { }:

  • box x, y, z (full sizes), sphere r, capsule r, length or cylinder r, length (along z);
  • at x, y, z: where its centre is, in the part's frame;
  • rot: how it is turned there. rot y 90deg gives a capsule along x.

joint, on a part: how the part moves in the part it is in. For a top part, that is the world.

  • The kind: 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 frame.
  • rot: how it is turned there. Write roll, pitch, yaw (rot 0, 0, 90deg) or one axis (rot z 90deg). It becomes the body's quat. The joint's axis, the shape and the parts inside are in the turned frame.
  • range lo..hi: its limits, in rad or m.
  • = value: where it starts.

What is written#

The output folder mujoco/ is written again each time. It holds <system>.xml and plant_bridge.py.

<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. 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 sensor:

    • 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.

    With another name, the unit of what it sends decides: rad, rad/s, m/s², N, N·m.

  • A kind actuator part drives the hinge or slide of the body it is in.

    • It is a motor, or a servo chosen by the unit of the field that commands it: position (rad, m) or velocity (rad/s, m/s).
    • The field's range is its control range. prop kp and prop kv are its gains.
    • On a joint free body (a drone), it is a thruster: a motor on a site at its joint at. It pushes along z, or along its joint slide axis. It also 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.

  • A body part's props say how it touches and how its joint moves:

    • prop friction = 1.2: its geom's sliding friction. A model with one uses elliptic friction cones and impratio="10", as MuJoCo advises for grasps.
    • prop damping = 0.02, prop armature = 0.002 (the motor's inertia seen at the joint), prop frictionloss = 0.05 (dry friction, N·m or N): its joint's.
    • prop couple = {joint: cap, ratio: 0.0005}: an equality. This joint's position is ratio × that joint's: a screw thread, a cap that rises 0.5 mm per rad it turns. That joint is named by its path, or from this part. A ratio below 1 is written the other way round, so MuJoCo holds it stiffly.

plant_bridge.py runs the plant. A command message sets its actuator's data.ctrl from the time it arrives. Each sensor's message is filled from data.sensordata at the message's period: its float fields in order, and the time in a stamp field. It runs in one of four ways:

  • --stdio: in 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. What it sent to the actuators goes back.
    • MUJOCO_PYTHON names the Python that has mujoco. Without it, python3 is used.
  • --csv out.csv [--control seq.csv] [--for 2s]: a fixed control sequence in, the readings out, in CSV. The control file has a row per time in seconds and a column per actuator.
  • --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 is published on its port's topic (cart/encoder/position, or the link's or port's topic), at its period. That is also its QoS deadline, as the nodes ask.
    • Each actuator's command is 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 follows 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

Checks#

archec check reports what does not read (body). In a system whose parts say their shape or joint, it also reports 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.

Sensors, actuators and keyframes#

How the sensors read and the actuators act is in the model (spec). In the MJCF:

  • a sensor's noise is its σ. The bridge draws the noise from --seed, with the same generator as archec sim.
  • its field's range is MuJoCo's cutoff; its port's latency is delay.
  • an actuator's command range is ctrlrange, its force-range is forcerange, its latency is delay.
  • an actuator with a gain or a lag is a <general> actuator (gainprm, dyntype filterexact).

Each keyframe of the system is a <key> (see keyframes). It holds the positions of its hinges, slides and balls, the places and turns of its free bodies, and their velocities (qpos, qvel). plant_bridge.py --keyframe K starts from it, and so does archec sim --plant … --keyframe K.

Plants#

Four plants are ready to run. Each is a project to start from:

archec new my-arm --template two-link-arm

In a store, they are the collection plants. Each one has:

  • 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
Template The plant The controller Measured in MuJoCo 4.0
two-link-arm two hinged links, a torque motor and an encoder at each gravity compensation and PD within 0.02 rad of its pose at 0.78 s
cart-pole a cart on a rail, a pole with a bob, 0.1 rad off upright LQR, its gain from archec physics --linearize --lqr the pole within 0.1 rad, upright within 0.02 rad at 1.08 s
diff-drive-rover a free chassis, two wheels, a caster, encoders, IMU, GPS odometry, heading and speed loops through three waypoints at the last at 10.4 s, 0.073 m off at most
quadrotor a free body, four thrusters with their drag torque, IMU, attitude, position place, attitude and altitude loops, the mixer a 0.5 m climb within 0.05 m in 0.4 s

examples/cart-pole-mpc balances the same cart-pole with a model predictive controller (mpc.ts).

  • archec physics --linearize --lqr gives the linear cart-pole (A, B), and the LQR's cost to go P, used as the end of the horizon.
  • Every 5 ms, the controller plans the forces of the next 0.1 s, within ±3 N, and sends the first one.
  • Its test plan passes against MuJoCo. With only a force limit, a clamped LQR does about as well there.

Limits#

  • ROS 2 QoS has no latency budget, so a link's latency is not carried to the nodes.
  • Binaries on an RTOS are not ROS 2 nodes. The topics they need bridged (CAN) are only listed.
  • In Gazebo, shapes, poses, joints and the vehicle's dynamics are not taken from the model: they stay yours to write in the world file.
  • A sensor that Gazebo does not simulate (a radar) is only 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.
  • In MuJoCo, the ground and the colours are not in the model. Of contacts, only a geom's sliding friction is (prop friction): every geom touches every other but its parent's, as MuJoCo does by default.