[WBC 02] - Representing Floating-Base Robots in Code
How floating-base WBC code represents q, v, actuator torque, generalized vector packing, Eigen matrix products, and solver-to-command boundaries.
WBC02. Representing Floating-Base Robots in Code
In the previous post, we looked at Whole-Body Control as a software architecture problem.
WBC is not just a torque calculator. It is the layer that connects robot dynamics, contacts, task hierarchy, actuator limits, and hardware commands.
Before building that layer, we need to answer a simpler question:
How should a floating-base robot be represented in code?
This sounds like an implementation detail.
It is not.
Many WBC bugs start from a wrong mental model of the robot state.
For a fixed-base manipulator or a robot hand, it is often safe to think like this:
q // joint position
qdot // joint velocity
tau // joint torque
For an 8-DoF robot hand, this works.
nq = 8
nv = 8
na = 8
Configuration dimension, velocity dimension, and actuator dimension are all the same. The code can store q, qdot, and tau using the same joint ordering.
But once a floating base enters the system, this simple picture breaks.
1. Fixed-base robots hide the problem
A fixed-base robot has its base rigidly attached to the world.
A typical industrial manipulator bolted to a table is a fixed-base system. A robot hand attached to a wrist can also be treated as fixed-base if the wrist frame is considered fixed for the control problem.
In this case, the robot motion is described only by the actuated joints:
q = [joint_positions]
v = [joint_velocities]
So a simple state structure can work:
struct RobotState {
Vector q;
Vector qdot;
Vector tau;
};
The generalized coordinates are essentially joint coordinates. The generalized velocity is essentially joint velocity. The torque vector has the same dimension.
This is why fixed-base control code can make the q, qdot, and tau story look deceptively simple.
Floating-base robots do not have that luxury.
A humanoid, quadruped, or floating hand-arm system has a base that moves in space. The robot state must include both the base motion and the actuated joint motion:
q = [base_pose, joint_positions]
v = [base_velocity, joint_velocities]
The floating base has 6-DoF rigid body motion:
translation: 3 DoF
rotation: 3 DoF
total: 6 DoF
Those six degrees of freedom are part of the robot dynamics.
But they are not directly actuated.
2. Why humanoids are underactuated
A natural question is:
Humanoids usually have actuators at almost every joint. So why do we call them underactuated?
The answer is that underactuation here is not about whether the robot has many joint motors.
It is about whether every generalized degree of freedom in the dynamics can be directly actuated.
A humanoid may have actuators at the hip, knee, ankle, shoulder, elbow, wrist, and neck. But the floating base itself is not directly actuated. There is no motor between the torso and the world that can directly command the base position or orientation.
So the robot can be highly actuated at the joint level, while still being underactuated at the full-system level.
For a humanoid with na actuated joints:
v = [base_twist, joint_velocities]
The generalized velocity dimension is:
nv = 6 + na
But the actuator torque vector only contains joint torques:
tau.size() = na
The missing six dimensions are the floating-base degrees of freedom. They are part of the dynamics, but they are not directly commanded.
This is why the actuation term has the structure:
Sᵀ tau = [0_base_6, tau_actuated]
A compact floating-base dynamics equation is:
M(q) qddot + h(q, v) = Sᵀ tau + Jᵀ lambda
Here, Sᵀ tau injects torque only into the actuated joint equations. The floating-base part receives no direct actuator torque.
The base is controlled indirectly.
For a standing humanoid, joint torques pass through the leg kinematic chains, create ground reaction forces at the feet, and those contact forces affect the motion of the floating base:
joint torques
→ leg kinematic chains
→ foot contact forces
→ whole-body dynamics
→ floating-base motion
This is why contacts are central in humanoid WBC. Without support contacts, the floating base cannot be controlled in the same way. With support contacts, the robot uses the environment as a physical pathway to regulate base motion, center of mass behavior, and balance.
So the key point is:
A humanoid is not underactuated because it lacks joint motors. It is underactuated because the floating base belongs to the dynamics but not to the actuator space.
3. Separate nq, nv, and na
The first rule of floating-base code is to separate nq, nv, and na.
nq = configuration dimension
nv = velocity / acceleration dimension
na = actuator dimension
For fixed-base robots, these are often the same:
nq = nv = na
For floating-base robots, they are usually different.
The main reason is orientation.
A free-flyer base is often stored as position plus quaternion:
base configuration = position 3 + quaternion 4 = 7
But base velocity is represented as a 6D twist:
base velocity = linear velocity 3 + angular velocity 3 = 6
So for a floating-base robot with 8 actuated joints:
nq = 7 + 8 = 15
nv = 6 + 8 = 14
na = 8
The consistency checks are:
q.size() == nq
v.size() == nv
qddot.size() == nv
tau.size() == na
This is one of the first places floating-base code goes wrong.
In fixed-base code, it is common to write qdot and assume that it has the same dimension as q.
For floating-base robots, that assumption is dangerous.
The vector v is not simply the Euclidean derivative of the stored configuration vector q. Quaternion orientation is stored using four numbers, but angular velocity is a 3D vector.
That is why dynamics libraries such as Pinocchio use v for generalized velocity instead of treating velocity as raw qdot.
This distinction affects:
state storage
numerical integration
pose error computation
Jacobian dimensions
mass matrix dimensions
solver variable dimensions
command conversion
In WBC software, this is not a naming detail. It is a structural requirement.
4. Quaternion is storage. Lie group operations are computation.
Floating-base orientation needs a representation.
Common choices include:
Euler angles 3D, but singularity-prone
Quaternion 4D, with unit-norm constraint
Rotation matrix 9D, with orthogonality constraint
Lie algebra 3D, local representation
A common question is:
If we use Lie group operations, do we still need quaternions?
Yes.
They have different roles.
Quaternion:
storage representation for orientation
Lie group operations:
integration, difference, and local error computation
Twist:
tangent-space velocity representation
A quaternion is useful for storing global orientation. It avoids Euler-angle singularities and is more compact than a rotation matrix.
But a quaternion should not be treated like an ordinary 4D vector during control.
For example, this is usually wrong:
orientation_error = quat_des.coeffs() - quat.coeffs();
q_next = q + v * dt;
The first line computes an error in quaternion coefficient space, not a meaningful 3D orientation error.
The second line treats the configuration as if it lived in a flat Euclidean vector space.
For floating-base configurations, use manifold-aware operations:
dq = difference(model, q_current, q_desired);
q_next = integrate(model, q_current, v * dt);
The key idea is:
q lives on the configuration manifold.
v lives in the tangent space.
For a base orientation task, the error should be a 3D local angular error, not a 4D quaternion difference:
R_err = R_des.inverse() * R_current;
e_R = log(R_err);
The convention may vary, but the principle is the same:
Controller errors should live in the same tangent space as the velocities and accelerations used by the solver.
5. Keep the spaces separate
Floating-base WBC operates across multiple spaces:
configuration space:
q ∈ R^nq
generalized velocity / acceleration space:
v, qddot ∈ R^nv
actuator space:
tau ∈ R^na
contact force space:
lambda ∈ R^nc
These spaces are related, but they are not interchangeable.
For example, the solver may compute:
qddot_sol // size nv
lambda_sol // contact force dimension
tau_sol // size na
The acceleration solution includes both base acceleration and joint acceleration. But only the joint torque part can be sent to actuators.
The base acceleration is not directly commanded. It is produced indirectly by actuator torques and contact forces.
This also means contact is not optional in floating-base WBC. If the contact model is wrong, the base dynamics are wrong. If the contact force is infeasible, the desired motion is not physically realizable.
A useful mental model is:
generalized dynamics space:
where the equations are solved
actuator space:
where motor commands exist
contact force space:
where the robot interacts with the environment
Confusing these spaces leads to one of the most common mistakes in floating-base control: treating the humanoid as if it were just a fixed-base manipulator with more joints.
It is not.
The base is not fixed. The base is not directly actuated. The base is controlled through the body and contacts.
6. Make generalized vector packing explicit
The API can expose generalized vectors directly, or it can hide the packing behind semantic inputs. Both designs can work, but the boundary has to be clear.
A low-level API may ask the caller to construct q and v directly:
q.head<7>() = base_pose_as_xyz_quaternion;
q.tail(na) = joint_position;
v.head<6>() = base_twist;
v.tail(na) = joint_velocity;
In that case, the contract must be explicit:
Is the base pose first or last?
Is quaternion ordering xyzw or wxyz?
Is the base velocity spatial or body-frame?
Is qdot the same thing as v?
Is tau dimension nq, nv, or na?
For a higher-level controller API, another option is to accept semantic state components and let RobotSystem pack the generalized vectors internally.
For example:
struct FloatingBaseState {
pinocchio::SE3 pose_world_base;
pinocchio::Motion twist_world_base;
};
Then the public API can separate fixed-base and floating-base updates:
void updateState(Vector q_actuated,
Vector qdot_actuated,
Vector tau);
void updateState(Vector q_actuated,
Vector qdot_actuated,
Vector tau,
const FloatingBaseState& floating_base);
For a fixed-base robot:
robot.updateState(q_actuated, qdot_actuated, tau);
For a floating-base robot:
robot.updateState(q_actuated, qdot_actuated, tau, floating_base);
In the semantic-input version, RobotSystem constructs q and v using the dynamics library convention:
// fixed-base
q = q_actuated;
v = qdot_actuated;
// floating-base
q = [base_position, base_quaternion, q_actuated];
v = [base_twist, qdot_actuated];
The responsibility split is:
User:
provides joint state and floating-base state semantically.
RobotSystem:
converts semantic state into q and v.
Controller:
uses q, v, dynamics terms, tasks, and contacts.
Internally, the canonical state should match the dynamics library:
struct RobotState {
Vector q; // size nq
Vector v; // size nv
Vector tau; // size na
};
This makes the dynamics code clear:
M(q) // nv x nv
h(q, v) // nv
qddot // nv
tau // na
The WBC solver usually operates in nv space:
decision variable = [qddot, lambda]
qddot ∈ R^nv
lambda ∈ R^contact_force_dim
So this is wrong for floating-base systems:
qddot.size() == q.size()
The correct check is:
qddot.size() == nv
Dimension checks are not optional housekeeping. In WBC code, they are part of the safety structure.
7. Solver results are not actuator commands
Another common source of confusion is the relationship between solver outputs and hardware commands.
An inverse-dynamics WBC solver may produce:
qddot_sol
lambda_sol
tau_sol
But the hardware interface may expect:
struct RobotCommand {
Vector q;
Vector qdot;
Vector tau;
Vector kp;
Vector kd;
};
These command fields are not the same as solver variables.
For example, qddot_sol may need to be integrated:
qdot_cmd = qdot_current + qddot_sol * dt;
q_cmd = integrate(model, q_current, qdot_cmd * dt);
Then the command adapter may apply:
velocity clamp
position clamp
torque clamp
rate limit
driver-specific scaling
The solver operates in dynamics-level quantities:
qddot_sol
lambda_sol
tau_sol
The command adapter produces hardware-level quantities:
q_cmd
qdot_cmd
tau_ff_cmd
kp_cmd
kd_cmd
Mixing these layers creates ambiguity.
If a variable is named q_cmd, it should mean the command being sent toward the hardware interface. It should not mean desired task position, raw solver output, or unclamped integrated state.
A useful convention is:
_des:
desired value from planner, task, or reference generator
_sol:
solver result
_cmd:
command sent toward the hardware interface
The exact naming convention can vary, but the semantic distinction should exist.
Solver results are not automatically actuator commands.
8. Dimensions show up in matrix multiplication
The easiest way to see why these distinctions matter is to write the matrix products that the controller actually evaluates.
For a floating-base inverse dynamics problem, the equation has the form:
M(q) qddot + h(q, v) = S^T tau + J_c(q)^T lambda
The dimensions are:
q in R^nq
v in R^nv
qddot in R^nv
tau in R^na
lambda in R^nc
M in R^(nv x nv)
h in R^nv
S in R^(na x nv)
J_c in R^(nc x nv)
So the multiplication is:
(nv x nv)(nv) + (nv)
= (nv x na)(na) + (nv x nc)(nc)
This one line already tells us several implementation details. qddot must live in nv space. tau must live in actuator space. A contact Jacobian has nv columns because it maps generalized velocity to contact velocity. S^T maps actuator torque into generalized force space.
With Eigen, these dimensions become the row and column sizes of Matrix and Vector.
using Vector = Eigen::VectorXd;
using Matrix = Eigen::MatrixXd;
const int nq = model.nq;
const int nv = model.nv;
const int na = robot.numActuatedDofs();
const int nc = contact.forceDim();
Vector q(nq);
Vector v(nv);
Vector qddot(nv);
Vector tau(na);
Vector lambda(nc);
Matrix M(nv, nv);
Vector h(nv);
Matrix S(na, nv);
Matrix Jc(nc, nv);
Then Eigen computes each product by matching the inner dimension:
Vector generalized_inertia(nv);
generalized_inertia.noalias() = M * qddot;
// (nv x nv) * (nv x 1) -> (nv x 1)
Vector generalized_actuation(nv);
generalized_actuation.noalias() = S.transpose() * tau;
// (nv x na) * (na x 1) -> (nv x 1)
Vector generalized_contact(nv);
generalized_contact.noalias() = Jc.transpose() * lambda;
// (nv x nc) * (nc x 1) -> (nv x 1)
Vector residual =
generalized_inertia + h
- generalized_actuation
- generalized_contact;
The staged version is intentionally verbose. The same computation could be written in one expression:
Vector residual =
M * qddot + h
- S.transpose() * tau
- Jc.transpose() * lambda;
Eigen represents the right-hand side as an expression and evaluates it when assigning into residual. Breaking the expression into named terms makes the vector spaces easier to inspect, and noalias() tells Eigen that the destination does not overlap with the product inputs.
This is not just notation. Eigen checks matrix product compatibility through the product dimensions.
For example:
Vector tau_wrong(nv);
Vector generalized_actuation =
S.transpose() * tau_wrong;
// (nv x na) * (nv x 1)
// invalid unless na == nv
With dynamic-size Eigen types such as MatrixXd and VectorXd, this is checked at runtime in debug builds. With fixed-size Eigen types, incompatible products can be rejected at compile time. Most robot models use dynamic sizes, so it is still useful to add explicit API checks:
assert(q.size() == nq);
assert(v.size() == nv);
assert(qddot.size() == nv);
assert(tau.size() == na);
assert(lambda.size() == nc);
assert(M.rows() == nv && M.cols() == nv);
assert(S.rows() == na && S.cols() == nv);
assert(Jc.rows() == nc && Jc.cols() == nv);
If tau is accidentally allocated with size nv, this product should fail a dimension check for a floating-base robot where nv != na. If qddot is allocated with q.size(), the bug may only appear when the robot becomes floating-base, because nq and nv are no longer the same.
Task equations have the same structure. A task Jacobian maps generalized velocity to task velocity:
xdot = J_task(q) v
For acceleration-level WBC, the task objective usually contains:
J_task(q) qddot + Jdot_v = xddot_cmd
The dimensions are:
(task_dim x nv)(nv) + (task_dim) = (task_dim)
In code:
Matrix Jtask(task_dim, nv);
Vector Jdot_v(task_dim);
Vector xddot_cmd(task_dim);
Vector task_error = Jtask * qddot + Jdot_v - xddot_cmd;
Again, the matrix multiplication makes the space explicit. The task does not multiply by qdot with size nq; it multiplies by generalized velocity or acceleration in tangent space with size nv.
This is also why floating-base integration should use the dynamics library operation instead of raw vector addition:
Vector v_next = v + qddot_sol * dt;
Vector q_next = pinocchio::integrate(model, q, v_next * dt);
The solver result is still not automatically the hardware command. A command adapter may extract the actuated part and apply command policy:
struct IDSolution {
Vector qddot_sol; // nv
Vector lambda_sol; // nc
Vector tau_sol; // na
};
struct RobotCommand {
Vector q; // na
Vector qdot; // na
Vector tau; // na
Vector kp; // na
Vector kd; // na
};
RobotCommand makeCommand(const RobotState& state,
const IDSolution& sol,
double dt) {
Vector v_next = state.v + sol.qddot_sol * dt;
Vector q_next = pinocchio::integrate(model, state.q, v_next * dt);
RobotCommand cmd;
cmd.q = actuatedPosition(model, q_next);
cmd.qdot = actuatedVelocity(model, v_next);
cmd.tau = sol.tau_sol;
return cmd;
}
The important point is not the exact helper function names. The important point is that the matrix products expose the vector spaces, and the code should preserve those spaces instead of treating every quantity as just another Vector.
9. Summary
Floating-base robot representation is the first hard problem in WBC software.
For fixed-base robots, it is often acceptable to think:
q = joint position
qdot = joint velocity
tau = joint torque
For floating-base robots, this is not enough.
The configuration contains base pose and actuated joint positions:
q = [base_pose, joint_positions]
The generalized velocity contains base twist and actuated joint velocities:
v = [base_twist, joint_velocities]
The actuator torque contains only actuated joint torques:
tau = [joint_torques]
Therefore:
q.size() == nq
v.size() == nv
qddot.size() == nv
tau.size() == na
For a floating-base robot with quaternion base orientation:
nq = 7 + na
nv = 6 + na
tau.size() = na
This is why floating-base robots are underactuated in the WBC formulation.
The mechanism may have actuators at nearly every joint, but the full dynamical system also includes the floating base. That base is not directly actuated. It is controlled indirectly through joint torques, kinematic chains, support contacts, and whole-body dynamics.
A good RobotSystem API should prevent configuration space, tangent space, actuator space, and contact force space from being casually mixed.
That separation is not just code style.
It is the foundation that makes the rest of WBC possible.
In the next post, we move one layer up and look at how a pose target becomes a task-space acceleration constraint.