Debug G1 MPC/WBID with PlotJuggler

Why Part 3 moves to PlotJuggler

In Part 1, we looked at Unitree G1 from the low-level side: MuJoCo, DDS, LowCmd, LowState, domain ID, and network interface. In Part 2, we converted a G1 motion into MCAP so it could be replayed as kinematics in Foxglove. Those two steps are useful, but controller debugging quickly asks a different question: which numeric signal went wrong first?

Foxglove is excellent for 3D scenes, TF, camera streams, point clouds, and log replay. PlotJuggler is better when you need to compare many time-series signals: CoM x/y/z, velocities, roll/pitch/yaw, contact force, active contact, foot references, and QP solve time. When a humanoid falls, watching the MuJoCo viewer is rarely enough. You need to see where the MPC expected the CoM to go, how far the current SRBD state drifted, whether the swing-foot reference was reasonable, which contacts were active, and whether WBID started taking too long to solve.

This article uses ioloizou/g1_locomotion, a Unitree G1 locomotion stack that combines Single Rigid Body Dynamics (SRBD), Model Predictive Control (MPC), and Whole-Body Inverse Dynamics (WBID) in a cascaded architecture. The repository README tells you to run:

roslaunch g1_mujoco_sim mpc_wbid_simulation.launch

Then, from another terminal, start PlotJuggler:

rosrun plotjuggler plotjuggler

Finally, load the provided layout:

g1_mujoco_sim/config/MPC_QP_layout.xml

By the end, you should understand the messages g1_msgs/SRBD_state.msg, g1_msgs/State.msg, and g1_msgs/Feet_reference.msg; the topics /srbd_current, /mpc_solution, /feet_ref_pos, and /wbid_statistics; and how to read wbid_solve_time when the WBID QP starts becoming too slow for the simulation loop.

Series roadmap

The G1 MuJoCo: Control, Foxglove and PlotJuggler series has six parts:

If you are new to MuJoCo, read Getting Started with MuJoCo. If your longer-term interest is G1 policy sim-to-real, Running GR00T-VisualSim2Real on G1 gives a wider context.

What runs under the launch file?

mpc_wbid_simulation.launch does more than open a viewer. It starts the simulation node ros_run_simulation.py, starts mpc_to_wbid_node, loads robot_description from g1_description/g1_23dof.urdf, runs robot_state_publisher at 250 Hz, and opens RViz with an SRBD/full-body configuration. That gives you three main data streams:

MuJoCo state
  -> current SRBD state publisher
  -> /srbd_current

MPC node
  -> predicted SRBD horizon and contact plan
  -> /mpc_solution

WBID loop
  -> QP inverse dynamics, torque command, solve time
  -> /wbid_statistics/full

The MPC layer does not directly command every joint. It optimizes at the SRBD level: torso orientation, center of mass, velocities, contact points, and contact forces. WBID receives the current state, the MPC solution, and foot references, then solves a whole-body QP to compute inverse-dynamics torques. In ros_run_simulation.py, the WBID loop calls WBID.stack.update(), WBID.setReference(...), WBID.solveQP(), and then retrieves torques with WBID.getInverseDynamics(). Immediately afterward, it updates self.wbid_solve_time and publishes it through a pal_statistics registry named /wbid_statistics.

The beginner takeaway is simple: if the robot falls, MuJoCo is not automatically the culprit. The MPC horizon may already be poor, the contact schedule may be inconsistent, the foot reference may jump, or the WBID math may still be valid but too slow for the timestep. PlotJuggler helps you separate those cases.

Setup and run

The repository README recommends using the opensot Docker image from the g1-locomotion branch of hucebot/opensot_docker. Once you are inside the container, build the ROS packages in the g1_locomotion workspace, checkout the walking-demo branch if you are following the README exactly, and source the workspace:

cd g1_locomotion
git checkout walking-demo
cd g1_mpc && git checkout walking-demo
cd .. && make all
cd ../../ && source setup.bash

If you already have your own workspace, the minimum check is that ROS can see g1_mujoco_sim, g1_msgs, g1_description, g1_mpc, pal_statistics, and PlotJuggler:

rospack find g1_mujoco_sim
rospack find g1_msgs
rosmsg show g1_msgs/SRBD_state
which plotjuggler || true

If rosrun plotjuggler plotjuggler is missing, your container may not include PlotJuggler. On ROS Noetic, this often means installing the PlotJuggler package and the ROS plugin package. In a locked Docker environment, prefer the repository's Dockerfile and dependency setup first, because ROS, XBot/OpenSoT, and MuJoCo bindings need to match.

Start the simulation in terminal 1:

source setup.bash
roslaunch g1_mujoco_sim mpc_wbid_simulation.launch

Wait for RViz and the MuJoCo viewer to appear. The README says the straight-line walking experiment runs for a few steps and then stops. In terminal 2, inside the same container and with the workspace sourced:

source setup.bash
rosrun plotjuggler plotjuggler

Load the layout in PlotJuggler:

File -> Load layout
g1_mujoco_sim/config/MPC_QP_layout.xml

If the panels appear but no curves update, open the ROS topic streaming data source in PlotJuggler, select the topics used by the layout, and start streaming. The XML layout stores this selected-topic list:

/feet_ref_pos
/mpc_solution
/mpc_statistics/full
/srbd_current
/wbid_statistics/full

You can also check the topics from a terminal:

rostopic list | grep -E "srbd|mpc|feet|wbid"
rostopic hz /srbd_current
rostopic hz /mpc_solution
rostopic hz /wbid_statistics/full

If rostopic hz does not receive messages, PlotJuggler is not the primary issue. Check the launch file, sourced workspace, ROS master, and MPC/WBID nodes first.

The three messages to understand

The central message is g1_msgs/SRBD_state.msg:

std_msgs/Header header
State[] states_horizon
ContactPoint[] contacts
geometry_msgs/Point landing_position

It contains an SRBD state horizon, a list of contact points, and the landing position of the swing leg. The same type is used for /srbd_current and /mpc_solution, but the meanings differ:

State.msg is the element type inside states_horizon:

int32 trajectory_index
geometry_msgs/Vector3 orientation
geometry_msgs/Vector3 position
geometry_msgs/Vector3 angular_velocity
geometry_msgs/Vector3 linear_velocity
float32 gravity

orientation is torso roll, pitch, and yaw in radians. position is CoM x/y/z in meters. angular_velocity is torso angular velocity in rad/s. linear_velocity is CoM linear velocity in m/s. Most curves in the States tab come from these fields.

Feet_reference.msg is shorter:

std_msgs/Header header
ContactPoint[] feet_positions

Each ContactPoint has name, position, force, and active. For /feet_ref_pos, the most important values are feet_positions[i]/position/*. They tell you where the planner or WBID layer expects the foot to be. If the foot reference jumps, the robot may kick through the air or land in the wrong place even when the CoM plots look reasonable.

What is inside MPC_QP_layout.xml?

The provided layout is not cosmetic. It encodes a practical debugging workflow. Its main tabs are:

In the States tab, the layout uses states_horizon[1] for /mpc_solution and states_horizon[0] for /srbd_current. This is practical: the current state only needs the current element, while the MPC curve uses the next horizon element that WBID is about to chase. If you change horizon indexing in your MPC node, update the layout to match your code.

A common mistake is to look at one curve and immediately blame the controller. In MPC/WBID, read signals in pairs:

CoM position desired/current
CoM velocity desired/current
Torso orientation desired/current
Torso angular velocity desired/current
Contact active planned/current
Foot reference/current contact position
Solve time vs control timestep

If only CoM x has some error while contact and foot reference still make sense, the MPC may simply be allowing transient drift during a swing phase. If contact active is wrong, the CoM error is probably a consequence. If solve time rises before the fall, timing or QP conditioning may be the root cause rather than the reference.

Reading the States tab

Start with States, because it is the easiest tab to understand. You will see position, linear velocity, orientation, and angular velocity. For a straight-line humanoid walking test, the rough expectations are:

When you are starting out, do not keep every curve visible. Select a 5 to 10 second window, zoom into the segment where the robot begins to lose stability, and compare /mpc_solution/states_horizon[1]/position/x with /srbd_current/states_horizon[0]/position/x. If the MPC curve is smooth but the current state does not follow, investigate WBID, contact, or torque saturation. If the MPC solution already spikes, investigate MPC cost, constraints, state estimates, or references.

A simple technique is to use PlotJuggler's vertical tracker across plots. When CoM y starts drifting, immediately inspect contact active at the same timestamp. If a contact phase changes at exactly that moment but force z does not rise, the planner may be treating a foot as supporting before the simulation contact actually supports weight.

Reading contact force and contact active

The Contact Forces tab plots /mpc_solution/contacts[0..3]/force/x/y/z. The repository uses four contact points for two feet. In the simulation code, the left-foot contact task is active if contact point 0 or 1 is active, and the right-foot task is active if contact point 2 or 3 is active. Do not read every point as a separate foot. Read them in pairs:

Left foot  = contacts[0], contacts[1]
Right foot = contacts[2], contacts[3]

During double support, you generally expect vertical force to be distributed across both feet, depending on CoM location. During single support, the stance foot should carry most of the load. If contact active says the right foot is in stance but right-foot force z is close to zero, contact state, frames, or constraints may be inconsistent.

The Contact Active tab plots both planned active contact from /mpc_solution and current active contact from /srbd_current. This is worth checking before tuning gains. If the schedule is wrong, increasing PD gains or changing QP weights only hides the symptom.

For beginners, the first target is not a pretty plot. The target is for contact active and contact force to tell the same story. If active is true but force remains zero for too long, or active is false while force z is large, debug contact before touching MPC weights.

Reading /feet_ref_pos

/feet_ref_pos publishes Feet_reference, where feet_positions is a list of ContactPoint values. The default layout plots feet_positions[0]/position/x/y/z together with current contact position. This is where swing-foot bugs become obvious.

A healthy foot reference usually has three properties:

1. x/y changes smoothly from step to step, without an unreasonable jump.
2. z rises during swing and returns near the ground during landing.
3. the landing position is consistent with the direction of CoM travel.

If z does not lift while contact active turns off, the foot may drag. If z rises too high or lands too late, WBID has to create an aggressive leg motion, which can increase QP solve time or ask for more torque than the model can deliver. If x/y jumps in one large step, inspect the gait procedure, landing position, and coordinate units. In robotics, many "unstable controller" bugs are actually discontinuous-reference bugs.

You can add these curves manually in PlotJuggler:

/feet_ref_pos/feet_positions[0]/position/x
/feet_ref_pos/feet_positions[0]/position/y
/feet_ref_pos/feet_positions[0]/position/z
/srbd_current/contacts[0]/position/x
/srbd_current/contacts[0]/position/y
/srbd_current/contacts[0]/position/z

Then place them in the same plot to inspect tracking. If index 0 is not the foot you care about in your code version, run rostopic echo /feet_ref_pos -n 1 and check the name field before editing the layout.

wbid_solve_time: early warning for a slow QP

In ros_run_simulation.py, WBID registers a statistic:

self.registry = StatisticsRegistry("/wbid_statistics")
self.wbid_solve_time = 0.0
self.registry.registerFunction("wbid_solve_time", (lambda: self.wbid_solve_time))

After each sim_step, the code calls WBID.solveQP(), reads inverse dynamics, updates self.wbid_solve_time, and calls self.registry.publish(). The layout reads the first statistic value:

/wbid_statistics/full/statistics[0]/value

In the Solving Times tab, it is plotted together with an MPC statistic:

/mpc_statistics/full/statistics[4]/value
/wbid_statistics/full/statistics[0]/value

You do not need to understand the full pal_statistics schema before using this plot. First, confirm the statistic name:

rostopic echo /wbid_statistics/full -n 1

Look for an entry named close to wbid_solve_time, then inspect its value. If your layout is still aligned with the source code, /wbid_statistics/full/statistics[0]/value is the WBID QP solve time.

Use it pragmatically:

Do not look only at the mean. For a walking controller, a spike at the exact contact transition can be more damaging than a good average. Zoom into the moment when Contact Active changes and check whether solve time spikes at the same timestamp. If it does, you have a concrete debugging path: the contact transition is making the QP difficult.

Checklist when the layout has no data

If PlotJuggler opens but curves do not update, follow this checklist:

# 1. Is ROS master alive?
rostopic list

# 2. Is the workspace sourced?
rospack find g1_mujoco_sim
rospack find g1_msgs

# 3. Are the simulation topics being published?
rostopic list | grep -E "srbd_current|mpc_solution|feet_ref_pos|wbid_statistics"

# 4. Do messages actually arrive?
rostopic echo /srbd_current -n 1
rostopic echo /wbid_statistics/full -n 1

# 5. Are the rates reasonable?
rostopic hz /srbd_current
rostopic hz /wbid_statistics/full

If /srbd_current has data but PlotJuggler does not, the problem is usually the ROS streaming plugin or an unsubscribed layout. If /mpc_solution has no data, check mpc_to_wbid_node. If /wbid_statistics/full has no data, check the pal_statistics dependency and the registry setup in the simulation node. If every topic has data but layout curves stay empty, the message paths in the XML may not match your current message version. Drag the topic fields from the data tree into a plot manually and compare the real field names.

A 10-minute debugging workflow

When the robot falls or the gait looks unstable, use this sequence:

1. Open States, zoom into the 2 seconds before the fall, and compare current CoM against MPC CoM.
2. Open Contact Active at the same timestamp and check whether planned and current contacts diverge.
3. Open Contact Forces and see whether stance-foot force z vanishes or spikes.
4. Open Contact Positions and compare foot reference against current contact position.
5. Open Solving Times and check whether wbid_solve_time spikes at the contact transition.
6. Return to the terminal and echo the suspicious topic to confirm it is not a layout-only issue.

After 10 minutes, you should usually be able to classify the bug:

This is why PlotJuggler is worth adding early. It forces controller debugging to be evidence-driven instead of viewer-driven.

Technical sources used

The technical details in this article were checked against the README and source files of ioloizou/g1_locomotion, especially g1_mujoco_sim/launch/mpc_wbid_simulation.launch, g1_mujoco_sim/config/MPC_QP_layout.xml, g1_mujoco_sim/src/ros_run_simulation.py, and the message definitions under g1_msgs/msg/. For PlotJuggler itself, prefer the ROS Index plotjuggler package page and the official PlotJuggler/plotjuggler repository, rather than unclear third-party domains.

Related Posts

• Bring Up G1 MuJoCo with Low-Level DDS
• Convert LAFAN1 G1 Motions to Foxglove MCAP
• WBID and PD for G1 in MuJoCo