TOCATL Hexapod — ROS 2 + MuJoCo Walking Simulator

The real TOCATL is driven by a console controller over CAN, with 18 Xiaomi CyberGear quasi-direct-drive actuators across six identical three-joint legs. The goal of this project was to reconstruct that whole locomotion pipeline in simulation — geometry, gait math, joint map, and control scheme — so that walking behaviour can be developed, tested, and recorded without touching hardware, and so the same gait node can later retarget the real CAN backend without changing any locomotion logic.

Everything is derived from the existing repository's pure-math modules rather than re-derived: link lengths, forward and inverse kinematics, and joint-to-motor transforms come from legacy_geometry.py; gait phase offsets, duty factors, and the walking envelope come from legacy_reference.py; and the canonical 18-motor ordering comes from motor_map.py. The legacy deployment scripts were read only as reference — never executed, since they enable real motors on startup.

The simulator is complete, walks firmly, renders the real CAD body, and is committed on the branch codex/tocatl-mujoco-sim.

Two tunings mattered more than the rest. Raising the stance stiffness ramp rate (kp_rate_up) from 1 to 25 is what makes the legs plant rather than settle, which is the entire difference between a firm-looking walk and a mushy one. And the slip metric had to be measured during genuinely loaded contact rather than over the whole stance phase mask — otherwise the number reports foot motion from moments when the foot is not carrying weight at all.

TOCATL is a radially symmetric hexapod: a central body carrying six identical legs, each with three revolute joints — hip yaw about the leg-local Z axis (swings the leg around its mount), hip pitch about the leg-local Y axis (lifts the thigh), and knee about the leg-local Y axis (bends the shin relative to the thigh). Eighteen joints in total, one CyberGear actuator each.

Maximum radial reach from the hip is d2 + d3 + d4 ≈ 834 mm, though the practical planted reach stays well inside that. The body is modelled as a 600 mm diameter disc, and leg k is mounted at 30° + k·60°, so the six hips sit every 60° with each hip-yaw axis pointing radially outward.

Inverse kinematics takes θ1 = atan2(y, x) and then solves the planar two-link problem in the radial–vertical plane for θ2 and θ3 with the law of cosines, taking the positive-knee branch.

The single most important kinematic detail: the legacy FK rotates hip yaw about [d2, 0, d1] rather than about the mount point. The MJCF reproduces this exactly — verified to under a micrometre — which is why IK targets track with zero error instead of accumulating a constant offset that would only show up later as mysterious foot drift.

Motor IDs follow leg·10 + joint, where joint 1 is hip yaw, 2 is hip pitch, and 3 is knee: leg 1 is 11/12/13, leg 2 is 21/22/23, and so on through leg 6 at 61/62/63. This exact ordering is preserved in every 18-element joint vector so the sim maps one-to-one onto the real robot later. The real robot splits these across two CAN buses; in simulation that split is irrelevant and survives only in metadata comments.

The real command streamer sends MIT-style tuples (position, velocity, kp, kd, torque) per motor and the motor closes the loop internally. Rather than use MuJoCo position actuators, the sim uses pure torque actuators and computes τ in software from the incoming command. That choice is what preserves the real controller's per-joint, per-phase gain scheduling — with position actuators the gain schedule simply cannot be expressed. Torque is clipped to the CyberGear peak of ±12 N·m, and the legacy hip-pitch support-torque cap of 9.25 N·m serves as a useful sanity bound.

A single global phase u ∈ [0, 1) advances at du = dt / cycle_time. Each leg reads its own phase u_i = (u + phase_offset[i]) mod 1; if u_i < duty_factor the leg is in stance with progress s = u_i / duty, otherwise it is in swing with s = (u_i − duty) / (1 − duty).

Each leg gets a neutral foot point radially out from its hip at R_foot, on the ground plane, with the body at height H_body. Travel direction and stride length come from the commanded body velocity, clamped to the stride maximum; a yaw command adds a tangential per-leg component.

• Stance — the foot moves linearly from +L/2 to −L/2 along the travel direction at z = 0. In world coordinates the foot stays put while the body advances, which is precisely the no-slip condition.
• Swing — the foot returns from −L/2 to +L/2, lifting along z = step_height · sin(π·s) for a clean arch.

Each foot target is then converted from the body frame into the leg's local frame by subtracting the hip mount position and rotating by −(mount_angle + body_yaw), run through IK, and packed into the 18-vector.

Stance legs must be stiff so they plant hard and carry weight; swing legs must be soft so they do not fight the air. The schedule runs stance Kp ≈ 100 against swing Kp ≈ 10 with Kd ≈ 3, ramping Kp up quickly on entering stance and down slowly on entering swing. The gait node passes (pos, vel, kp, kd) per joint to the sim node, which applies the MIT-mode torque law above.

All new code lives in a single new ament_python package, src/tocatl_sim, leaving the existing control package and its safety state machine, command limiter, and calibration completely untouched.

Keeping the gait outside the physics node is deliberate: it mirrors the real robot's node split of gait generator → command sender → motors, so the identical gait_node can later drive real CAN through a different backend without any change to locomotion logic. A launch file starts all three nodes, and a params YAML exposes every tunable — masses, gains, envelope, timestep, and camera.

Commanded speeds slew toward their target rather than stepping to it, so motion reads smoothly on camera, and hold-to-move is used because it looks far better in a recording than press-to-set-velocity.

The MJCF is generated parametrically by a small Python builder that loops over the six legs using the mount angles, so geometry can never drift out of sync between legs. No CAD meshes are required for the dynamics: the base is a disc plate of radius 0.30 m with a free joint, and each leg is a coxa capsule for the d2 offset, a femur capsule of length 0.330 m, and a tibia capsule of length 0.458 m, ending in a small high-friction foot sphere.

• Contacts — feet use condim=6 with friction="1.2 0.02 0.001" plus a little solref/solimp softness so contacts do not jitter.
• Actuators — one torque motor per hinge with forcerange="-12 12".
• Mass — roughly 13.6 kg total, with link mass placed near the joints since the CyberGear actuators dominate it, keeping the centre of mass low and central.
• Foot radius — 12 mm.
• Sensors — frame position and quaternion on the base, joint position and velocity on every hinge, and a touch sensor per foot, which is what makes the headless acceptance test possible.

To make the sim look like the actual robot, 17 SolidWorks parts were converted to STL entirely in-browser through a WASM converter, so the CAD never left the machine. The lofted shell parts tessellate empty, so the six-petal dome was instead rebuilt parametrically in a dome_builder.py script — creases, arched leg openings, and a light trim — and junk disc and cone artifacts in some exports were removed by connected-component filtering. The motors are stylized Ø86 × 36.5 mm cylinders.

Placement lives in a cad_layout.py module, with the hip turret transform derived by cylinder-fitting its motor bore. The stance was rematched to a photo of the real robot: body at 220 mm, a 0.59 m foot ring, knees riding above the dome. Setting impratio=10 kills the foot creep that a sprawled stance otherwise produces. Critically, all mesh geoms are massless, so the dynamics are bit-identical to the capsule model and a single launch argument reverts to capsules.

Rather than build the whole thing and hope, the sim was brought up one rung at a time, each rung fixed before advancing, with the headless acceptance test asserting the metrics so tuning could iterate automatically before ever opening the viewer.

The acceptance thresholds encoded in the test are a forward displacement of at least 0.15 m and within 25% of commanded, mean roll and pitch under 3°, base-height standard deviation under 15 mm, no base–ground contact, no joint NaNs, and per-foot horizontal slip under 15 mm during that foot's stance phase.

• No import of can, the CyberGear driver, python-cybergear, or socketcan anywhere in the package.
• No call to CAN setup, motor enable, or zero-position routines.
• The legacy/ tree is read-only reference. The legacy walking scripts were read to understand the gait, and the math was ported — the files were never executed.
• Only the hardware-free pure-math modules are imported, never copied, so the sim and the real controller cannot drift apart.
• The software-safety CI passes locally on the branch.

Focus the MuJoCo window and drive with the keyboard map above. To record a run headlessly, launch the offscreen recorder with the EGL rendering backend and it writes walk.mp4 directly. The environment is Ubuntu 24.04 with ROS 2 Jazzy; MuJoCo, NumPy, and SciPy come from pip, and the venv needs system site packages so mujoco and rclpy are importable together.

Alongside the build, I set myself a structured curriculum covering every concept the simulator depends on, with the standard for each topic being that I can explain the full locomotion pipeline unaided — motivation, the alternatives and why this approach wins, the mathematical intuition with every variable accounted for, its role in this project, the engineering trade-offs, and its failure modes.