C++ Tutorial

This tutorial builds a C++ simulation application in layers: compile a robot scene, add KDL control, add reset hooks, add objects and cameras, use the Simulate UI, record video, and then extend the scene to more robots and more complex task assets.

The snippets load the bundled Kinova GEN3 arm and Robotiq 2F-85 gripper from assets/ through example_paths.hpp.

1. How To Read This Tutorial

The wrapper has four layers. Keep them separate and the API stays simple:

Layer	Type	Responsibility
Scene description	SceneSpec, RobotSpec, AttachmentSpec, SceneObject, CameraSpec	Describe what should be compiled into MuJoCo
Runtime environment	Env	Own mjModel/mjData, registered robots, scene slots, the viewer, and reset hooks
Robot control handle	Robot	KDL chain, measured ports, command ports
Visualization/recording	Env::viewer, VideoRecorder	Interactive Simulate UI and offscreen MP4 recording
Python package	mj_kdl_wrapper	Python wrappers for the same scene, robot, reset, viewer, and recorder concepts

The important ownership rule:

• Env owns mjModel, mjData, its scene slots (env.scene) and its viewer (env.viewer).
• Robot is registered with one Env and borrows its model/data; Env never deletes it.
• VideoRecorder owns only its EGL/rendering/ffmpeg resources; it renders an Env when recording frames.

Most examples follow this flow:

1. Build a SceneSpec.
2. Call init_env().
3. Initialize one or more Robot handles with init_robot_from_mjcf(&robot, &env, ...), which also registers them with the Env.
4. Install env.on_reset if the task has object/controller state.
5. Call open_viewer(&env), or run headless (the loop is the same).
6. In the loop: step(&env), update(&env), compute commands, write command ports.
7. Cleanup: cleanup(&env) closes the viewer and frees the model/data.

2. Start With One Robot Scene

Scenes are declared with SceneSpec. timestep, add_floor, and add_skybox have no defaults - they are choices the wrapper refuses to guess on your behalf. build_scene fails unless timestep is set and > 0. gravity_z defaults to Earth gravity (-9.81 m/s^2). Calls that can fail return mj_kdl::Status: false on failure, with the reason in error.

#include "example_paths.hpp"
#include "mj_kdl_wrapper/mj_kdl_wrapper.hpp"
 
mj_kdl::SceneSpec scene;
scene.timestep   = 0.002;   // [s]
scene.add_floor  = true;
scene.add_skybox = true;
scene.robots.push_back(mj_kdl::RobotSpec{
    .path = mj_kdl_examples::asset("kinova_gen3/gen3.xml"),
});
 
mj_kdl::Env env;
if (mj_kdl::Status s = mj_kdl::init_env(&env, &scene); !s) {
    throw std::runtime_error("failed to build scene: " + s.error);
}

Env owns env.model and env.data. Call mj_kdl::cleanup(&env) when done (the Env destructor does it too). An Env is neither copied nor moved: robots and slots point into it.

build_scene() still returns a raw model/data pair for tools that only compile or save a scene, but robots, stepping, frames and the viewer all work on an Env.

What SceneSpec Compiles

build_scene() creates one MuJoCo mjSpec, attaches robot MJCF trees into it, adds global scene options, adds objects and cameras, then compiles the model. The robot list may be empty when the scene only contains objects.

Important fields:

mj_kdl::SceneSpec scene;
scene.timestep   = 0.002;
scene.gravity_z  = -9.81;
scene.add_floor  = true;
scene.add_skybox = true;
scene.robots     = {};
scene.objects    = {};
scene.cameras    = {};

Use RobotSpec::prefix for repeated robots. The prefix is applied during MJCF attachment so names stay unique in the compiled model:

scene.robots.push_back(mj_kdl::RobotSpec{
    .path   = mj_kdl_examples::asset("kinova_gen3/gen3.xml"),
    .prefix = "left_",
    .pos    = { -0.6, 0.0, 0.0 },
});

Use RobotSpec::pos and RobotSpec::quat to place the robot root in the world. quat is [x, y, z, w] and defaults to identity { 0, 0, 0, 1 }.

Cleanup Order

One call tears everything down:

mj_kdl::cleanup(&env);      // closes the viewer, forgets the robots, frees mjData and mjModel

cleanup(&robot) unregisters a single robot while the Env keeps running; a Robot destroyed before its Env does this itself.

If you created raw mjModel* and mjData* with build_scene(), free them with mj_kdl::destroy_scene(model, data). Never do that for env.model/env.data; Env owns those pointers once init_env() succeeds.

3. Initialize A KDL Robot

Robot is the runtime handle for one controllable articulation. It holds the KDL chain, joint names and limits, measured ports, and command ports; its MuJoCo index maps are private.

mj_kdl::Robot robot;
if (mj_kdl::Status s = mj_kdl::init_robot_from_mjcf(&robot, &env, "base_link", "bracelet_link");
    !s) {
    throw std::runtime_error("failed to init robot: " + s.error);
}

init_robot_from_mjcf() registers the robot with the Env, which from then on reads, commands and resets it. Its command ports start out holding the current pose.

4. Run A Position Control Loop

For position mode, write jnt_pos_cmd; update(&env) copies it to MuJoCo actuator controls.

robot.ctrl_mode = mj_kdl::CtrlMode::POSITION;
 
mj_kdl::open_viewer(&env, "position control");
 
// step() is false only when the window closes, so the loop needs its own end.
while (env.data->time < 10.0 && mj_kdl::step(&env)) {
    mj_kdl::update(&env);
    mj_kdl::pace_realtime(&env);   // step() never sleeps; pace the loop yourself
 
    for (int i = 0; i < robot.n_joints; ++i) {
        robot.jnt_pos_cmd[i] = robot.jnt_pos_msr[i];
    }
}

open_viewer() starts MuJoCo Simulate in a render thread. Your loop still owns stepping, updating control, and task logic. update(&env) reads and commands every registered robot and scene slot; commands written after it are applied by the next update().

5. Add KDL Gravity Compensation

KDL dynamics work directly from the generated chain:

robot.ctrl_mode = mj_kdl::CtrlMode::TORQUE;
KDL::ChainDynParam dyn(robot.chain, KDL::Vector(0.0, 0.0, scene.gravity_z));
KDL::JntArray q(robot.n_joints);
KDL::JntArray g(robot.n_joints);
 
while (env.data->time < 10.0 && mj_kdl::step(&env)) {
    mj_kdl::update(&env);
    mj_kdl::pace_realtime(&env);   // step() never sleeps; pace the loop yourself
 
    for (int i = 0; i < robot.n_joints; ++i) {
        q(i) = robot.jnt_pos_msr[i];
    }
    dyn.JntToGravity(q, g);
 
    for (int i = 0; i < robot.n_joints; ++i) {
        robot.jnt_trq_cmd[i] = g(i);
    }
}

This is the core pattern used by ex_gravity_comp.

6. Attach A Gripper Or Tool

Attachments are MJCF assets attached under a body, site, or frame in the accumulated robot spec. They are applied in order, so mount -> sensor -> gripper chains are natural.

AttachTarget is a tagged pair of AttachKind { World, Body, Site, Frame } and an element name. Sites are the natural mount-point: the Kinova GEN3 MJCF ships pinch_site on the bracelet, with its own pos/quat that already encode the gripper offset and 180-degree flip. So the gripper attachment is just:

mj_kdl::AttachmentSpec gripper{
    .mjcf_path = mj_kdl_examples::asset("robotiq_2f85/2f85.xml"),
    .attach_to = { mj_kdl::AttachKind::Site, "pinch_site" },
    .prefix    = "g_",
};
 
mj_kdl::RobotSpec arm{
    .path        = mj_kdl_examples::asset("kinova_gen3/gen3.xml"),
    .attachments = { gripper },
};
 
scene.robots.clear();
scene.robots.push_back(arm);

pos and quat on the spec are still available as offsets. For body and frame parents they ride on an intermediate frame, preserving the child's authored pose. For site parents they compose with the child root's authored pose so authored values are never silently dropped.

When the model has no suitable site, fall back to a body name and add the offset by hand:

gripper.attach_to = { mj_kdl::AttachKind::Body, "bracelet_link" };
gripper.pos[2]    = -0.061525;
// 180 deg about x is exactly [x, y, z, w] = { 1, 0, 0, 0 }
gripper.quat[0]   = 1.0;
gripper.quat[3]   = 0.0;

Tell KDL about the attached tool when initializing the robot:

mj_kdl::ToolFrameSpec tool{
    .tool_body = "g_base_mount",
    .tcp_site  = "g_pinch",
};
 
mj_kdl::init_robot_from_mjcf(&robot, &env, "base_link", "bracelet_link", "", &tool);

tool_body lumps the tool subtree inertia into the KDL chain, at the pose the model is in when the robot is initialized. Name the tool's root body: the 2F-85's is g_base_mount, whose mass MuJoCo infers from its mesh; starting at g_base leaves it out and makes KDL gravity 5-8% low. tcp_site adds a terminal frame from a MuJoCo site.

Attachment Chains

attachments is ordered. Each attachment is applied to the robot spec after all previous attachments, so later attach_to values may refer to bodies introduced by earlier attachments.

mj_kdl::AttachmentSpec gripper{
    .mjcf_path = mj_kdl_examples::asset("robotiq_2f85/2f85.xml"),
    .attach_to = { mj_kdl::AttachKind::Site, "pinch_site" },
    .prefix    = "g_",
};
 
// The mug attaches to "g_base", a body the gripper attachment introduced.
mj_kdl::AttachmentSpec mug{
    .mjcf_path = mj_kdl_examples::asset("mug.xml"),
    .attach_to = { mj_kdl::AttachKind::Body, "g_base" },
    .prefix    = "mug_",
};
 
scene.robots.push_back(mj_kdl::RobotSpec{
    .path        = mj_kdl_examples::asset("kinova_gen3/gen3.xml"),
    .attachments = { gripper, mug },
});

Both assets ship with the package; ex_table_pour runs this chain (with prefix pour_) to hold the mug in the gripper.

For contact stability, add contact exclusions only when two attached bodies are known to overlap structurally:

gripper.contact_exclusions.push_back({ "bracelet_link", "g_base" });

Do not use exclusions to hide unstable task contacts; fix geometry, friction, or controller gains instead.

7. Add Tables, Objects, And Asset Sites

SceneObject supports primitive objects and MJCF-backed assets. Tables are just assets, not special first-class fields. SceneObject::attach_to accepts the same tagged AttachTarget as RobotSpec and AttachmentSpec. Inside build_scene the order is decorations -> objects (declaration order) -> robots -> cameras, so a robot or a later object may reference any prior object via its name or one of its sites.

SceneObject has no defaults for shape, size, rgba, mass, or friction. For MJCF-backed objects (when mjcf_path is set) those fields are ignored at runtime, so leaving them zero-initialised is fine. For primitives, build_scene runs explicit checks:

• shape == Shape::Unspecified -> error, the object is skipped.
• size[i] <= 0 for the relevant dimensions of the shape -> error, skipped.
• mass <= 0 on a non-fixed primitive -> error, skipped.

mj_kdl::SceneObject table{
    .name      = "table",
    .mjcf_path = mj_kdl_examples::asset("table.xml"),
    .pos       = { 0.0, 0.0, 0.7 },
    .fixed     = true,
};
 
scene.objects.push_back(table);
 
// Primitives require shape, size, rgba, mass, and friction.
scene.objects.push_back(mj_kdl::SceneObject{
    .name     = "cube",
    .shape    = mj_kdl::Shape::BOX,
    .size     = { 0.03, 0.03, 0.03 },                 // half-extents [m]
    .pos      = { 0.35, 0.05, 0.73 },                 // free cubes stay world-anchored
    .rgba     = { 0.1f, 0.3f, 1.0f, 1.0f },
    .mass     = 0.1,                                  // [kg]
    .condim   = mj_kdl::Condim::Torsional,            // friction model
    .friction = { 0.8, 0.02, 0.001 },                 // [slide, spin, roll]
});

Condim is a typed enum (Tangential = 3, Torsional = 4, Rolling = 6) matching MuJoCo's contact-dimensionality integers. Default is Tangential.

scene.robots can be empty. Object-only scenes still compile and can be opened in the Simulate UI with open_viewer(&env, "object scene").

After build_scene, an MJCF-backed SceneObject exposes its root body in the compiled scene under obj.name (the asset's internal root body name is rewritten so callers never need to know it). All other elements (sites, geoms, joints, child bodies) keep their authored names, with obj.prefix prepended when it is set. The same asset used twice needs distinct prefixes; otherwise the build fails on the repeated names.

So a site authored inside the asset is found by its own name:

KDL::Frame world_T_table_top;
mj_kdl::get_site_frame(&env, "table_top", &world_T_table_top);

Frame getters recompute the kinematics only when the state changed since they were last computed, so reading many frames per step costs one forward pass, and a direct qpos write is picked up without any extra call.

Combined, this lets a robot sit on the tabletop without hand-threading heights:

const std::string mount = "table_top";
 
scene.robots.push_back(mj_kdl::RobotSpec{
    .path      = mj_kdl_examples::asset("kinova_gen3/gen3.xml"),
    .attach_to = { mj_kdl::AttachKind::Site, mount },
});

A fixed object can also attach to the table body directly:

scene.objects.push_back(mj_kdl::SceneObject{
    .name      = "fixture",
    .mjcf_path = "fixture.xml",
    .attach_to = { mj_kdl::AttachKind::Body, "table" },
    .fixed     = true,
});

MuJoCo restricts freejoints to top-level bodies, so non-fixed primitives and asset roots with a freejoint must use AttachKind::World (the default). mj_compile reports the violation if this rule is broken.

8. Add Cameras

Add fixed scene cameras through SceneSpec::cameras. CameraSpec requires pos and fovy; quat is [x, y, z, w] and defaults to identity { 0, 0, 0, 1 }.

scene.cameras.push_back(mj_kdl::CameraSpec{
    .name = "front",
    .pos  = { 0.0, -0.8, 1.45 },                  // required
    .quat = { 0.300706, 0.0, 0.0, 0.953717 },     // 35 deg about x, tilt down
    .fovy = 45.0,                                 // required
});

After compile, all cameras are visible to MuJoCo and include:

• robot MJCF cameras,
• cameras added through SceneSpec::cameras,
• cameras in MJCF object assets.

List them:

for (int i = 0; i < env.model->ncam; ++i) {
    std::cout << mj_id2name(env.model, mjOBJ_CAMERA, i) << "\n";
}

Use one in the viewer, or set the recorder's cam directly:

mj_kdl::use_camera(&env.viewer, env.model, "front");
recorder.cam.type       = mjCAMERA_FIXED;
recorder.cam.fixedcamid = mj_name2id(env.model, mjOBJ_CAMERA, "front");

The Simulate UI also has its own live camera selector in the Rendering panel.

9. Write Production Reset Hooks

reset(&env) resets MuJoCo, re-seeds every registered robot and scene slot so stale commands do not hit the first post-reset step, runs your hook, then reads the measurements back.

KDL::JntArray q_home(robot.n_joints);
double cube_start[3] = { 0.35, 0.05, 0.73 };
 
env.on_reset = [&](mj_kdl::ResetContext *ctx) {
    mj_kdl::set_joint_pos(&robot, q_home);
    mj_kdl::set_body_pose(ctx->env, "cube", cube_start);
    task_state = TaskState::HOME;
    episode_step = 0;
};
 
mj_kdl::ResetOptions opts{
    .keyframe     = 0,
    .use_keyframe = true,
};
 
mj_kdl::ResetInfo info = mj_kdl::reset(&env, &opts);

Put robot, object, controller, randomization, and task-specific state in the hook. Do not hide reset work in the control loop.

Reset Lifecycle

reset(&env) performs the reset in this order:

1. Reset MuJoCo data to keyframe/default state and call mj_forward().
2. Re-seed everything the Env holds from the reset state:
   • every registered Robot's ports: measured values read, jnt_pos_cmd = measured position, velocity/torque commands zero, jnt_saturated cleared, a requested ctrl_mode kept; its F/T readings,
   • every scene slot: joint/free-body readings re-read, wrench commands zero, actuator commands = the reset ctrl.
3. Call env.on_reset, if provided, with a ResetContext.
4. Read the measurements (ports, F/T, slot readings) from the result.

That order is deliberate. The hook runs after the re-seed, so a command it primes (a gravity torque, say) is not overwritten, and a pose it sets is what the ports read. Commands are not re-seeded after the hook: a hook that moves a POSITION robot sets its jnt_pos_cmd too, or the first update drives it back. Each part's runtime state lives in one struct that reset assigns afresh, so a field added later is reset too; a part without a reset does not compile. A robot's ports are rewritten in place, so a pointer you keep to a port element survives it. The Simulate UI's reset button runs the same path.

Use ResetContext when your hook needs direct MuJoCo access:

env.on_reset = [&](mj_kdl::ResetContext *ctx) {
    const double q_identity[4] = { 0.0, 0.0, 0.0, 1.0 };   // [x, y, z, w]
    const double cube_pos[3]   = { 0.35, 0.05, 0.73 };
    mj_kdl::set_body_pose(ctx->env, "cube", cube_pos, q_identity);
 
    controller_integral.assign(robot.n_joints, 0.0);
    task_state = TaskState::HOME;
};

For randomized starts, generate random values in the hook and write them to MuJoCo before reset() returns.

10. Use The Simulate UI

The full viewer path is:

mj_kdl::open_viewer(&env, "task");
 
while (env.data->time < 10.0 && mj_kdl::step(&env)) {
    mj_kdl::update(&env);
    mj_kdl::pace_realtime(&env);   // step() never sleeps; pace the loop yourself
    // control...
}

With the viewer open, step() also honours its pause, perturbation and recording. Setting robot.paused on every registered robot pauses physics in both modes.

Useful wrapper-specific controls:

Control	Behavior
,	slow the wrapper real-time factor
.	speed the wrapper real-time factor
Simulation -> RTF	current wrapper real-time factor
Simulation -> Recorder	path, camera, resolution, FPS, start/stop, status

Recorder camera options include Current, Free, Tracking, and every fixed camera compiled into the model.

The right panel adds debug sections:

Section	Behavior
Frames	per-body/site coordinate triads (RViz-style), with a Scale slider
Perturb	Body shows the double-clicked selection; Drag = Camera/Force/Torque, then left-drag to apply (or Ctrl+right/left-drag)

The Equality and Group enable sections are hidden by default; rebuild with -DSHOW_EQUALITY_PANEL=ON / -DSHOW_GROUP_PANEL=ON to restore them. These debug geoms appear in the live viewer only, not in recorded MP4s.

Simulate UI Recorder Controls

The recorder controls live in the left Simulation panel:

Field	Meaning
Path	Output MP4 path, relative to the process working directory unless absolute
Camera	Current, Free, Tracking, or any compiled fixed camera
Resolution	360p, 480p, 720p, or 1080p
FPS	Recording frame rate
Start rec	Opens EGL recorder and starts feeding frames
Stop rec	Closes ffmpeg and finalizes the MP4
Rec	idle, recording, or failed

Current means the recorder follows the Simulate viewer camera. A fixed camera records from that camera even if the live viewer is moved elsewhere.

When recording stops successfully, the terminal prints:

[mj_kdl] recording saved to recording.mp4

If the status changes to failed, check that ffmpeg is installed and the path is writable.

Draw A Live Trajectory Trace Overlay

The viewer owns a user scene that the render thread merges into every frame. Two helpers let you draw your own line geometry into it – the built-in use is a live polyline of recent end-effector positions, which makes it obvious at a glance whether the EE is tracking a commanded path:

#include <deque>
 
std::deque<KDL::Vector> trace;          // ring buffer of recent EE points
constexpr size_t kTraceMax = 4096;      // bounded by the user-scene geom budget
 
while (env.data->time < 10.0 && mj_kdl::step(&env)) {
    mj_kdl::update(&env);
    mj_kdl::pace_realtime(&env);   // step() never sleeps; pace the loop yourself
    // ... run your controller; advance the EE ...
 
    // FK gives the EE in the chain-root frame; the overlay renders in world
    // frame, so lift it through the (fixed) base body pose -- cache this once.
    static KDL::Frame world_T_base;
    static bool have_base = mj_kdl::get_body_frame(&env, "base_link", &world_T_base);
 
    KDL::Frame ee_base;
    fk_solver.JntToCart(q, ee_base);
    trace.push_back(world_T_base * ee_base.p);
    if (trace.size() > kTraceMax) trace.pop_front();
 
    mj_kdl::clear_trace(&env.viewer);             // reset the overlay each frame
    static constexpr float kOrange[4] = {1.0f, 0.5f, 0.1f, 1.0f};
    for (size_t i = 1; i < trace.size(); ++i)
        mj_kdl::add_trace_segment(&env.viewer, trace[i - 1], trace[i], kOrange);
}

Key points:

• Both helpers are no-ops while the viewer is closed (e.g. headless runs), so the same loop compiles and runs unchanged with no display – guard the bookkeeping with if (!headless) to skip the allocation entirely.
• add_trace_segment() is thread-safe and silently drops segments once the user-scene geom buffer is full (8192 geoms). Keep your ring buffer at or below that to avoid a truncated trace.
• Pass nullptr for rgba to use the default warm orange.
• Points are in world frame. Robot FK is in the chain-root frame, so compose with the base body's world pose (constant for a fixed base; read it once).

The motion-spec code generator wires this up automatically: declare a TRACE block in the ENVIRONMENT of a .robmot model (enabled, length, color) and every generated demo renders the trace with no hand-written loop code.

11. Record Video

Interactive recording is available in the Simulate UI:

1. Open the Simulation panel.
2. Scroll to Recorder.
3. Set Path, Camera, Resolution, and FPS.
4. Press Start rec.
5. Press Stop rec.

The terminal prints:

[mj_kdl] recording saved to <path>

Headless recording uses the same VideoRecorder API:

mj_kdl::VideoRecorder recorder;
mj_kdl::init_video_recorder(
    &recorder, env.model, "episode.mp4", mj_kdl::VideoResolution::R1080p, 60);
 
for (int i = 0; i < 3000; ++i) {
    mj_kdl::step(&env);
    if (i % 4 == 0) {
        mj_kdl::record_frame(&recorder, &env);
    }
}
 
mj_kdl::cleanup(&recorder);

12. Build A Tabletop Pick-Place Example

This section assembles the pieces into a complete pick-place task. The goal is to pick a cube from one table location, move it to another location, open the gripper, and retreat. The full implementation is src/examples/ex_table_pick_place.cpp; the code below shows the structure you should reproduce in your own application.

The application has five parts:

1. Build a scene with an arm, gripper, table asset, cube, and camera.
2. Read the table site to compute reliable tabletop coordinates.
3. Build KDL FK/IK/dynamics solvers.
4. Define a reset hook and state machine.
5. Run torque impedance control against state-specific IK targets.

12.1 Build The Scene

Start with the gripper attachment. The Kinova pinch_site already encodes the tool offset and 180-degree flip, so no pos/quat are needed:

mj_kdl::AttachmentSpec gripper{
    .mjcf_path = mj_kdl_examples::asset("robotiq_2f85/2f85.xml"),
    .attach_to = { mj_kdl::AttachKind::Site, "pinch_site" },
    .prefix    = "g_",
};

Add the table as an MJCF-backed SceneObject. The table asset origin is the tabletop surface center, so placing it at z = 0.7 makes the top surface 0.7 m.

mj_kdl::SceneObject table{
    .name      = "table",
    .mjcf_path = mj_kdl_examples::asset("table.xml"),
    .pos       = { 0.0, 0.0, 0.7 },
    .fixed     = true,
};

Add a free cube. For a box, size is half-extents, so the world-frame center z is surface_z + half_height. Free objects must stay world-anchored (MuJoCo restricts freejoints to top-level bodies).

constexpr double kSurfaceZ = 0.7;
constexpr double kCubeHalf = 0.025;
 
mj_kdl::SceneObject cube{
    .name     = "cube",
    .shape    = mj_kdl::Shape::BOX,
    .size     = { kCubeHalf, kCubeHalf, kCubeHalf },
    .pos      = { 0.35, 0.10, kSurfaceZ + kCubeHalf },
    .rgba     = { 0.1f, 0.25f, 1.0f, 1.0f },
    .mass     = 0.1,
    .condim   = mj_kdl::Condim::Torsional,
    .friction = { 0.8, 0.02, 0.001 },
};

Assemble the scene. Set timestep, add_floor, and add_skybox explicitly, then mount the arm on the table's table_top site instead of writing surface_z into the robot position by hand:

mj_kdl::SceneSpec scene;
scene.timestep   = 0.002;
scene.add_floor  = true;
scene.add_skybox = true;
scene.objects.push_back(table);
scene.objects.push_back(cube);
 
const std::string mount = "table_top";
 
scene.robots.push_back(mj_kdl::RobotSpec{
    .path        = mj_kdl_examples::asset("kinova_gen3/gen3.xml"),
    .attach_to   = { mj_kdl::AttachKind::Site, mount },
    .attachments = { gripper },
});
scene.cameras.push_back(mj_kdl::CameraSpec{
    .name = "task",
    .pos  = { 0.1, -0.9, 1.45 },
    // extrinsic XYZ (35, 0, 5) deg: tilt down, yaw slightly right
    .quat = { 0.300420, 0.013117, 0.041601, 0.952809 },
    .fovy = 45.0,
});
 
mj_kdl::Env env;
mj_kdl::init_env(&env, &scene);

12.2 Read Table Sites Instead Of Hardcoding Geometry

The table asset defines a table_top site, compiled under that name (the table sets no prefix):

KDL::Frame world_T_table_top;
 
if (!mj_kdl::get_site_frame(&env, "table_top", &world_T_table_top)) {
    throw std::runtime_error("table_top site not found");
}
 
const double surface_z = world_T_table_top.p.z();

Now define task points from the table surface:

KDL::Vector pick_pos(0.35, 0.10, surface_z + 0.02);
KDL::Vector place_pos(0.20, -0.18, surface_z + 0.02);
 
KDL::Vector approach_offset(0.0, 0.0, 0.16);
KDL::Vector lift_offset(0.0, 0.0, 0.22);

This makes the task robust if you move the table asset or swap it for another asset with the same site convention.

12.3 Initialize Robot, Tool, And Solvers

Initialize the robot with gripper inertia and TCP site:

mj_kdl::Robot robot;
mj_kdl::ToolFrameSpec tool{
    .tool_body = "g_base_mount",
    .tcp_site  = "g_pinch",
};
 
mj_kdl::init_robot_from_mjcf(&robot, &env, "base_link", "bracelet_link", "", &tool);
 
robot.ctrl_mode = mj_kdl::CtrlMode::TORQUE;

Create KDL solvers:

KDL::ChainFkSolverPos_recursive fk(robot.chain);
KDL::ChainIkSolverVel_pinv ik_vel(robot.chain);
KDL::ChainIkSolverPos_NR_JL ik_pos(
    robot.chain, q_min, q_max, fk, ik_vel, 100, 1e-5);
KDL::ChainDynParam dyn(robot.chain, KDL::Vector(0.0, 0.0, scene.gravity_z));

q_min and q_max come from robot.joint_limits. Keep them in KDL::JntArray so IK respects the compiled model limits.

12.4 Generate IK Waypoints

Use one orientation for the gripper and solve a sequence of poses:

KDL::Rotation grasp_R = KDL::Rotation::RPY(M_PI, 0.0, M_PI / 2.0);
 
KDL::Frame pick_above(grasp_R, pick_pos + approach_offset);
KDL::Frame pick_frame(grasp_R, pick_pos);
KDL::Frame lift_frame(grasp_R, pick_pos + lift_offset);
KDL::Frame place_above(grasp_R, place_pos + approach_offset);
KDL::Frame place_frame(grasp_R, place_pos);
KDL::Frame retreat_frame(grasp_R, place_pos + lift_offset);

Solve each waypoint seeded from the previous solution:

auto solve = [&](const KDL::Frame &target, const KDL::JntArray &seed) {
    KDL::JntArray q_out(robot.n_joints);
    if (ik_pos.CartToJnt(seed, target, q_out) < 0) {
        throw std::runtime_error("IK failed");
    }
    return q_out;
};
 
KDL::JntArray q_home = measured_home(robot);
KDL::JntArray q_pick_above = solve(pick_above, q_home);
KDL::JntArray q_pick       = solve(pick_frame, q_pick_above);
KDL::JntArray q_lift       = solve(lift_frame, q_pick);
KDL::JntArray q_place_above = solve(place_above, q_lift);
KDL::JntArray q_place      = solve(place_frame, q_place_above);
KDL::JntArray q_retreat    = solve(retreat_frame, q_place);

In production code, print the target names when IK fails. It saves time when a single pose is outside the workspace or has an impossible orientation.

12.5 Define The State Machine

Keep state configuration data-driven. Each row says where to move, how long to interpolate, how long to wait before forcing transition, and what the gripper should do.

enum class TaskState {
    HOME,
    PICK_ABOVE,
    PICK,
    CLOSE,
    LIFT,
    PLACE_ABOVE,
    PLACE,
    OPEN,
    RETREAT,
    HOLD,
};
 
struct StateConfig {
    TaskState state;
    KDL::JntArray target;
    double duration;
    double timeout;
    double settle_tol;
    double gripper_cmd;
};
 
constexpr double kClosed = 0.82; // the bundled 2F-85's ctrl is its driver angle [rad]
 
std::vector<StateConfig> plan = {
    { TaskState::HOME,        q_home,        1.0, 2.5, 0.08, 0.0     },
    { TaskState::PICK_ABOVE,  q_pick_above,  2.0, 4.0, 0.08, 0.0     },
    { TaskState::PICK,        q_pick,        1.5, 3.5, 0.06, 0.0     },
    { TaskState::CLOSE,       q_pick,        0.8, 1.5, 0.00, kClosed },
    { TaskState::LIFT,        q_lift,        2.0, 4.0, 0.08, kClosed },
    { TaskState::PLACE_ABOVE, q_place_above, 2.0, 4.0, 0.08, kClosed },
    { TaskState::PLACE,       q_place,       1.5, 3.0, 0.06, kClosed },
    { TaskState::OPEN,        q_place,       0.8, 1.5, 0.00, 0.0     },
    { TaskState::RETREAT,     q_retreat,     1.5, 3.0, 0.08, 0.0     },
    { TaskState::HOLD,        q_retreat,     1.0, 1.0, 0.00, 0.0     },
};

On entry to a state, capture the measured joint position. Interpolate from that entry pose to the state target:

KDL::JntArray q_enter = current_q(robot);
double t_enter = env.data->time;
 
double alpha = std::clamp(
    (env.data->time - t_enter) / cfg.duration, 0.0, 1.0);
 
KDL::JntArray q_des(robot.n_joints);
for (int i = 0; i < robot.n_joints; ++i) {
    q_des(i) = q_enter(i) + alpha * (cfg.target(i) - q_enter(i));
}

Transition when either the duration has elapsed and the joint error is small, or the timeout is reached:

bool settled = joint_error(q_des, current_q(robot)) < cfg.settle_tol;
bool duration_done = env.data->time - t_enter >= cfg.duration;
bool timed_out = env.data->time - t_enter >= cfg.timeout;
 
if ((duration_done && settled) || timed_out) {
    advance_to_next_state();
}

12.6 Apply Torque Impedance

Use KDL gravity plus joint PD:

KDL::JntArray q(robot.n_joints);
KDL::JntArray dq(robot.n_joints);
KDL::JntArray gravity(robot.n_joints);
 
for (int i = 0; i < robot.n_joints; ++i) {
    q(i)  = robot.jnt_pos_msr[i];
    dq(i) = robot.jnt_vel_msr[i];
}
 
dyn.JntToGravity(q, gravity);
 
for (int i = 0; i < robot.n_joints; ++i) {
    const double kp = 120.0;
    const double kd = 18.0;
    robot.jnt_trq_cmd[i] =
        gravity(i) + kp * (q_des(i) - q(i)) - kd * dq(i);
}

The gripper actuator is model-specific. Bind it once as a scene slot; update(&env) writes its command to ctrl, clamped to its ctrlrange:

mj_kdl::SceneActuatorSlot *gripper = mj_kdl::bind_scene_actuator(&env.scene, "g_fingers_actuator");
// in the loop:
gripper->command = cfg.gripper_cmd;

12.7 Reset The Pick-Place Task

Reset must restore both the robot and task objects:

const double cube_start[3] = { 0.35, 0.10, surface_z + kCubeHalf };
 
env.on_reset = [&](mj_kdl::ResetContext *ctx) {
    mj_kdl::set_joint_pos(&robot, q_home);
    mj_kdl::set_body_pose(ctx->env, "cube", cube_start);
 
    current_state = TaskState::HOME;
    state_index = 0;
    q_enter = q_home;
    t_enter = 0.0;
};

Because reset(&env) re-seeds registered robots and scene slots before this hook and reads the result back after it, the first control step after reset starts from the hook's pose without stale torque or gripper commands. This controller is in TORQUE; a POSITION robot moved in the hook would also need its jnt_pos_cmd set there.

12.8 Run With Viewer And Recorder

Start the Simulate UI:

mj_kdl::open_viewer(&env, "table pick-place");
mj_kdl::use_camera(&env.viewer, env.model, "task");
 
bool task_done = false;   // run_state_machine() sets it when HOLD ends
while (!task_done && mj_kdl::step(&env)) {
    mj_kdl::update(&env);
    mj_kdl::pace_realtime(&env);   // step() never sleeps; pace the loop yourself
    run_state_machine();
    apply_impedance_command();
}

Use , and . to slow down or speed up the wrapper real-time factor. In the Simulation panel, the Recorder subsection can write an MP4 using Current, Free, Tracking, or any compiled fixed camera.

12.9 What To Validate

Before treating the example as working, check:

• IK waypoints are reachable before the simulation loop starts.
• The cube is a free body and set_body_pose() can reset it.
• The gripper command closes enough to hold the cube but does not destabilize the arm.
• Joint torque limits are reasonable for the gains.
• The state machine can recover after pressing Reset in the Simulate UI.
• Headless mode runs a fixed number of steps and reports cube final position.

This is the architecture used by ex_table_pick_place: a small scene spec, a reset hook, precomputed IK waypoints, a data-driven state table, and one control law used consistently across states.

13. Build A Multi-Robot Scene

Use prefixes to disambiguate the second robot:

scene.robots = {
    mj_kdl::RobotSpec{
        .path = mj_kdl_examples::asset("kinova_gen3/gen3.xml"),
        .pos  = { -0.7, 0.0, 0.0 },
        .attachments = { gripper },
    },
    mj_kdl::RobotSpec{
        .path   = mj_kdl_examples::asset("kinova_gen3/gen3.xml"),
        .prefix = "r2_",
        .pos    = { 0.7, 0.0, 0.0 },
        .attachments = { gripper },
    },
};

Then initialize two robot handles:

const mj_kdl::ToolFrameSpec right_tool{ .tool_body = "r2_g_base_mount", .tcp_site = "r2_g_pinch" };
 
mj_kdl::Robot left;
mj_kdl::Robot right;
mj_kdl::init_robot_from_mjcf(&left, &env, "base_link", "bracelet_link", "", &tool);
mj_kdl::init_robot_from_mjcf(&right, &env, "r2_base_link", "r2_bracelet_link", "", &right_tool);

The prefix argument is prepended to every name the call resolves (bodies, tool body, TCP site, F/T sensors), as ex_rnea_pick_place does with "r2_"; passing already-prefixed names with no prefix, as above, is the same. Each robot gets its own KDL chain and command ports, while both share the same Env; one update(&env) reads and commands both.

14. Grow Into Task Examples

The included examples show how these pieces combine:

• ex_table_pick_place: IK waypoints, phase table, torque impedance, table asset sites, a scene wrench pushing the arm mid-carry.
• ex_table_pour: gripper-held bottle asset, free particles, receiver asset; --record writes an MP4.
• ex_rnea_pick_place: two prefixed arms at one table, each with its own RNEA computed torque, plus free objects and scene cameras.
• ex_achd_pick_place, ex_achd_table_slide: ACHD; the slide also presses on the table through ACHD's external-force input.
• ex_admittance_ft: F/T admittance around an RNEA task-space inner loop.

Read ../examples.md for behavior summaries and expected outputs.

15. Modify A Running Scene

For occasional changes, use the scene add/remove helpers. They rebuild the model, so this is for task setup and coarse changes, not per-frame object spawning.

mj_kdl::SceneObject obstacle{
    .name  = "obstacle",
    .shape = mj_kdl::Shape::CYLINDER,
    .size  = { 0.04, 0.12, 0.0 },                 // {radius, half_length, 0}
    .pos   = { 0.3, -0.2, 0.82 },
    .rgba     = { 0.6f, 0.6f, 0.6f, 1.0f },
    .fixed    = true,                             // a fixed primitive may leave mass unset
    .friction = { 0.8, 0.02, 0.001 },             // required for primitives
};
 
if (mj_kdl::Status s = mj_kdl::scene_add_object(&env, obstacle); !s) std::cerr << s.error;
// env.model/env.data are replaced; the physics state carries over by name, and registered
// robots, scene slots, the viewer and recorders follow.

A scene slot whose name is gone from the rebuilt model is unbound and skipped. A failed rebuild returns the error and leaves the Env as it was. Any MuJoCo IDs you cached yourself may be invalid; recompute them.

16. Debugging Checklist

Use this checklist when a scene behaves incorrectly:

Symptom	Check
Robot does not move in position mode	Model has an actuator on each joint, and update(&env) runs every cycle
A call fails	Print the returned Status::error; it names the cause
First step after reset jumps	Reset goes through reset(&env), not a raw mj_resetData(); a hook that moves a POSITION robot sets its jnt_pos_cmd too
KDL gravity is wrong with a tool	ToolFrameSpec::tool_body points at the tool subtree root (2F-85: g_base_mount, not g_base)
Torque commands saturate	jnt_saturated; compare against joint_force_limits(&robot)
TCP frame is wrong	ToolFrameSpec::tcp_site names an authored MuJoCo site
Object asset site not found	Use the asset's own site name, with SceneObject::prefix in front if one is set
Recorder fails	BUILD_RECORDER=ON, EGL available, ffmpeg installed, output path writable
Camera missing in recorder list	Camera must exist in the compiled mjModel (mj_name2id(model, mjOBJ_CAMERA, name) >= 0)

17. Development Checks

Run the default build:

cmake --build build --target mj_kdl_wrapper -j$(nproc)

Run clang-tidy on wrapper code:

clang-tidy -p build src/mj_kdl_wrapper.cpp

Run tests:

ctest --test-dir build --output-on-failure

The vendored src/simulate_ui/simulate.cc is MuJoCo sample UI code with small wrapper UI additions. It is intentionally excluded from clang-tidy style cleanup so local changes stay reviewable against upstream.