Writing a module¶
This chapter writes a small module, a wheel odometer for a differential-drive robot, and runs it
next to the MuJoCo physics on the sample AMR. The complete code is in
tests/tutorial/ (package lagrange_tutorial). Its tests keep this
page true.
Read Concepts first for signals, units and the scheduler.
Modules and plugins. This chapter compiles the module into its own program with the C++ interface. To run a module inside
lagrange_server, build it against the versioned C ABI (core/plugin_abi/include/lagrange_plugin.h) as a shared library and load it withlagrange_server --plugin LIB: Writing a plugin.
1. The interface¶
A module implements lagrange::IModule (core/module/include/lagrange/module/IModule.hpp):
Method |
When |
Do |
|---|---|---|
|
any time |
domain, name, version, whether it is a substitute, deterministic, minimum tier |
|
before wiring |
its step size; a profile’s |
|
once, before wiring |
look at the world; declare every input and output |
|
once, after wiring succeeded |
allocate state; write initial outputs |
|
every step |
read inputs, compute, write outputs. No allocation, no I/O, no lookups by name |
|
on save, reset, replay |
the complete dynamic state, for deterministic runs |
Modules never throw. They return a Status, and the scheduler reports failures with the
module’s name.
2. The odometer¶
The odometer reads the two wheel speeds and integrates them:
distance travelled:
d += |v_left + v_right| / 2 · dtheading:
θ += (v_right − v_left) / track · dt
Here v = ω · r is each wheel’s rim speed. It has two inputs and two outputs:
Signal |
Direction |
Unit |
|---|---|---|
|
in, written by MuJoCo every 1 ms |
rad/s |
|
out |
m |
|
out |
rad |
The header (Odometer.hpp) holds
the settings (joint names, wheel radius, track width), the four SignalIds and the state:
class Odometer final : public IModule {
public:
struct Settings {
std::string leftJoint{"left_wheel_joint"};
std::string rightJoint{"right_wheel_joint"};
double wheelRadiusM{0.10};
double trackWidthM{0.46};
};
// ... the IModule methods ...
private:
Settings settings_;
SignalId left_, right_, distance_, heading_;
double distanceM_{0.0};
double headingRad_{0.0};
};
Declare¶
Declaring is where units and the timing of inputs are set
(Odometer.cpp):
Status Odometer::declare(const WorldView& world, const ModuleConfig&, SignalDeclarer& declarer) {
// Fail early, with a reason, when the robot has no such joints.
const auto& joints = world.joints().names;
for (const std::string& name : {settings_.leftJoint, settings_.rightJoint}) {
if (std::find(joints.begin(), joints.end(), name) == joints.end()) {
return Status::error(StatusCode::InvalidConfig, "odometer: no joint " + name);
}
}
const auto input = [&](const std::string& joint) {
SignalSpec spec;
spec.name = "joint/" + joint + "/velocity";
spec.unit = units::rad_per_s; // checked against the writer's unit at wiring
spec.hold = Hold::WindowAverage;
return declarer.input(spec);
};
left_ = input(settings_.leftJoint);
right_ = input(settings_.rightJoint);
SignalSpec distance;
distance.name = "base/odometer/distance";
distance.unit = units::m;
distance_ = declarer.output(distance);
// ... heading, in units::rad ...
return {};
}
The odometer steps every 10 ms (preferredStep() returns 10_ms), and physics writes the wheel
speeds every 1 ms. Hold::WindowAverage hands the odometer the mean of the ten values committed
since its previous step, so the integral misses nothing in between. The default,
Hold::ZeroOrder, would give only the latest value.
Step¶
Status Odometer::step(const StepContext& context, SignalBus& bus) {
const double left = bus.read(left_) * settings_.wheelRadiusM;
const double right = bus.read(right_) * settings_.wheelRadiusM;
const double dt = context.dt.seconds();
distanceM_ += std::abs(left + right) / 2.0 * dt;
headingRad_ += (right - left) / settings_.trackWidthM * dt;
bus.write(distance_, distanceM_);
bus.write(heading_, headingRad_);
return {};
}
saveState writes distanceM_ and headingRad_; restoreState reads them back. With that,
the module is deterministic, and info() says so.
3. Run it¶
odometer_demo.cpp builds a run the way
lagrange_server does:
auto robot = server::loadUrdf(asset("robots/amr_diffdrive/amr_diffdrive.urdf"));
server::addUrdf(*robot, asset("worlds/warehouse_small/warehouse_small.urdf"));
Scheduler scheduler(SchedulerOptions{.deterministic = true});
scheduler.add(std::make_unique<rigid_body::MujocoBackend>());
scheduler.add(std::make_unique<server::DriveCommand>(server::DriveCommand::Settings{}, nullptr));
scheduler.add(std::make_unique<server::DiffDriveController>(drive));
scheduler.add(std::make_unique<tutorial::Odometer>(tutorial::Odometer::Settings{}));
auto wired = scheduler.wire(WorldView(robot->world)); // names, units, order
scheduler.stepUntil(SimTime{10'000'000'000}); // 10 s
DriveCommand without an external source plays the demo figure-eight. Build and run:
$ colcon build --packages-up-to lagrange_tutorial && source install/setup.bash
$ ros2 run lagrange_tutorial odometer_demo 10
t (s) odom (m) odom (rad) true (rad)
1 0.142 0.061 0.061
2 0.590 0.397 0.395
3 1.195 0.900 0.887
4 1.795 1.400 1.378
5 2.395 1.900 1.868
6 2.995 2.399 2.357
7 3.595 2.899 2.846
8 4.194 -2.884 -2.946
9 4.795 -2.384 -2.457
10 5.395 -1.883 -1.966
The last column is the true heading change from MuJoCo’s pose of base_link. The odometer’s
heading runs about 2 % ahead of it: the simulated robot turns a little less than ideal
kinematics on the nominal 0.46 m track predict. A real robot shows the same kind of
systematic error, and real odometers are calibrated for it. The distance agrees with the true
path length within 2 % (test_odometer checks both).
4. Get a unit wrong¶
Suppose another module reads the wheel speed as a linear velocity:
spec.name = "joint/left_wheel_joint/velocity";
spec.unit = units::m_per_s; // the writer says rad/s
The run does not start:
$ ros2 run lagrange_tutorial odometer_demo --wrong-unit
the simulation could not start:
UnitMismatch joint/left_wheel_joint/velocity: writer rigid_body/mujoco: rad/s (AngularVelocity); reader sensors/wrong_unit: m/s (LinearVelocity)
The scheduler never converts. Fix the declaration, or convert in your module with a factor you name and document.
5. Test it¶
tests/tutorial/test/test_odometer.cpp shows the pattern for a module test: wire the module
with the real physics on the sample robot, run, and compare with ground truth from the bus. It
also checks the wiring errors (a wrong unit, a missing joint).
colcon test --packages-select lagrange_tutorial && colcon test-result --verbose
Checklist¶
Every input and output has a unit. Use
units::constants; never a bare number.Choose each input’s hold:
ZeroOrderfor states,WindowAveragefor quantities you integrate.A loop between two modules needs one input marked
feedback = true.step()allocates nothing and looks nothing up by name; keep theSignalIds fromdeclare().Name physical constants and give their units and source.
If
info()says deterministic, save and restore the complete state, and uselagrange::random(Philox, seeded byStepContext::seed) for noise.A new physical model ships with a test against a reference, as here against MuJoCo’s ground truth.