Real-Time Iteration (RTI) MPC pipeline for floating-base legged robots, built on top of Horizon and phase_manager.
The pipeline is driven entirely by a single YAML file (default path: ~/.ros/receding_kyon.yaml).
model: # URDF, floating base, fixed joints
problem: # Horizon problem variables, costs, constraints
tasks: # CartesianTask / ContactTask definitions
gait_manager: # stance/flight phase timing per contactProblem parses the YAML and builds the Horizon MPC problem (decision variables, parameters, cost terms, constraints). Only basic functionalities are ported from Python to C++.
| Member | Type | Description |
|---|---|---|
from_yaml(node) |
method | Parse YAML → populate variables, parameters, costs, constraints, dynamics |
var_map |
map<string, Variable::Ptr> |
All decision variables |
param_map |
map<string, Parameter::Ptr> |
All parameters |
cost_map |
map<string, Cost::Ptr> |
Cost terms |
constr_map |
map<string, Constraint::Ptr> |
Constraints |
N |
int |
Number of shooting nodes |
dt |
VectorXd |
Time step(s) |
dynamics |
casadi::Function |
Discrete dynamics |
Initialization (call once, before the RTI loop):
| Method | Description |
|---|---|
setModel(model) |
Attach a FullModelInverseDynamics |
setFunctions() |
Register all costs and constraints with the iLQR solver |
loadInitialGuess() |
Seed state/input trajectories from the problem's static x_ini / u_ini (from YAML) |
updateBounds() |
Push initial state/input bounds to the solver |
setParameterValues() |
Push initial parameter values (weights, references, …) to the solver |
initWeightMasks() |
Set every *_weight_mask parameter to all-ones |
setFunctionNodes() |
Set active node indices for every cost/constraint in the solver |
bootstrap() |
Warm-up: run 100 iLQR iterations from the initial guess |
RTI loop (call every cycle):
| Method | Description |
|---|---|
loadSolutionAsInitialGuess(shift) |
Roll last solution by shift and use as warm-start; pins x0 |
setParameterValues() |
Push updated parameter values to the solver |
updateBounds() |
Re-pin x0 in xlb/xub and push bounds to the solver |
setFunctionNodes() |
Update active node indices (after pm->update()) |
rti() |
Single RTI step |
FullModelInverseDynamics builds a CasADi kinematics/dynamics object from the URDF.
| Method | Description |
|---|---|
getKinDyn() |
Access the underlying CasadiKinDyn object |
getJointNames() |
Ordered list of non-fixed joint names |
isFloatingBase() |
true if the robot has a floating base |
nq() / nv() |
Configuration / velocity space size |
fk(frame) |
Forward kinematics CasADi function for a frame |
jacobian(frame) |
Jacobian CasADi function for a frame |
getContacts() |
List of contact frame names |
getContactMap() |
Map from contact frame → associated force variable names |
getForceMap() |
Map from force variable name → contact frame |
getInitialState(var) |
Initial state vector for a given variable |
getInitialInput(var) |
Initial input vector for a given variable |
getQInit() |
Per-joint initial configuration (map<name, value>) |
getBaseInit() |
7-element base pose initial value (position + quaternion) |
TaskInterface owns the task registry. Built-in task types:
YAML type |
C++ class | Description |
|---|---|---|
Cartesian |
CartesianTask |
Cartesian position/orientation tracking |
Contact |
ContactTask |
Contact force / no-slip constraint |
| Method | Description |
|---|---|
from_yaml(config) |
Instantiate all tasks from the tasks: YAML section |
getTask(name) |
Retrieve a task by name |
getTasks() |
Return the full ordered task list |
getContactList() |
Ordered list of contact frame names (from ContactTask entries) |
getProblemInterface() |
Access the underlying ProblemInterface |
printSolverState() |
Dump active costs/constraints and their node sets |
Initialization (call once, before the RTI loop):
| Method | Description |
|---|---|
bootstrap() |
Delegate to ProblemInterface::bootstrap() |
loadSolutionAsInitialGuess(shift) |
Shift and reseed warm-start from bootstrap solution |
RTI loop (call every cycle):
| Method | Description |
|---|---|
loadSolutionAsInitialGuess(shift) |
Roll last solution by shift and reseed warm-start; pins x0 |
updateParameters() |
Delegate to ProblemInterface::setParameterValues() |
updateBounds() |
Delegate to ProblemInterface::updateBounds() |
updateIndices() |
Rebuild solver per-node cost/constraint index tables |
rti() |
Delegate to ProblemInterface::rti() |
getSolution() |
Return last solution as map<name, MatrixXd> |
GaitManager reads from the YAML file to populate phases of the PhaseManager. In particular:
- One
Timelineper contact, each holding a stance phase and a flight phase. - A
ContactSwingobject per contact that generates smooth arc trajectories. - Base-velocity reference state (
setBaseVelocity()).
| Method | Description |
|---|---|
fromYaml(node) |
Load stance/flight timing and initial mode from the gait_manager: YAML section |
initializeTimeline() |
Populate all timelines with their initial phases |
run() |
Dispatch current mode (append phases) and update base-velocity parameters — call once per cycle |
switchMode(mode, enable) |
Programmatically activate / deactivate an OperationMode |
getOperationMode() |
Current OperationMode |
setBaseVelocity(vel) |
Set 6-DOF base velocity reference [vx,vy,vz,wx,wy,wz] |
setParams(action, params) |
Override gait parameters (step_duration, step_height, double_stance) |
getParams(action) |
Read current gait parameters for an action |
getContactTimelines() |
Map of contact name → Timeline::Ptr |
getContactTimeline(c) |
Single timeline for contact c |
getStancePhase(c) / getFlightPhase(c) |
Phase objects for a contact |
hasSwing() |
Whether any swing task is configured |
getContactSwing(c) |
ContactSwing object for contact c |
print_phases() |
Print current timeline state to stdout |
GaitManager::run() dispatches the current OperationMode:
| Mode | Behaviour |
|---|---|
STAND |
Fills all timelines with stance phases |
TROT |
Alternates diagonal pairs (LF+RH, RF+LH) |
CRAWL |
Sequential single-foot swing |
WALK |
Slow, overlapping stance transitions |
STEP |
Single step on demand |
DRAG |
One contact drags along the ground |
IDLE |
No phase modifications |
Gait parameters (step_duration, step_height, double_stance) are per-mode and can be changed at runtime via setParams() or the ROS parameter server.
The ROS layer is optional and sits entirely on top of the core pipeline. It is composed of two classes that handle all I/O with the ROS ecosystem, leaving the solver loop free of any ROS dependency.
GaitManagerROSInterface is a zero-logic wrapper around GaitManager.
All it does is translate ROS messages/services into calls on GaitManager.
| Service | Type | Description |
|---|---|---|
/horizon/stand/switch |
std_srvs/SetBool |
Enable/disable STAND mode |
/horizon/trot/switch |
std_srvs/SetBool |
Enable/disable TROT mode |
/horizon/crawl/switch |
std_srvs/SetBool |
Enable/disable CRAWL mode |
/horizon/walk/switch |
std_srvs/SetBool |
Enable/disable WALK mode |
/horizon/step/switch |
std_srvs/SetBool |
Enable/disable STEP mode |
/horizon/drag/switch |
std_srvs/SetBool |
Enable/disable DRAG mode |
/horizon/idle/switch |
std_srvs/SetBool |
Enable/disable IDLE mode |
| Topic | Type | Direction | Description |
|---|---|---|---|
/horizon/base_velocity/reference |
geometry_msgs/Twist |
Sub | Sets base velocity reference (linear x/y, angular z) |
/horizon/operation_mode |
horizon_msgs/OperationMode |
Pub | Current operation mode (if horizon_msgs available) |
/horizon/<mode>/step_duration (int)
/horizon/<mode>/step_height (double)
/horizon/<mode>/double_stance (int)
These are read at each service call and pushed to GaitManager.
JoyCommands subscribes to /joy and:
- Converts stick axes to a
geometry_msgs/Twistpublished on/horizon/base_velocity/reference. - Calls gait-switch services on button edge events (non-blocking, detached thread).
| Topic | Type | Direction | Description |
|---|---|---|---|
/joy |
sensor_msgs/Joy |
Sub | Raw joystick input |
/horizon/base_velocity/cmd |
geometry_msgs/Twist |
Pub | Commanded base velocity |
| Button | Action |
|---|---|
btn[4] (hold) |
CRAWL on / off |
btn[5] (hold) |
TROT on / off |
btn[6] (momentary) |
STAND |
btn[7] (momentary) |
IDLE |
btn[0] |
Raise COM (+0.01 m) |
btn[2] |
Lower COM (−0.01 m) |
| Axis | Effect |
|---|---|
axes[1] |
Linear X (forward / backward) |
axes[0] |
Linear Y (strafe) |
axes[3] |
Angular Z (yaw) |
Scaling is set via JoyCommandsOptions::lin_vel_scale (default 0.2 m/s) and ang_vel_scale (default 0.3 rad/s).
Per-section timing comparison between the Python and C++ RTI pipelines, measured over 455 iterations (STAND → TROT forward → TROT rotate → STAND), 1 iLQR step per iteration.
Both pipelines run the same problem (kyon quadruped, same YAML config).
The original problem can be found in the kyon_controller repository, benchmarking branch — specifically the file python/receding_horizon.py.
| Section | Python (mean) | C++ (mean) | Speedup |
|---|---|---|---|
| loadSolutionAsInitialGuess | 0.33 ms | 0.023 ms | ~14× |
| pm.shift() | 0.72 ms | 0.172 ms | ~4× |
| gm.run() | 0.39 ms | 0.102 ms | ~4× |
| pm.update() | 7.22 ms | 1.325 ms | ~5× |
| updateParams+Bounds+Indices | 5.33 ms | 0.614 ms | ~9× |
| rti (solver) | 8.38 ms | 9.84 ms | — |
| TOTAL | 22.64 ms | 12.16 ms | ~1.9× |
The solver step is comparable between the two (same underlying C++ iLQR), while all surrounding pipeline overhead is substantially lower in C++.