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
}
}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
handlerwith the message's fields of the same names, then calls itson-receive. - a service per operation of an interface the port
provides, answered by the function of the same name. A port thatrequiresan interface gets a client. - a timer on ROS time that calls a periodic thread's
step. Withuse_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 samplekeeps 1 message;depth nkeeps n.reliabilitycarries over.- The message's
periodbecomes the QoS deadline on both ends, so they stay compatible. - The link's
deadlineorstale-afterbecomes 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'
solverstep, 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 amass. - 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(itschannelsprop gives the rings), acamera, animuor anavsat. It runs at the rate of what it sends.
- It has a world: physics at the parts'
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 agz.msgs.Doubletopic 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 withuse_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, lengthorcylinder 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 90deggives 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:
hingeorslidealongx,y,z(or-x, or[x, y, z]),ball,free, orfixed(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'squat. 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
massor ashapeis 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 sensorpart is a sensor of the body it is in, on a site at itsjoint 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 actuatorpart 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) orvelocity(rad/s, m/s). - The field's
rangeis its control range.prop kpandprop kvare its gains. - On a
joint freebody (a drone), it is a thruster: a motor on a site at itsjoint at. It pushes along z, or along itsjoint slideaxis. It also turns the body back about that axis, byprop reactionN·m per N (a propeller's drag).
- It is a
The timestep is the parts'
solverstep, 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 andimpratio="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 witharchec 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
sendevents. - 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_PYTHONnames the Python that hasmujoco. Without it,python3is used.
- The protocol is JSON lines, with the simulator's own
--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 withmjpython.--ros2 [--namespace NS] [--rate 1] [--for T]: a ROS 2 node (rclpy), for the nodes ofarchec gen --target ros2.- Each sensor's message is published on its port's topic
(
cart/encoder/position, or the link's or port'stopic), at itsperiod. 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. /clockis the plant's time, for the nodes'use_sim_time.- The namespace is the deployment's (
packaging ros2). --rate 0runs as fast as it can;--rate 1follows the wall clock.- Without
--for, it runs until stopped. - It needs a sourced ROS 2 (rclpy, rosgraph_msgs) and the built
<project>_msgs.
- Each sensor's message is published on its port's topic
(
$ 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
noiseis its σ. The bridge draws the noise from--seed, with the same generator asarchec sim. - its field's
rangeis MuJoCo'scutoff; its port'slatencyisdelay. - an actuator's command range is
ctrlrange, itsforce-rangeisforcerange, itslatencyisdelay. - an actuator with a
gainor alagis 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,shapeandjoint, its sensors and actuators; - a controller in the action language;
- the system that wires them;
- requirements, with an EARS sentence or a
formalproperty; - 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 --lqrgives 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
latencyis 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.