DEV Community

Richard Minsoo Go
Richard Minsoo Go

Posted on

Quadruped Robot Software — Ch.06: Whole Body Control for Quadrupeds [EN]

STATERA Library · Series 08 · Volume 21 · Chapter 06 · EN
Source: https://github.com/Statera-Library/08_Vol_21_Quadruped_Robot_Software/blob/main/Chapter_06_Whole_Body_Control_for_Quadrupeds/Chapter_06_Whole_Body_Control_for_Quadrupeds_EN.md

Volume 21. Quadruped Robot Software

Chapter 06. Whole Body Control for Quadrupeds

06.01. WBC Framework for Quadruped Tasks and Priorities

Whole-Body Control (WBC) provides a unified framework for coordinating all actuated joints, floating-base motion, and contact forces of a quadruped robot. Instead of controlling each leg independently, WBC treats the robot as a coupled multibody system whose motions must simultaneously satisfy dynamic equations, contact constraints, actuator limits, and task objectives. This integrated formulation is essential for stable locomotion and physically consistent behavior.

A quadruped is naturally modeled as a floating-base system because its trunk is not rigidly attached to the environment. The generalized configuration therefore includes the six-degree-of-freedom pose of the base together with the joint coordinates of all legs. Since the floating base cannot be actuated directly, desired body motion must be produced indirectly through joint torques and reaction forces generated at feet that maintain contact with the terrain.

The foundation of WBC is the rigid-body equation of motion, commonly expressed as M(q)q̈ + h(q,q̇) = Sᵀτ + Jcᵀfc. Here, the mass matrix describes inertial coupling, h contains Coriolis, centrifugal, and gravitational effects, τ represents actuator torques, and fc represents contact forces. This equation connects desired accelerations and contact interactions to physically realizable joint commands.

Contact states fundamentally change the controllability of a quadruped. During stance, a foot is constrained by the terrain and can generate reaction forces that support and accelerate the body. During swing, the same foot becomes a motion task and must follow a desired trajectory without contributing ground force. WBC continuously reorganizes these constraints and objectives according to the contact schedule supplied by the gait or locomotion planner.

Tasks are mathematical descriptions of behaviors that the controller should achieve. A base task may regulate trunk position, height, velocity, roll, pitch, and yaw, while swing-foot tasks track trajectories toward future footholds. Additional tasks may control posture, center of mass, joint configuration, momentum, or manipulation devices. WBC combines these heterogeneous objectives within a common optimization or hierarchical control structure.

Task priority becomes important because not every objective can be satisfied exactly at the same time. Maintaining physically valid contact and respecting system dynamics generally have greater importance than accurately tracking a nominal posture. A controller may therefore organize objectives into levels such as dynamic feasibility, contact consistency, body stabilization, swing-foot tracking, and posture regulation. Lower-priority tasks use only the remaining control freedom.

Strict hierarchical WBC can enforce priorities mathematically through null-space projection or hierarchical optimization. Once a higher-priority task is satisfied, lower-level commands are projected into directions that do not interfere with that solution. This approach is valuable when safety-critical constraints must never be sacrificed for secondary performance objectives, although numerical implementation becomes more demanding as the number of priority levels increases.

Another widely used approach formulates WBC as a quadratic program (QP). Desired task accelerations, contact forces, or tracking errors are represented through quadratic costs, while dynamics, contact conditions, friction limits, and actuator bounds appear as equality or inequality constraints. Different task weights establish soft priorities. The QP then finds the control variables that provide the best compromise while remaining within the feasible physical region.

Contact-force optimization is particularly important for quadrupeds because ground reaction forces determine how the floating body is supported and accelerated. The controller distributes forces among stance feet according to desired body motion while preventing unrealistic solutions. Normal forces must remain compatible with unilateral contact, meaning that a foot may push against the ground but cannot pull the ground toward the robot.

Tangential contact forces must also remain inside an admissible friction region. Coulomb friction is commonly approximated using a linearized friction cone or friction pyramid so that it can be incorporated efficiently into a QP. These constraints reduce slipping by ensuring that horizontal forces remain compatible with available normal force and assumed terrain friction. Contact-force limits may additionally account for foot geometry and hardware capabilities.

Swing-leg control introduces a different objective. The controller receives a desired foot trajectory from the locomotion planner and converts its tracking requirements into joint-level behavior through the foot Jacobian and robot dynamics. Position, velocity, and acceleration references can be combined with feedback terms. Because the body itself moves during locomotion, accurate swing tracking requires considering the coupled motion of both the floating base and leg joints.

Body stabilization usually occupies a high level within the task hierarchy. Desired trunk orientation and translational motion may originate from a velocity command, gait generator, model predictive controller, or navigation system. WBC translates these references into dynamically consistent accelerations, forces, and torques. In this sense, it acts as the bridge between higher-level locomotion planning and the robot\'s low-level torque-controlled actuators.

Posture regulation is commonly assigned a lower priority because its role is to shape internal joint configurations rather than dominate locomotion. A nominal posture can keep joints away from mechanical limits, improve manipulability, reduce awkward leg configurations, and maintain useful clearance. When higher-priority tasks require different joint motions, posture tracking is intentionally relaxed so that stability and contact objectives remain achievable.

Physical limits must be included explicitly in practical WBC. Joint torque saturation, joint position and velocity limits, maximum contact forces, friction constraints, and allowable accelerations define the feasible operating envelope. Ignoring these limits can produce mathematically attractive but physically impossible commands. Constraint-aware optimization allows the controller to degrade tracking performance gracefully rather than demanding actions that the actuators cannot execute.

The relationship between WBC and Model Predictive Control (MPC) is complementary rather than competitive. MPC can predict body motion and optimize future contact forces or foothold-related behavior over a finite horizon, while WBC operates at a faster rate to realize the immediate reference using the full robot model. MPC therefore answers what global motion should occur, whereas WBC determines how the complete articulated robot should execute that motion now.

State estimation is equally critical because WBC depends on reliable estimates of base pose, velocity, joint states, contact conditions, and sometimes terrain orientation. Errors in these quantities directly affect dynamic compensation and contact-force computation. Information from IMUs, joint encoders, foot-contact sensing, vision, LiDAR, or proprioceptive estimators can therefore influence control quality even when the WBC algorithm itself remains unchanged.

Contact transitions require special treatment because abrupt switching between stance and swing can create discontinuities in desired force or acceleration. Practical controllers introduce smooth force ramps, trajectory blending, touchdown detection, and compliant behavior near contact events. These mechanisms reduce impact and prevent sudden torque changes while allowing the optimization problem to transition between different sets of constraints as the gait evolves.

Robustness also requires recognizing that the mathematical model is never exact. Link masses, inertias, actuator dynamics, terrain properties, and contact locations contain uncertainty. Feedback terms compensate for moderate modeling errors, while force sensing, disturbance estimation, adaptive control, or learned residual models may further improve performance. WBC should therefore be viewed as a model-based framework operating continuously with measured state feedback rather than as pure feedforward dynamics.

At the actuator interface, optimized generalized behavior must ultimately become commands such as joint torque, position, velocity, or combinations of these quantities. High-performance quadrupeds often use torque-oriented control because it provides direct access to dynamic interaction with the environment. Nevertheless, impedance-controlled actuators can also realize WBC outputs by combining feedforward torque with joint-space stiffness and damping for additional local robustness.

A complete quadruped WBC architecture therefore forms a real-time hierarchy connecting perception, state estimation, gait planning, trajectory generation, dynamic optimization, and actuator control. Higher layers specify where and how the robot should move, while WBC reconciles these intentions with instantaneous contact conditions and physical constraints. This separation allows locomotion strategies to change without redesigning the entire low-level control system.

The central design principle is that quadruped behavior should not be interpreted as twelve independent joint-control problems. Every body acceleration, foot placement, and ground reaction force affects the dynamics of the complete robot. Whole-Body Control exploits these couplings explicitly, resolving competing objectives according to their priorities while preserving feasibility, balance, contact consistency, and actuator safety during dynamically changing locomotion tasks.

06.02. Task Hierarchy Locomotion Payload Orientation [w/Code]

Task hierarchy in quadruped Whole-Body Control (WBC) determines how multiple objectives compete for the robot's limited dynamic and kinematic capabilities. Locomotion, payload stabilization, body orientation, contact maintenance, and posture regulation cannot always be satisfied simultaneously. A hierarchy assigns relative importance so that essential physical constraints remain protected while secondary objectives use the remaining control authority.

The highest level of a practical hierarchy is normally associated with physical feasibility and safety. The equations of motion, stance-foot contact conditions, unilateral ground reaction forces, friction limits, and actuator constraints define what the robot can physically execute. These requirements are not merely desirable tracking objectives. They establish the feasible solution space within which locomotion and payload-related tasks must operate.

Contact consistency is particularly important because stance feet create the mechanical connection between the floating-base robot and the environment. A commanded body acceleration is meaningful only if compatible ground reaction forces can be generated through the active contacts. Consequently, WBC must prevent solutions that require a stance foot to penetrate the terrain, pull on the ground, or generate tangential forces beyond the available friction region.

Locomotion objectives usually occupy the next major layer of the hierarchy. They describe desired translational velocity, body height, center-of-mass motion, yaw behavior, and progression toward navigation goals. These references may originate from a human command, autonomous navigation system, gait planner, or Model Predictive Control (MPC). WBC converts them into instantaneous whole-body accelerations, contact forces, and joint torques.

Locomotion should not be interpreted only as forward velocity tracking. A quadruped must coordinate base translation, angular motion, foothold placement, stance-force distribution, and swing-leg trajectories. When climbing slopes or traversing irregular terrain, maintaining a commanded velocity may conflict with stability or contact feasibility. The hierarchy therefore permits velocity tracking to be relaxed when necessary to preserve physically sustainable motion.

Body orientation is another central task because roll, pitch, and yaw influence balance, perception, payload behavior, and future foot placement. Roll and pitch references may be aligned with gravity or adapted to estimated terrain geometry, while yaw generally follows the desired travel direction. Orientation errors are commonly converted into desired angular accelerations or momentum changes that the WBC realizes through coordinated contact forces.

Orientation priorities depend strongly on the operating scenario. During ordinary walking, moderate roll and pitch deviations may be acceptable if they help the robot negotiate uneven terrain. For a robot carrying sensitive equipment, however, maintaining payload orientation may become more important than maintaining nominal trunk orientation. The controller must therefore distinguish between body attitude requirements and mission-dependent payload requirements.

Payload control introduces additional dynamic coupling because the carried mass changes the robot's total mass distribution, center of mass, inertia, and required support forces. A payload mounted rigidly on the trunk modifies the dynamics directly, whereas an articulated or suspended payload can introduce additional degrees of freedom and disturbances. WBC must incorporate these effects if accurate balance and motion control are required.

When payload mass is significant, locomotion references may need to become subordinate to payload stability. Rapid acceleration, aggressive turning, or large body rotations can produce undesirable inertial forces on the payload. A task hierarchy can therefore reduce commanded acceleration or modify body motion while maintaining the payload within specified orientation, acceleration, or force limits. Mission performance is preserved by explicitly representing these requirements rather than treating them as disturbances.

Payload orientation may be defined relative to gravity, the robot trunk, the terrain, or a task-specific reference frame. For example, a sensor package may need a stable horizontal orientation, while transported liquid may require limits on tilt and acceleration. A manipulator mounted on the quadruped may require its base to remain within a restricted orientation range to preserve reachability and manipulation accuracy.

The relationship between trunk orientation and payload orientation becomes especially important when an active stabilization mechanism is available. If the payload is mounted on a gimbal, manipulator, or actuated platform, the robot body can move while the payload remains approximately stabilized. WBC can exploit this additional redundancy by distributing orientation correction between the floating base and payload actuators according to task priority and available motion range.

Hierarchical control can implement these priorities using strict or soft formulations. In a strict hierarchy, a higher-priority task is solved first and lower-priority tasks are constrained to the null space remaining after that solution. This ensures that a locomotion or safety-critical orientation objective cannot be degraded by posture optimization. Such formulations provide clear priority semantics but may require several sequential optimization problems.

Soft hierarchy instead represents objectives through weighted costs within a common quadratic program (QP). Large weights can emphasize payload orientation or trunk stabilization, while smaller weights regulate posture and secondary tracking. This formulation is computationally convenient and allows smooth compromises, but weights must be chosen carefully because a sufficiently large lower-level error can potentially influence an intended higher-priority objective.

A hybrid hierarchy is often attractive for real quadrupeds. Hard constraints can protect rigid-body dynamics, contact feasibility, friction, joint limits, and torque limits, while high-weight objectives regulate balance and mission-critical payload behavior. Lower-weight costs can then handle velocity tracking, swing-foot accuracy, nominal posture, energy consumption, or torque regularization. This separates requirements that must never be violated from objectives that may be traded.

Swing-foot control illustrates how task priority changes dynamically during locomotion. During the swing phase, accurate foot positioning is necessary for successful future contact, but small trajectory deviations may be tolerated if joint limits or body stabilization require additional freedom. Near touchdown, vertical velocity and contact preparation may become more important than tracking every point of the nominal swing trajectory.

The hierarchy can also vary according to gait phase. During four-foot support, the robot possesses substantial contact redundancy and can simultaneously regulate body orientation and payload stability. During trot or dynamic transitions, fewer contacts are available and the feasible force distribution becomes more restricted. The controller may temporarily reduce lower-priority objectives to concentrate control authority on balance and contact consistency.

Terrain conditions provide another reason for adaptive priorities. On flat high-friction ground, velocity tracking and body-level orientation control can be relatively aggressive. On slippery, deformable, or steep terrain, contact-force feasibility becomes more restrictive. WBC can reduce tangential force demands, allow slower motion, modify trunk orientation, or increase posture compliance instead of forcing the original locomotion command under unsuitable conditions.

Task scaling provides a systematic mechanism for handling infeasible combinations of objectives. Rather than allowing the optimization problem to fail, the controller can reduce the magnitude of selected task commands until a feasible solution is recovered. For example, desired forward acceleration can be scaled while payload orientation remains tightly controlled. This produces graceful performance degradation instead of abrupt loss of control.

Redundancy resolution is fundamental to task hierarchy because quadrupeds frequently possess multiple ways to accomplish similar body behavior. Different combinations of joint accelerations and contact forces may generate nearly identical trunk motion. WBC uses this redundancy to satisfy secondary objectives such as maintaining comfortable joint configurations, reducing torque, balancing contact loads, avoiding singular configurations, or minimizing unnecessary motion.

Posture tasks usually occupy a lower level because they shape the internal configuration without directly defining the primary mission. Desired joint angles can keep legs near efficient operating regions and maintain sufficient workspace for future steps. However, if posture regulation conflicts with balance, foothold tracking, payload stabilization, or physical constraints, it should yield rather than forcing the robot toward a dynamically unfavorable configuration.

Joint-limit avoidance may require stronger treatment than ordinary posture regulation. As a joint approaches its mechanical boundary, the controller can introduce inequality constraints or rapidly increasing avoidance costs. This prevents a low-priority locomotion objective from driving the mechanism into an unusable configuration. Similar techniques can maintain clearance from self-collision or preserve minimum leg extension margins required for disturbance recovery.

Payload-aware force distribution can further improve stability. If a heavy payload shifts the combined center of mass toward one side of the robot, equal force distribution among the feet is no longer appropriate. WBC can allocate larger normal forces to contacts positioned beneath the shifted load while respecting friction and actuator limits. This enables the contact-force solution to reflect the actual mass distribution of the robot-payload system.

Dynamic payloads require even greater coordination. A moving manipulator, oscillating load, or changing cargo position creates time-varying inertial effects that can disturb locomotion. If these motions are modeled, WBC can anticipate their influence and compensate through body motion and contact-force redistribution. If they are not modeled explicitly, disturbance observers and feedback control must reject the resulting errors after they appear.

Orientation control should also account for the difference between geometric tracking and dynamic feasibility. Demanding perfectly level roll and pitch while crossing strongly inclined terrain may create poor leg configurations or excessive contact forces. A terrain-aware controller can generate an orientation reference that balances global vertical alignment, local surface geometry, foothold reachability, and payload requirements rather than imposing a single fixed attitude.

Higher-level planners can assist WBC by generating references that already respect approximate dynamic limitations. MPC may optimize future body motion and contact forces while considering payload mass and gait timing, whereas WBC resolves the detailed full-body response at higher frequency. This division reduces conflicts because the task hierarchy receives references that are more likely to remain feasible under current and predicted contact conditions.

State estimation directly affects hierarchy execution. Incorrect terrain orientation, payload mass, base attitude, or contact classification can cause the controller to prioritize objectives using an inaccurate representation of the physical system. Payload-aware quadrupeds may therefore require load estimation, center-of-mass identification, contact estimation, and reliable inertial sensing in addition to conventional joint-state measurements.

A well-designed task hierarchy is ultimately mission dependent rather than universal. A fast scouting robot may prioritize locomotion speed and disturbance recovery, while a logistics quadruped carrying fragile cargo may prioritize payload acceleration and orientation. A quadruped equipped with a manipulator may prioritize end-effector accuracy during manipulation and return locomotion tasks to higher importance when traveling between work locations.

The essential principle is to preserve a clear distinction between constraints, primary tasks, and secondary preferences. Physical feasibility and safety establish the boundaries of possible behavior; locomotion, payload, and orientation objectives determine mission behavior within those boundaries; posture and optimization criteria exploit remaining redundancy. By organizing these layers explicitly, WBC can adapt the entire quadruped to changing terrain, contact states, payload conditions, and operational priorities without losing dynamic consistency.

06.03. QP Based WBC Solver OSQP qpOASES [w/Code]

Quadratic Programming (QP) is one of the most practical mathematical foundations for Whole-Body Control (WBC) in quadruped robots. It allows body acceleration, joint motion, ground reaction forces, and actuator torques to be optimized together while explicitly enforcing physical constraints. The resulting controller can coordinate many competing objectives without separating locomotion into independent joint-level problems.

A standard convex QP minimizes an objective of the form 1/2 xᵀHx + gᵀx while satisfying equality and inequality constraints. In quadruped WBC, the decision vector x may contain generalized accelerations, joint torques, contact forces, task slack variables, or combinations of these quantities. The Hessian H and gradient g encode tracking objectives and regularization terms, while constraints describe physical feasibility.

One common formulation selects generalized acceleration q̈ and contact force fc as primary decision variables. Joint torques can then be recovered from the rigid-body dynamics after optimization. Another formulation includes q̈, fc, and τ simultaneously, allowing actuator limits to appear directly in the optimization. The appropriate choice depends on controller architecture, computational budget, actuator interface, and desired constraint structure.

The floating-base equations of motion provide fundamental equality constraints. Because the six-dimensional base has no direct actuator, its translational and rotational dynamics must be generated through contact forces. The QP therefore ensures that candidate accelerations and forces satisfy the robot model. This prevents the optimizer from requesting body motions that cannot be produced through the current stance contacts.

Stance-foot kinematics introduce additional equality constraints. A rigid stationary contact ideally satisfies Jc q̈ + J̇c q̇ = 0, preventing acceleration of the stance foot relative to the terrain. Depending on the contact model, selected directions may instead permit controlled compliance or sliding. These equations connect generalized robot acceleration to the instantaneous geometry of each active contact.

Swing-foot motion is normally represented as a tracking objective rather than a rigid constraint. Desired Cartesian acceleration can be generated from feedforward trajectory acceleration together with position and velocity feedback. The QP minimizes the difference between this desired acceleration and the acceleration predicted through the swing-foot Jacobian. Weighting determines how strongly trajectory tracking competes with other objectives.

Body position and orientation tasks can be constructed similarly. Desired linear and angular accelerations are calculated from reference trajectories and feedback errors, then mapped into generalized coordinates through task Jacobians. The optimizer attempts to reproduce these accelerations while remaining dynamically feasible. Consequently, perfect tracking may be relaxed automatically when contact geometry or actuator capability becomes restrictive.

Ground reaction forces require inequality constraints because feet can push against the ground but normally cannot pull it. Normal force is therefore constrained to remain nonnegative and may also have an upper limit. Tangential forces must remain inside the available friction region. These conditions are essential because unconstrained force optimization can produce mathematically effective but physically impossible contact solutions.

The nonlinear Coulomb friction cone is commonly approximated by a linear friction pyramid for real-time QP computation. Constraints such as |fx| ≤ μfz and |fy| ≤ μfz can be represented as linear inequalities, where μ is the assumed friction coefficient. Conservative values improve robustness against slipping, although excessive conservatism can unnecessarily reduce the robot\'s available locomotion capability.

Actuator limitations can also be represented directly. Joint torque bounds prevent the optimizer from exceeding motor, transmission, or thermal capability, while acceleration and velocity-related constraints can protect mechanical hardware. Joint-position limits may be handled through predictive bounds that restrict accelerations before a joint reaches its physical boundary. This makes constraint management part of control rather than an external saturation step.

QP objectives often combine several weighted terms. Body tracking, swing-foot tracking, desired contact-force tracking, posture regulation, torque minimization, acceleration regularization, and force smoothing can coexist in one cost function. Large weights emphasize important tasks, whereas smaller weights use residual freedom. Careful normalization is required because task variables may have very different physical units and numerical magnitudes.

Slack variables are useful when strict satisfaction of a task could make the QP infeasible. A constraint can be softened by introducing a controlled violation variable and penalizing it strongly in the objective. This allows the solver to return a physically meaningful degraded solution instead of failing completely. Safety-critical constraints, however, should generally remain hard unless a carefully designed fallback mechanism exists.

Numerical conditioning strongly influences real-time WBC performance. Poorly scaled matrices, nearly dependent constraints, extreme task weights, or ill-conditioned Jacobians can increase solver iterations and produce unstable solutions. Scaling forces, accelerations, torques, and task residuals to comparable numerical ranges improves robustness. Small regularization terms can also make the Hessian better conditioned and reduce ambiguity in redundant solutions.

OSQP is a widely used open-source solver for convex quadratic programs. It uses an operator-splitting approach based on the Alternating Direction Method of Multipliers (ADMM) and is designed to exploit sparse problem structure. WBC problems often contain sparse dynamics and constraint matrices, making OSQP attractive for robotics research, prototyping, and systems where reliable handling of structured QPs is important.

An important characteristic of OSQP is that it can solve convex QPs without requiring every iteration to perform the same operations as classical active-set methods. Its formulation handles equality and inequality constraints in a unified manner. Warm starting from the previous control cycle can substantially improve practical performance because consecutive WBC optimization problems usually differ only slightly during continuous locomotion.

OSQP also supports factorization reuse when the sparsity pattern remains unchanged. In a quadruped controller, the dimensions and matrix structure often remain similar across many control cycles, especially within the same contact phase. Updating numerical values while preserving symbolic structure reduces computational overhead. Contact switching still requires careful implementation because the active constraint structure can change with gait phase.

qpOASES represents a different solver philosophy and is well known for online and model-predictive optimization. It uses an active-set strategy designed to exploit the fact that sequential QPs often change gradually. The solver identifies constraints that are active at the optimum and updates this working set as parameters change. This behavior can be effective for control applications requiring repeated solutions of closely related QPs.

The online active-set strategy of qpOASES can provide very fast solutions when the optimal active set changes only modestly between control cycles. Warm-start information from the previous QP is therefore particularly valuable. However, computational time can vary when contact transitions, disturbances, or major reference changes cause substantial modifications to the active constraint set, which must be considered in hard real-time implementations.

Choosing between OSQP and qpOASES should therefore depend on the actual WBC formulation rather than solver popularity alone. OSQP is attractive for sparse convex problems and offers predictable iterative behavior with adjustable accuracy. qpOASES can be highly efficient for smaller dense or moderately sized sequential QPs with favorable active-set evolution. Benchmarking should use the robot\'s real matrix dimensions and contact transitions.

Solver accuracy must also be selected according to control requirements. Extremely tight numerical tolerances may consume unnecessary computation, whereas loose tolerances can create constraint violations or noisy torque commands. A WBC implementation should evaluate primal residuals, dual residuals, constraint margins, solution quality, and computation time together rather than judging a solver solely by whether it reports successful convergence.

Warm starting is especially effective because quadruped dynamics evolve continuously at high control frequency. The previous solution for acceleration, torque, and contact force is usually close to the next optimum. Reusing primal variables, dual variables, or active-set information can reduce computation significantly. At gait transitions, however, obsolete contact forces and multipliers should be reset or transformed appropriately.

Real-time implementation requires deterministic data handling around the solver. Robot state estimation, dynamics computation, Jacobian evaluation, QP assembly, numerical solution, and command transmission must all fit inside the control period. Dynamic memory allocation and unnecessary matrix reconstruction should be minimized. Preallocated sparse structures and fixed-size linear algebra can substantially reduce latency variation in high-frequency control loops.

The controller must also define behavior for solver failure. Maximum iteration limits, numerical errors, infeasibility, or delayed computation should never result in undefined actuator commands. A practical WBC system can retain a previous safe command briefly, switch to a simplified stabilizing controller, reduce task demands, increase slack, or initiate a controlled stop depending on the severity and duration of the failure.

Monitoring solver health is therefore part of robot safety. Useful runtime signals include solution status, iteration count, computation time, maximum constraint violation, friction margin, torque margin, and task residuals. Sudden changes can indicate contact-estimation errors, unrealistic planner commands, poor model parameters, or numerical conditioning problems before they become visible as severe physical instability.

Contact transitions are among the most demanding moments for a QP-based WBC solver. When a foot touches down or lifts off, contact Jacobians, force variables, and inequality constraints change. Smooth force ramping and consistent initialization prevent large discontinuities. Some implementations preserve fixed QP dimensions and activate or deactivate contacts through bounds, reducing structural changes and simplifying memory management.

Hierarchical behavior can be implemented inside QP-based WBC using weights, sequential QPs, or lexicographic optimization. A single weighted QP is computationally simple but does not guarantee strict task priority. Sequential QPs provide stronger priority semantics by preserving higher-level solutions before optimizing lower levels. The appropriate architecture depends on whether mission-critical objectives require mathematically strict separation.

QP-based WBC becomes particularly powerful when connected to Model Predictive Control (MPC). MPC can provide desired body trajectories and contact-force references over a future horizon, while the WBC QP enforces full-body dynamics and actuator constraints at the current instant. The QP effectively converts reduced-order planning decisions into feasible commands for the complete articulated quadruped.

A successful implementation is ultimately determined by the complete control pipeline rather than the solver alone. Accurate dynamics, reliable contact estimation, properly scaled tasks, physically meaningful constraints, warm-start strategies, bounded computation time, and robust fallback logic are all essential. OSQP or qpOASES provides the numerical optimization engine, but the quality of WBC depends on how carefully the physical control problem is formulated around that engine.

06.04. Swing and Stance Leg Task Management [w/Code]

Swing and stance leg task management is a central function of Whole-Body Control (WBC) because each quadruped leg repeatedly changes its physical role during locomotion. A stance leg supports and accelerates the floating body through ground reaction forces, whereas a swing leg moves toward a future foothold without intentionally supporting the robot. WBC must change these task definitions continuously while preserving dynamic consistency.

The gait scheduler provides the fundamental contact mode for each leg. Depending on the selected gait, such as walk, trot, pace, bound, or crawl, every foot receives planned liftoff and touchdown times. WBC uses this contact schedule to determine which feet are modeled as active contacts and which are treated as free-moving end effectors. Accurate synchronization between the gait generator and WBC is therefore essential.

During stance, the foot becomes part of the robot-environment constraint system. For an ideal rigid contact, its velocity relative to the terrain should remain approximately zero and its acceleration should satisfy the corresponding contact kinematics. The controller uses the stance-foot Jacobian to constrain generalized acceleration while optimizing contact forces that support desired body motion and maintain balance.

Ground reaction force is the primary control mechanism available to a stance leg. By coordinating normal and tangential forces across all supporting feet, WBC can regulate center-of-mass acceleration, trunk orientation, angular momentum, and disturbance rejection. The force assigned to each foot must remain compatible with friction, unilateral contact, actuator capability, and the geometry of the current support configuration.

Stance-force distribution should not necessarily be equal among all supporting legs. The required distribution depends on the center-of-mass position, desired acceleration, terrain geometry, payload, and number of active contacts. A foot positioned near the projected load may carry more normal force, while another contact may contribute more strongly to yaw or lateral stabilization. WBC resolves this distribution as a coupled optimization problem.

The swing phase begins when a foot is intentionally unloaded and released from its contact constraints. Its role changes from force generation to motion tracking. A swing trajectory typically moves the foot from the previous foothold toward a planned landing location while maintaining sufficient terrain clearance. Position, velocity, and acceleration references are provided to WBC and converted into dynamically consistent joint behavior.

Swing trajectories are commonly designed with smooth polynomial, spline, or phase-based functions. Horizontal motion progresses toward the target foothold, while vertical motion raises the foot to a clearance height and subsequently lowers it for touchdown. Smooth velocity and acceleration profiles are important because discontinuous references can generate abrupt joint torques, excite structural vibration, and interfere with body stabilization.

Foot clearance is not a fixed geometric requirement in all environments. Flat indoor terrain may permit a relatively low swing height, reducing energy consumption and unnecessary leg motion. Rough outdoor terrain may require increased clearance based on elevation maps, obstacle estimates, or uncertainty. WBC must realize the resulting trajectory while ensuring that joint limits and leg workspace remain feasible.

Swing-foot tracking is usually formulated as a Cartesian task. If Js represents the swing-foot Jacobian, the commanded foot acceleration is related to generalized acceleration through Js q̈ + J̇s q̇. Feedback terms based on position and velocity errors can be added to the desired feedforward acceleration. This formulation allows the optimizer to coordinate swing motion with simultaneous trunk and stance-leg behavior.

The relative priority of swing-foot tracking must be chosen carefully. Accurate placement is important because the touchdown location determines the geometry of the next support phase. However, enforcing swing tracking too strongly can conflict with body stabilization, joint limits, or torque constraints. Practical WBC therefore allows limited trajectory deviation when preserving balance or hardware feasibility requires additional control freedom.

Liftoff is a transition rather than an instantaneous change of physical role. Before removing a stance constraint, the controller can progressively reduce the desired contact force on that leg. This unloading process transfers support to the remaining feet and prevents abrupt changes in body acceleration. Once the normal force approaches an appropriate level, the leg can transition into swing motion with reduced disturbance.

Touchdown requires similarly careful management. The controller should avoid driving the foot aggressively into the terrain using a rigid position objective. Near the expected contact time, downward velocity can be reduced and vertical compliance increased. Contact sensors, joint torque estimates, force measurements, or proprioceptive detection can identify actual touchdown and trigger the transition from swing tracking to stance constraints.

Planned and measured contact states may differ. A foot can encounter the ground earlier than expected because of an obstacle or terrain-height error, or it may fail to contact at the expected time because the surface is lower than predicted. Robust WBC therefore should not rely exclusively on the gait schedule. Contact estimation provides feedback that allows task management to respond to the physical environment.

Early contact requires rapid but smooth modification of the swing task. Continuing the original downward trajectory after contact can generate a large impact force or cause slipping. Once reliable early contact is detected, the controller can terminate vertical swing tracking, introduce compliant contact behavior, and gradually activate stance-force capability. This transition should avoid instantaneous changes in commanded torque.

Late contact presents the opposite problem. If touchdown is expected but no contact is detected, immediately imposing a rigid stance constraint would create an incorrect model of the robot-environment interaction. The controller may extend the leg downward within safe workspace limits while maintaining body support through the remaining contacts. The planned stance task is activated only after contact becomes sufficiently credible.

Contact confidence can be treated continuously rather than as a purely binary variable. During uncertain transitions, force limits, task weights, or constraint stiffness can be adjusted according to estimated contact probability. A newly detected contact may initially receive a small allowable normal force, which increases as confidence grows. Such gradual activation reduces sensitivity to noisy contact classification.

Stance-foot slip is another important task-management condition. The rigid-contact assumption becomes invalid when tangential ground forces exceed available friction or the terrain moves beneath the foot. Slip can be detected using discrepancies between predicted and measured foot velocity, inertial motion, or force information. WBC can then reduce tangential force demand and modify body or foothold objectives.

Terrain orientation affects both stance and swing management. A stance foot on an inclined surface should use a contact frame aligned with the local terrain normal when reliable geometry is available. Friction constraints can then be expressed relative to that surface. For swing motion, terrain estimates influence landing height, approach direction, clearance, and the orientation requirements of specialized feet or end effectors.

The support polygon and contact geometry change every time a leg enters or leaves stance. During four-leg support, substantial redundancy exists for distributing forces and stabilizing the trunk. During diagonal two-leg support in a trot, force authority is more restricted. WBC must therefore adapt task weights, feasible force ranges, and body objectives according to the instantaneous support configuration.

Dynamic gaits make this adaptation particularly important because stance periods are short and forces can become large. Rapid force changes at touchdown and liftoff may excite the body or exceed actuator limits. Force-rate regularization can penalize excessive differences between consecutive contact-force solutions. This creates smoother load transfer while still allowing the controller to respond to disturbances.

Joint-space posture objectives remain active underneath swing and stance tasks but should not interfere with their primary roles. During swing, posture regulation can guide the leg toward comfortable configurations while the foot follows its Cartesian trajectory. During stance, internal posture objectives can avoid joint limits and singularities while contact forces support the body. Higher-priority contact and balance requirements must remain protected.

Leg workspace management is particularly important for foothold feasibility. A navigation or locomotion planner may request a foot position that lies near or beyond the kinematic reach of the leg. WBC cannot correct an fundamentally unreachable target through optimization alone. Workspace constraints or reachability checks should therefore modify foothold commands before excessive joint motion or infeasibility occurs.

Swing-leg motion also affects the floating body through inertial coupling. Although a swing leg does not generate intentional ground reaction force, rapidly accelerating its links produces reaction forces and moments on the trunk. Full-body dynamics naturally capture these effects. This becomes increasingly important for heavy legs, high-speed running, or robots carrying additional equipment on distal links.

Payload conditions can modify stance and swing priorities. A heavy or offset payload changes the combined center of mass and may require asymmetric stance-force distribution. If payload orientation must remain stable, swing-leg acceleration may need to be limited to reduce trunk disturbances. Task management therefore connects leg-level behavior to mission-level requirements rather than treating each leg independently.

Model Predictive Control (MPC) can provide WBC with future contact schedules, desired ground reaction forces, body trajectories, and foothold locations. WBC then realizes the current portion of that plan using the complete robot model and measured state. Differences between predicted and actual contact can be corrected locally, while significant deviations can be communicated upward so that the future plan is regenerated.

Real-time implementation benefits from maintaining a consistent mathematical structure across gait transitions. Instead of repeatedly changing optimization dimensions, some WBC systems retain variables for all feet and modify bounds, weights, or constraints according to contact mode. This can simplify memory management, preserve sparse matrix structure, and reduce numerical disturbances when the robot switches between swing and stance.

Safe failure handling must also consider individual leg tasks. If swing tracking becomes infeasible, the controller may reduce trajectory acceleration or select a safer intermediate configuration. If a stance contact becomes unreliable, force should be redistributed to other available contacts whenever possible. Severe loss of support may require gait recovery, emergency stepping, body lowering, or a controlled stop.

Effective swing and stance management ultimately depends on treating contact as a dynamic process rather than a fixed binary schedule. Planned gait phase, measured contact, terrain geometry, friction, force capability, workspace, and whole-body stability must be considered together. By continuously redefining each leg's task and smoothly managing transitions, WBC enables a quadruped to transform coordinated foot motion into stable, adaptable, and dynamically consistent locomotion.

06.05. Payload Carrying WBC with Load Estimation [w/Code]

Payload-carrying Whole-Body Control (WBC) extends conventional quadruped control by explicitly accounting for additional mass and inertia introduced by transported equipment or cargo. A payload changes the robot's total weight, center of mass, rotational inertia, required ground reaction forces, and actuator loading. If these changes are ignored, a controller based on nominal robot parameters can produce systematic tracking errors and reduced stability.

The simplest payload model assumes that the load is rigidly attached to the trunk. Its mass, center-of-mass offset, and inertia tensor can then be incorporated into the corresponding rigid-body parameters. The robot and payload behave approximately as one composite multibody system. This representation is appropriate for firmly mounted batteries, sensors, containers, tools, or cargo that does not move significantly relative to the body.

Payload mass alone is insufficient when the load is positioned away from the trunk center. An offset load creates gravitational moments and changes the combined center of mass of the robot-payload system. A heavy object mounted toward the rear, front, or side therefore produces asymmetric support requirements. WBC must redistribute contact forces among stance feet and may modify trunk posture to maintain adequate stability margins.

Rotational inertia becomes increasingly important when the robot accelerates angularly. A large or spatially extended payload can substantially increase roll, pitch, and yaw inertia even if its center of mass remains close to the trunk center. Orientation controllers designed for the unloaded robot may then demand insufficient torque or exhibit slower response. Payload-aware dynamics allow desired angular accelerations to be converted into more accurate force and torque requirements.

Load estimation is required when payload properties are unknown or change during operation. Logistics robots may repeatedly receive and release objects, while manipulation systems can pick up components with uncertain mass. Instead of relying on manually entered parameters, the controller can estimate additional mass, center-of-mass displacement, or disturbance wrench from measured robot motion, actuator effort, and contact behavior.

A basic mass estimator can compare the support force predicted by the nominal model with the force required to maintain measured vertical equilibrium. When the robot stands approximately still, the sum of vertical ground reaction forces should correspond to the combined gravitational load. The difference between measured or estimated support force and nominal robot weight provides information about the additional payload mass.

Dynamic operation requires richer estimation because inertial forces become mixed with gravity. During acceleration, measured joint torques, base acceleration, angular velocity, contact forces, and known robot dynamics can be combined in a parameter-estimation problem. The estimator searches for payload parameters that reduce the residual between predicted dynamics and observed behavior. Sufficient excitation is necessary to identify some parameters reliably.

Payload center-of-mass estimation can exploit the distribution of support forces among the feet. During quasi-static standing, the location of the combined center of pressure reflects the projected center of mass when external disturbances are small. By comparing this location with the known unloaded robot model, the controller can infer approximate payload offsets. Multiple postures or controlled motions can improve observability.

More general methods estimate an external wrench acting on the trunk. A six-dimensional wrench contains three force and three moment components and can represent the net effect of an unknown load. Disturbance observers, momentum observers, or inverse-dynamics residuals can estimate this wrench without immediately identifying detailed payload parameters. WBC can compensate for the estimated disturbance directly while slower algorithms identify physical load properties.

Filtering is essential because raw load estimates can contain sensor noise, modeling errors, impact transients, and contact-estimation errors. Low-pass filtering, recursive least squares, Kalman filtering, or Bayesian estimation can provide smoother parameter updates. However, excessive filtering introduces delay when a payload changes suddenly. The estimator must therefore balance noise rejection against the need for timely adaptation.

The controller should also distinguish persistent payload effects from short-duration external disturbances. A constant additional downward force and moment may indicate a carried load, whereas a brief lateral force may result from collision or human interaction. Treating every disturbance as a payload change can corrupt the model. Persistence tests, confidence measures, and parameter-rate limits help separate slowly varying load properties from transient events.

Once payload parameters are available, WBC can update the mass matrix, gravity vector, Coriolis terms, center-of-mass location, and centroidal quantities used by the optimization. Depending on computational architecture, the complete rigid-body model may be regenerated or payload contributions may be added analytically. The update should remain numerically smooth because abrupt model changes can produce discontinuities in optimized forces and torques.

Ground reaction force distribution is one of the most immediate consequences of payload adaptation. A heavier robot requires greater total normal force, while an offset payload requires asymmetric loading among the feet. The WBC optimization distributes these forces according to the updated combined center of mass while respecting friction cones, force limits, contact geometry, and actuator capability.

Payload carrying also reduces available dynamic margin. Motors that previously had substantial torque reserve may operate closer to saturation simply to support the increased weight. The controller must recognize that aggressive acceleration, high step frequency, or large body rotations may no longer be feasible. Torque constraints inside the WBC optimization naturally reveal this reduced capability and allow locomotion objectives to be relaxed.

A payload-aware controller can modify motion commands before actuator saturation occurs. Desired forward acceleration, yaw rate, body height, or gait frequency can be scaled according to estimated load and available torque margin. This approach is preferable to generating aggressive commands and clipping them afterward because command scaling preserves coordination between body motion, contact forces, and swing-leg behavior.

Foot placement may also need adaptation. Changes in combined center of mass alter the relationship between foothold geometry and body stability. A rear-heavy payload, for example, can require different nominal foot locations or stance lengths to maintain useful support margins. Higher-level gait or MPC modules can incorporate the estimated payload state and provide WBC with load-aware foothold and body-motion references.

Payload orientation becomes a separate control objective when the transported object must remain level or aligned with a specified frame. Fragile instruments, cameras, containers, liquids, or manipulation platforms may tolerate only limited roll and pitch. WBC can prioritize payload orientation over nominal trunk attitude when an actuated mount or manipulator provides sufficient degrees of freedom to separate the two.

When the payload is rigidly attached, payload and trunk orientations cannot be controlled independently. The controller must then regulate trunk motion according to payload requirements. Limiting angular acceleration, roll, pitch, and jerk can reduce inertial loading on transported objects. This may require slower locomotion over rough terrain even when the robot itself could physically tolerate more aggressive motion.

Suspended or partially compliant payloads require more complex modeling. A hanging load can oscillate like a pendulum, generating time-varying forces and moments that disturb the trunk. Simply increasing the robot mass does not capture this behavior. Additional payload states, such as swing angle and angular velocity, may need to be estimated and incorporated into planning or control to suppress oscillation.

Manipulated payloads introduce another form of dynamic coupling. When a quadruped-mounted arm picks up an object, the load may move through a large workspace relative to the trunk. The combined center of mass and inertia then change continuously with arm configuration. Whole-body dynamics should include the manipulator and object so that leg contact forces, body stabilization, and manipulation forces are coordinated consistently.

Contact constraints become more critical under heavy loads because required ground reaction forces approach friction and structural limits. A payload may increase normal force and potentially increase available friction, but it also increases tangential forces required for acceleration. On low-friction terrain, the controller may need to reduce acceleration substantially to keep each contact force inside its friction cone and prevent slipping.

Terrain inclination further modifies payload requirements. On a slope, gravity generates both normal and tangential components relative to the contact surface. An offset payload can additionally create moments that increase loading on selected feet. Terrain-aware WBC combines estimated surface normals with the updated robot-payload dynamics to calculate feasible force distributions and orientation references.

Load estimation should include confidence information whenever possible. Poor excitation, uncertain contacts, sensor bias, or rapid motion may make parameter estimates unreliable. Instead of immediately applying every estimate, the controller can blend between nominal and estimated models according to confidence. Parameter bounds can prevent unrealistic mass or center-of-mass values from destabilizing the control system.

Safety limits should depend on the estimated payload state. Maximum speed, acceleration, slope capability, step height, and allowable gait may differ significantly between unloaded and fully loaded operation. A supervisory controller can use estimated mass, center-of-mass offset, actuator margin, and contact margin to select appropriate operating envelopes. WBC then enforces the corresponding instantaneous constraints.

Load changes require special handling during pickup and release. When an object is acquired, the dynamics may change within a short time and contact forces must increase accordingly. During release, support requirements decrease suddenly. Detecting these events and smoothly transitioning model parameters prevents abrupt body motion. Manipulator force sensing or changes in joint torque can provide useful event information.

QP-based WBC provides a natural framework for integrating payload adaptation. Updated dynamics appear in equality constraints, while torque limits, friction constraints, and contact-force bounds remain explicit inequalities. Payload orientation, body tracking, and posture objectives can be weighted according to mission requirements. Slack variables can preserve feasibility when the requested motion becomes too demanding for the loaded robot.

Model Predictive Control (MPC) can further exploit load estimates by predicting future motion using the updated mass and inertia. The planner can generate slower accelerations, modified contact forces, and different footholds before WBC executes them. This reduces disagreement between planning and execution, particularly when payload mass represents a substantial fraction of the robot's own mass.

Validation should cover more than static load support. Testing should include different payload masses, center-of-mass offsets, gait speeds, slopes, accelerations, disturbances, pickup events, and uncertain friction conditions. Important metrics include estimation error, body tracking error, force distribution, torque margin, slip occurrence, solver feasibility, and recovery behavior after sudden load changes.

Payload-aware WBC ultimately treats cargo as part of the robot's instantaneous physical system rather than as an unmodeled disturbance. Load estimation provides the bridge between unknown operating conditions and model-based control. By continuously adapting dynamics, contact-force distribution, motion limits, and task priorities, a quadruped can carry changing loads while preserving balance, actuator safety, terrain compatibility, and predictable mission performance.

06.06. Manipulation Arm WBC Integration Spot Arm [w/Code]

Whole-Body Control (WBC) integration of a manipulation arm transforms a quadruped from a mobile locomotion platform into a mobile manipulation system. The legs, floating base, arm, and gripper become dynamically coupled parts of one robot. Instead of commanding the arm independently while the quadruped merely stands underneath it, integrated WBC coordinates locomotion, balance, contact forces, body posture, and manipulation objectives simultaneously.

A platform such as Spot equipped with an arm illustrates the mobile-manipulation concept clearly. The arm extends the reachable workspace beyond what fixed-base manipulation permits because the quadruped can reposition its body and feet around the task. Conversely, arm motion influences the supporting robot through reaction forces and moments. Effective control therefore requires coordination between manipulation and locomotion rather than treating them as unrelated subsystems.

The complete system can be modeled as a floating-base multibody mechanism containing the six-degree-of-freedom body pose, leg joints, arm joints, and gripper coordinates. The generalized state consequently becomes larger than that of a locomotion-only quadruped. Mass distribution and inertia also vary with arm configuration, making full-body dynamics particularly important when the manipulator carries heavy tools or objects far from the trunk.

Arm motion changes the combined center of mass of the robot. Extending the manipulator forward shifts the center of mass toward the front contacts, while lateral extension creates asymmetric loading between the left and right legs. WBC compensates by redistributing ground reaction forces, modifying body posture, or repositioning the feet. Without this coordination, aggressive arm motion can reduce stability even when the arm trajectory itself is kinematically feasible.

The manipulation end effector is represented as another operational-space task within WBC. Desired position and orientation trajectories can be converted into Cartesian velocity or acceleration references through the end-effector Jacobian. The optimization then determines compatible arm, body, and sometimes leg motion. This allows the robot to exploit all available degrees of freedom rather than forcing the arm alone to satisfy every manipulation objective.

Whole-body redundancy is one of the major advantages of integrated control. If the arm approaches a joint limit while reaching toward an object, the quadruped can translate or rotate its trunk to extend the effective workspace. If further adjustment is necessary, it can take a step and establish a new support configuration. Mobile manipulation therefore turns locomotion itself into a mechanism for increasing manipulation reachability.

Task hierarchy determines how this redundancy is used. Contact feasibility, actuator safety, and balance generally remain high-priority requirements. During manipulation, end-effector position, orientation, or interaction force may receive higher priority than nominal body posture. Joint posture, energy minimization, and preferred configurations can remain lower-priority objectives that exploit residual freedom without disturbing the manipulation task.

The appropriate hierarchy changes with operating mode. During navigation, locomotion velocity, body stabilization, and foothold generation dominate while the arm may remain folded in a transport posture. When the robot reaches a work location, manipulation accuracy can become more important and locomotion commands can be reduced. During combined walking and manipulation, both task groups must be coordinated continuously according to mission requirements.

Stationary manipulation does not imply that the legs should remain completely passive. Even when all four feet remain in contact, small body translations, rotations, and force redistributions can improve arm reachability and reduce joint effort. WBC can intentionally move the trunk within a safe support region while preserving foot contacts, effectively using the legs as a large positioning mechanism beneath the manipulator.

Contact interaction introduces additional requirements when the arm pushes, pulls, turns, drills, opens doors, or operates equipment. Forces generated at the end effector propagate through the arm and body to the supporting feet. The WBC dynamics must balance these interaction forces with ground reaction forces. Otherwise, a manipulation force that is acceptable for the arm alone may cause foot slip or destabilize the quadruped.

The end-effector interaction can be modeled as an external wrench acting on the robot. Force and torque references may be incorporated into the optimization, while friction constraints at the feet determine whether the required reaction wrench can be supported. This coupling establishes a whole-body manipulation capability envelope: achievable tool forces depend not only on arm strength but also on stance geometry, terrain friction, and leg actuator capacity.

Impedance control is particularly useful for manipulation tasks involving uncertain contact geometry. Instead of rigidly enforcing end-effector position, the controller specifies a relationship between position error and interaction force. The arm can then remain compliant when contacting handles, valves, surfaces, or tools. Whole-body impedance behavior can extend this concept by allowing controlled motion of both the arm and quadruped body.

Admittance control provides another approach when force measurements are available. Measured external force can be converted into a desired motion response, allowing the end effector or entire robot to yield naturally under interaction. The resulting motion reference can be passed to WBC, which determines how much response should be produced by arm joints, trunk motion, or stepping while preserving contact and balance constraints.

Force/torque sensing near the wrist improves interaction control by measuring the wrench applied at the end effector. Joint torque sensing or estimation can provide complementary information. These measurements help distinguish intended manipulation forces from unexpected collisions and model errors. They can also support payload estimation after an object is grasped, allowing the dynamic model to adapt to the newly acquired load.

Grasping an object changes the robot dynamics immediately. The object effectively becomes an additional payload attached through the arm, modifying the combined center of mass and inertia according to arm configuration. If object mass is significant, WBC should update its dynamics or estimate the resulting external wrench. Contact-force distribution among the feet can then adapt as the object is lifted, transported, or repositioned.

The support configuration strongly influences manipulation capability. A wide four-foot stance generally provides greater resistance to external moments than a narrow or dynamically changing support pattern. Before performing high-force manipulation, the robot may intentionally reposition its feet to create a more favorable support geometry. Manipulation planning should therefore consider stance selection as part of the task rather than assuming fixed foot locations.

Reachability should also be evaluated at the whole-body level. An object may lie outside the arm-only workspace but remain reachable after body translation, trunk rotation, height adjustment, or stepping. Whole-body inverse kinematics or optimization can evaluate combinations of these motions. This prevents unnecessary navigation movements while avoiding arm configurations near singularities or joint limits.

Collision avoidance becomes more complex after adding an arm. The manipulator must avoid the robot's trunk, legs, sensors, payload, and surrounding environment while the quadruped itself may be moving. Self-collision constraints can be incorporated through distance functions or optimization penalties. Environmental collision avoidance is usually coordinated with perception and motion planning before references are sent to the real-time WBC layer.

Manipulation accuracy depends on reliable state estimation. End-effector pose is influenced not only by arm encoder measurements but also by floating-base position and orientation errors. Small body-estimation errors can become significant at an extended tool tip. Visual localization, inertial sensing, joint encoders, terrain contact estimation, and sometimes external perception must therefore be combined to maintain an accurate manipulation reference frame.

Visual servoing can compensate for residual localization and calibration errors. A camera mounted on the head, body, wrist, or environment can estimate the relative pose between the tool and target. Instead of relying entirely on global coordinates, the manipulation controller updates end-effector references from visual error. WBC then realizes these corrections while simultaneously maintaining body stability and feasible ground contacts.

Locomotion and manipulation can also occur simultaneously. A quadruped may carry an object while walking, inspect a surface with a sensor, maintain a camera viewpoint, or move a tool along a structure. In these cases the end-effector trajectory and body trajectory are coupled over time. WBC distributes motion between stepping, trunk movement, and arm articulation while respecting contact transitions and actuator limits.

Walking manipulation places additional demands on trajectory planning because the support set changes as feet enter and leave contact. Arm motion that is feasible during four-foot support may become destabilizing during a two-foot support phase. The controller can reduce manipulation acceleration during vulnerable gait phases or synchronize arm movement with the contact schedule to maintain sufficient stability and force margin.

Model Predictive Control (MPC) can assist integrated WBC by anticipating future body motion, contacts, and manipulation requirements. A higher-level planner can predict how an arm trajectory will shift the center of mass or generate interaction forces and can adjust footholds in advance. WBC then resolves the instantaneous full-body dynamics and actuator constraints at a faster control frequency.

QP-based WBC provides a natural implementation framework for this integration. Floating-base dynamics, stance contacts, friction limits, torque bounds, and joint limits can be represented as constraints. End-effector tracking, interaction force, trunk orientation, swing-foot motion, and posture can appear as weighted tasks. The optimizer determines a dynamically consistent compromise across the complete quadruped-manipulator system.

Joint-limit management is particularly important because arm and leg joints may simultaneously approach unfavorable configurations. Rather than waiting for saturation, optimization can introduce avoidance costs or predictive inequality constraints. The trunk can then move proactively to preserve arm manipulability, while leg configurations retain enough workspace for balance recovery and future stepping.

Failure handling must account for both locomotion and manipulation. Unexpected object motion, failed grasp, excessive interaction force, foot slip, or arm tracking failure can change the stability condition rapidly. Depending on severity, the robot may release force, retract the arm, widen its stance, take a recovery step, lower the body, or stop the manipulation task while maintaining safe support.

Spot Arm demonstrates the broader architectural principle that a quadruped and manipulator should be treated as one coordinated physical system when performing mobile manipulation. The exact commercial implementation is platform specific, but the general WBC principle is universal: arm motion changes locomotion dynamics, leg contacts determine manipulation capability, and the floating base provides useful redundancy between them.

Integrated arm-quadruped WBC ultimately removes the artificial boundary between mobility and manipulation. The feet establish controllable interaction with the environment, the legs position and stabilize the floating base, and the arm performs task-level interaction. By optimizing these components together, a quadruped can reach farther, exert controlled forces, carry objects, manipulate while moving, and adapt its entire body to the physical requirements of the task.

06.07. WBC Real Time 1kHz Computational Optimization [w/Code]

Real-time Whole-Body Control (WBC) at 1 kHz requires the complete sensing, dynamics, optimization, and command pipeline to finish within approximately one millisecond. This timing requirement is demanding because a quadruped controller must evaluate floating-base dynamics, contact constraints, task Jacobians, optimization matrices, and actuator commands while maintaining predictable execution time under changing gait conditions.

A 1 kHz control rate is valuable because legged robots experience fast contact dynamics and rapid changes in joint torque, foot force, and body motion. Short control intervals allow disturbances and tracking errors to be corrected before they grow significantly. High-rate control is particularly important during touchdown, slipping, impact recovery, dynamic trotting, running, and manipulation where interaction forces can change much faster than higher-level planners operate.

The 1 ms period should be treated as an end-to-end deadline rather than as the available time for the QP solver alone. State acquisition, filtering, state estimation, rigid-body dynamics, Jacobian computation, task generation, QP assembly, numerical optimization, safety checks, communication, and actuator command transmission all consume time. A solver taking 0.8 ms may therefore already be unsuitable if the surrounding pipeline requires another 0.5 ms.

A practical architecture separates control functions according to their required update frequencies. Motor current or torque regulation may operate at the actuator level at several kilohertz, WBC may execute near 500 Hz to 1 kHz, state estimation may run at a comparable rate, and Model Predictive Control (MPC) may operate at tens or hundreds of hertz. Perception, terrain mapping, and global planning normally execute more slowly and provide asynchronous references.

Computational optimization begins with reducing unnecessary problem dimensions. The WBC decision vector should contain only variables required by the chosen formulation. Generalized accelerations, contact forces, and joint torques can all be optimized explicitly, but this increases QP size. Eliminating variables analytically through rigid-body dynamics or exploiting actuated and unactuated partitions can reduce computation when the resulting numerical structure remains well conditioned.

Sparse linear algebra is fundamental because robot dynamics and WBC constraints contain substantial structure. Contact Jacobians affect selected coordinates, task matrices have repeated patterns, and many QP entries remain zero. Storing and solving these systems as dense matrices wastes computation and memory bandwidth. Sparse representations allow numerical solvers to operate primarily on meaningful nonzero elements and become increasingly advantageous as system complexity grows.

Fixed matrix dimensions can improve real-time performance even though the number of active contacts changes during locomotion. Instead of resizing the optimization problem whenever a foot enters or leaves stance, variables for all potential contacts can remain allocated. Bounds or constraint coefficients can activate and deactivate individual contacts. This preserves memory layouts and sparsity patterns and reduces unpredictable allocation or factorization overhead.

Preallocation is another important real-time design principle. Dynamic memory allocation inside the control loop can produce nondeterministic latency because allocation time depends on runtime memory state. Matrices, vectors, solver workspaces, communication buffers, and logging structures should therefore be created before entering the high-frequency loop whenever possible. Runtime execution should primarily update numerical values inside existing storage.

Rigid-body dynamics computation must also be efficient. Recursive algorithms can calculate mass matrices, nonlinear effects, centroidal quantities, and inverse dynamics without repeatedly performing expensive symbolic operations. Robotics libraries commonly exploit articulated-body and recursive Newton-Euler methods. The objective is not merely high average speed but consistent computation across different robot configurations and contact phases.

Task Jacobians and their derivatives can consume substantial computation when many operational-space objectives are active. Only quantities required by the current controller should be evaluated. Reusing intermediate kinematic transforms, spatial velocities, and frame data prevents repeated calculations. A well-designed computation graph evaluates forward kinematics once and allows multiple WBC tasks to access the resulting cached quantities.

QP matrix assembly can become as expensive as solving the optimization itself. Reconstructing matrices from high-level objects every cycle introduces unnecessary copying and allocation. Efficient implementations maintain a predetermined matrix structure and update only coefficients affected by the current robot state, reference, or contact mode. Direct access to solver data structures can further reduce conversion overhead between robotics and optimization libraries.

Warm starting is particularly effective for high-frequency WBC because consecutive optimization problems are strongly correlated. Over one millisecond, joint positions, velocities, desired forces, and task references usually change only slightly. The previous primal and dual solutions therefore provide useful initial conditions. Active-set solvers can reuse working sets, while iterative solvers can initialize optimization variables and multipliers from the preceding control cycle.

Factorization reuse provides another major computational advantage. If the QP sparsity structure remains unchanged, symbolic factorization does not need to be repeated every cycle. Depending on the solver and formulation, numerical factorizations or portions of them may also be reused or updated efficiently. Designing the controller so that contact switching modifies values and bounds rather than complete matrix topology can substantially reduce computation.

Solver tolerance should be selected for control performance rather than mathematical perfection. Driving primal and dual residuals to extremely small values may require many additional iterations without producing a meaningful improvement in robot behavior. The appropriate tolerance is the loosest setting that still maintains acceptable contact constraints, torque commands, tracking accuracy, and stability. This tradeoff should be determined experimentally.

Iteration limits are necessary for deterministic timing. An iterative solver should not continue indefinitely because one difficult QP could violate the control deadline. A maximum iteration count or computation-time budget provides a predictable upper bound. If convergence is incomplete, the controller can evaluate whether the current approximate solution is sufficiently feasible or whether a fallback command must be used.

OSQP can be useful for real-time WBC when its sparse structure, warm starting, and factorization behavior are exploited carefully. Its operator-splitting formulation allows accuracy and iteration count to be traded against computation time. However, achieving 1 kHz is not guaranteed simply by selecting a fast solver. Actual performance depends on QP dimensions, conditioning, processor architecture, contact structure, tolerance settings, and surrounding software overhead.

Active-set solvers such as qpOASES can also perform efficiently when sequential QPs differ only modestly. If the active constraint set remains similar between adjacent cycles, hot-start strategies may converge rapidly. Contact transitions can cause larger changes and increase computation. Consequently, benchmarking should include touchdown, liftoff, saturation, disturbance recovery, and other difficult cases rather than only steady-state walking.

Numerical scaling directly affects computation time. Poorly scaled force, acceleration, torque, and orientation variables can slow convergence or create inaccurate solutions. Task normalization and variable scaling should keep relevant quantities within comparable numerical ranges. Appropriate regularization of weakly constrained directions can improve Hessian conditioning and reduce solver effort while producing smoother control commands.

Strict task hierarchies can increase computational cost because several QPs may need to be solved sequentially. For a 1 kHz implementation, a single weighted QP is often computationally attractive, although it provides softer priority semantics. Hybrid strategies can keep safety-critical dynamics and physical limits as hard constraints while representing performance objectives through weighted costs, reducing the number of optimization stages.

Parallel computation can accelerate selected portions of the pipeline, but it must be applied carefully. Independent kinematic calculations, perception interfaces, logging, and some model updates may run on separate threads. The core optimization often contains sequential numerical dependencies, so adding threads does not automatically reduce latency. Synchronization, cache contention, and operating-system scheduling can even make timing less predictable.

CPU affinity and real-time scheduling are important on general-purpose computers. The WBC thread can be pinned to a dedicated processor core to reduce migration and cache disruption. Real-time operating-system policies can assign it higher scheduling priority than logging, visualization, networking, or user-interface tasks. Background processes should not be allowed to introduce unpredictable delays into a safety-critical control loop.

Memory behavior matters because modern processors can execute arithmetic much faster than they can recover poorly organized data from memory. Fixed-size arrays, contiguous storage, cache-friendly matrix layouts, and reduced copying improve deterministic performance. Excessive abstraction layers can create temporary objects and hidden allocations, so profiling should measure memory operations in addition to floating-point computation.

Communication latency must be included in the real-time budget. Commands may pass through EtherCAT, CAN, Ethernet, serial links, or proprietary actuator networks, while sensor measurements travel in the opposite direction. Deterministic field buses and synchronized timestamps reduce timing uncertainty. A controller that computes commands in 0.4 ms can still perform poorly if communication introduces large or variable delays.

Time synchronization becomes important when IMU, joint encoder, force, vision, and external sensing data originate from different devices. WBC assumes that the state represents a consistent physical instant. Timestamp errors can appear as false velocities, contact inconsistencies, or orientation errors. Hardware synchronization, PTP, shared clocks, or carefully designed timestamp compensation can improve the temporal consistency of sensor fusion.

Logging should never block the high-frequency control thread. Writing files, formatting text, transmitting telemetry, or rendering visualization can produce large latency spikes. The real-time loop should place compact data into preallocated lock-free or bounded buffers, while lower-priority threads perform storage and visualization asynchronously. Logging rates can also be reduced while preserving critical diagnostic variables.

Deadline monitoring is necessary because average execution time does not demonstrate real-time capability. The system should record minimum, mean, percentile, and worst-case computation times together with deadline misses. A controller averaging 0.3 ms but occasionally requiring 2 ms may be less suitable than one consistently completing in 0.7 ms. Tail latency is therefore a critical metric for WBC deployment.

Worst-case testing should deliberately exercise difficult operating conditions. Rapid contact switching, all task constraints active, near-singular configurations, torque saturation, low-friction constraints, arm manipulation, payload changes, and disturbance recovery can increase computation. Benchmarking only nominal standing or steady trotting can hide timing failures that emerge precisely when the robot most requires reliable control.

A multi-rate architecture prevents unnecessary work from entering the 1 kHz loop. Terrain planning, foothold optimization, MPC horizon updates, payload identification, and complex collision calculations can execute at lower frequencies. Their outputs can be interpolated or held between updates. The WBC layer should perform only calculations that genuinely require millisecond-scale feedback while consuming prepared references from slower modules.

Safety behavior must remain deterministic when the deadline cannot be met. The robot can temporarily reuse the previous valid torque command, apply a predefined stabilizing controller, reduce task demands, or enter a controlled stop. Repeated deadline misses should trigger escalation rather than allowing stale commands to continue indefinitely. Timing failure is therefore treated as a control fault, not merely a software performance issue.

Profiling should guide optimization instead of relying on assumptions. Execution time should be measured separately for state estimation, dynamics, kinematics, QP assembly, solver execution, safety processing, and communication. Optimization effort can then target the actual bottleneck. Replacing a solver provides little benefit if matrix construction, memory allocation, or communication dominates the control period.

Achieving 1 kHz WBC is ultimately a system-engineering problem combining control formulation, numerical optimization, software architecture, operating-system behavior, communication, and hardware capability. The goal is not simply to compute one solution in less than one millisecond, but to produce safe, dynamically consistent commands every millisecond with bounded latency. Deterministic execution is therefore as important as raw computational speed.

06.08. WBC Constraint Relaxation Infeasible Recovery [w/Code]

Constraint relaxation is a fundamental mechanism for maintaining Whole-Body Control (WBC) operation when the original optimization problem becomes infeasible. In a quadruped, desired body motion, swing-foot tracking, contact-force limits, friction constraints, joint limits, and actuator limits may conflict. A robust controller must recognize these conflicts and degrade selected objectives safely instead of returning no valid command.

Infeasibility occurs when no decision vector can satisfy all equality and inequality constraints simultaneously. For example, a desired body acceleration may require ground reaction forces outside the available friction region, or a swing-foot trajectory may demand joint accelerations incompatible with torque limits. The optimization problem is mathematically valid, but the requested combination of physical requirements cannot be realized by the robot.

The distinction between hard constraints and soft constraints is central to infeasibility recovery. Hard constraints represent conditions that should not normally be violated, such as fundamental equations of motion, critical actuator limits, and essential safety boundaries. Soft constraints represent requirements that may be relaxed when necessary, such as exact trajectory tracking, nominal posture, preferred force distribution, or noncritical orientation objectives.

Constraint relaxation introduces controlled freedom into otherwise rigid requirements. Instead of enforcing an equality exactly as Ax = b, the controller can write Ax = b + s, where s is a slack variable. The optimization penalizes the magnitude of s so that the original constraint remains approximately satisfied whenever possible. When conflict occurs, the solver can use a nonzero slack rather than declaring the entire QP infeasible.

Slack variables can also relax inequality constraints. A requirement such as Cx ≤ d can be modified to Cx ≤ d + s with s constrained to be nonnegative. A large penalty on s discourages violation under normal conditions. This formulation provides an explicit numerical measure of how much relaxation is required and allows the controller to distinguish small temporary conflicts from severe loss of feasibility.

Penalty weights determine which objectives are sacrificed first. If body orientation is more important than nominal joint posture, orientation relaxation should carry a much larger penalty. The optimizer will then allow posture error before significant orientation error. Proper weight selection therefore converts engineering priorities into quantitative recovery behavior and must reflect the physical consequences of violating each requirement.

Not every constraint should receive a slack variable. Relaxing an actuator torque limit without an independent hardware protection mechanism could command unsafe torque, while relaxing a friction constraint excessively may generate physically impossible contact forces. Constraint relaxation should therefore be designed around a safety hierarchy in which physical and hardware boundaries remain protected while performance-related objectives absorb most conflicts.

Contact constraints are a common source of WBC infeasibility. A controller may assume that a stance foot remains stationary while measured contact conditions indicate slipping or incomplete support. Strictly enforcing zero foot acceleration together with aggressive body tracking can then become inconsistent. The controller may soften selected contact kinematics or revise the contact state when evidence shows that the assumed rigid contact model is no longer appropriate.

Friction limitations produce another important conflict. Desired horizontal acceleration requires tangential ground reaction force, but each foot can generate only the force permitted by its normal load and available friction. When the requested acceleration exceeds this capability, WBC should reduce the motion objective rather than artificially violate the friction cone. This preserves physically realizable contact forces and reduces the probability of slip.

Actuator saturation can make an otherwise feasible motion impossible. Large payloads, extreme body postures, fast swing-leg trajectories, or disturbance recovery may require joint torques beyond motor limits. Torque bounds should generally remain hard constraints, while desired accelerations or tracking objectives are relaxed. The resulting motion may be slower or less accurate, but the command remains compatible with actuator capability.

Joint position and velocity limits require predictive treatment because waiting until a joint reaches its boundary leaves little room for recovery. WBC can introduce velocity-damper constraints or configuration-dependent bounds that become progressively restrictive near a limit. If another task conflicts with these protective constraints, its tracking accuracy is reduced before the joint enters an unsafe region.

Hierarchical relaxation provides a structured method for resolving conflicts. Safety-critical constraints occupy the highest level, followed by balance and contact feasibility, essential task execution, body tracking, manipulation accuracy, and finally comfort or posture objectives. When infeasibility appears, lower-priority requirements are relaxed first. This prevents arbitrary compromises caused solely by numerical weighting.

Strict hierarchical QP can enforce this ordering by solving multiple optimization stages. The optimal value of a higher-priority task is preserved while lower-priority tasks are optimized in the remaining feasible space. This provides clear priority semantics but increases computation. For high-rate WBC, weighted QP or hybrid hierarchical formulations may provide a practical compromise between computational cost and predictable relaxation behavior.

Feasibility restoration can also be implemented as a separate optimization phase. When the nominal QP fails, the controller solves a secondary problem whose objective is primarily to minimize constraint violations. The restored solution may not achieve the requested motion, but it identifies a nearby dynamically consistent state from which normal operation can resume. This is preferable to repeatedly attempting the same infeasible command.

Detecting infeasibility requires more than checking a solver return code. Maximum iteration limits, poor numerical conditioning, invalid sensor data, and true physical inconsistency can all produce solver failure. The controller should inspect primal and dual residuals, constraint violations, solver status, numerical values, and execution time before deciding whether the problem is physically infeasible or numerically unsuccessful.

Numerical conditioning can create apparent infeasibility even when a physical solution exists. Poor scaling between torque, force, acceleration, and orientation variables may make the optimization difficult to solve accurately. Regularization, variable scaling, bounded weights, and well-conditioned Jacobians reduce false failure detection. Recovery logic should therefore distinguish formulation problems from genuine loss of physical feasibility.

Contact transitions deserve special handling because the feasible set changes rapidly during touchdown and liftoff. Immediately imposing full stance constraints at touchdown can conflict with residual swing velocity, while removing contact support instantaneously at liftoff can demand abrupt force redistribution. Gradually ramping contact-force bounds and task weights creates a smoother transition between feasible regions.

Unexpected early or late contact can also invalidate the planned optimization structure. If a swing foot touches the terrain early, continued trajectory tracking may demand motion through the surface. If touchdown occurs late, assuming support from a nonexistent contact produces invalid force solutions. Contact estimation should therefore update the WBC constraint set rapidly and trigger appropriate relaxation during uncertain transition periods.

Payload changes can reduce feasibility margins without immediately producing failure. Increased mass raises required support forces and joint torques, while an offset payload changes the distribution of contact loads. A payload-aware WBC can monitor torque, friction, and force margins and begin relaxing acceleration or posture objectives before the QP becomes infeasible. Preventive adaptation is preferable to emergency recovery.

Manipulation introduces additional conflicts because end-effector forces must ultimately be supported by the legs and terrain. A commanded pushing force may exceed the reaction wrench that the stance can sustain. Rather than sacrificing foot stability, WBC should reduce the manipulation force, modify body posture, or request a better stance. Whole-body feasibility therefore determines the actual manipulation capability.

Constraint margins provide useful early-warning indicators. Distance from torque saturation, friction-cone boundaries, joint limits, and feasible contact-force regions can be monitored continuously. As margins shrink, the controller can progressively reduce task aggressiveness. This creates graceful degradation rather than waiting for a binary transition from feasible operation to complete solver failure.

Adaptive task scaling is an effective form of preventive relaxation. Desired body acceleration, swing-foot acceleration, manipulation force, or orientation rate can be multiplied by a scale factor between zero and one. The controller can search for the largest scale that preserves feasibility. The robot then performs as much of the requested motion as its current physical condition allows.

Recovery behavior should remain temporally smooth. Large changes in slack variables or task weights between consecutive cycles can produce discontinuous forces and torques even when each individual QP is feasible. Rate penalties, filtered relaxation variables, force-rate regularization, and gradual priority transitions can reduce command discontinuities while still allowing rapid response to genuine emergencies.

A fallback controller is necessary when optimization-based recovery cannot produce a trustworthy command within the real-time deadline. Depending on robot state, fallback behavior may hold the previous valid torque briefly, switch to a simpler stabilizing controller, lower the body, increase support, stop stepping, or initiate a controlled shutdown. The fallback strategy should be deterministic and validated independently of the nominal WBC.

Repeated infeasibility should be treated differently from an isolated event. A single failed cycle may result from a temporary contact transition or numerical disturbance, whereas persistent failure indicates a structural problem such as incorrect state estimation, damaged hardware, unrealistic commands, insufficient friction, or an invalid model. Supervisory logic should escalate recovery actions according to failure duration and severity.

Recovery can involve modifying the gait rather than only changing optimization weights. A dynamic trot may provide insufficient support margin for a heavy payload or strong manipulation force. Switching toward a slower gait, increasing duty factor, widening stance, lowering the center of mass, or establishing four-foot support can enlarge the feasible region before normal task execution resumes.

The controller should record the cause and magnitude of relaxation for diagnostics. Slack values, saturated constraints, task scaling factors, solver status, contact states, torque margins, and recovery transitions provide valuable information for field debugging. Frequent relaxation of the same constraint often reveals a planning, modeling, calibration, or hardware problem that should be corrected outside the WBC layer.

Simulation is particularly useful for validating infeasibility recovery because extreme conflicts can be generated without risking hardware. Tests can deliberately request excessive acceleration, reduce friction, apply large disturbances, introduce payload errors, force joint limits, or corrupt contact timing. The controller should demonstrate bounded commands, predictable priority degradation, and successful return to normal operation.

Hardware validation should begin with conservative conditions and gradually approach the feasibility boundaries. Force limits, torque saturation, difficult terrain, payload variation, and external disturbances can then be introduced systematically. Evaluation should measure not only whether the robot remains standing, but also relaxation magnitude, recovery time, torque continuity, contact slip, solver timing, and recurrence of infeasibility.

Constraint relaxation should not be viewed as a method for hiding poor controller design. If normal operation continuously requires large slack variables, the reference generator, dynamic model, task hierarchy, actuator sizing, or contact assumptions are probably inappropriate. Relaxation is intended to manage temporary conflicts and uncertainty while exposing persistent structural problems through monitoring and diagnostics.

Robust WBC therefore treats feasibility as a continuously managed resource rather than a binary solver property. Physical margins are monitored before failure, selected objectives are relaxed according to explicit priorities, contact assumptions are adapted when reality differs from the plan, and deterministic fallback behavior protects the robot when optimization cannot recover. This architecture allows a quadruped to degrade gracefully instead of failing abruptly when competing tasks exceed its instantaneous physical capability.

06.09. WBC Torque Command Safety Limiting [w/Code]

Whole-Body Control (WBC) ultimately produces commands that are transmitted to physical actuators, making torque-command safety limiting the final protective boundary between optimization and hardware. Even when the WBC solution is dynamically consistent, commanded torques can become unsafe because of model errors, unexpected contacts, communication delays, disturbances, payload changes, or numerical transients. Safety supervision must therefore remain active immediately before actuation.

Torque limits originate from several physical constraints rather than from a single motor specification. Motor electromagnetic capability, inverter current limits, gearbox strength, bearing loads, structural limits, battery voltage, thermal state, and manufacturer-defined operating envelopes can all restrict allowable joint torque. A safe controller should represent these limits explicitly and distinguish continuous operating capability from short-duration peak capability.

The most direct protection is absolute torque saturation. For each joint, the commanded torque can be constrained between minimum and maximum allowable values before transmission to the actuator. This prevents software from requesting values beyond established hardware limits. However, simple clipping alone is insufficient because independently saturating joints after optimization can destroy the coordinated force distribution originally computed by WBC.

Whenever possible, actuator torque bounds should therefore be incorporated directly into the WBC optimization. If τmin ≤ τ ≤ τmax is enforced as an inequality constraint, the optimizer searches only among solutions that satisfy the available torque range. Body tracking, contact-force distribution, swing motion, or manipulation objectives can then be adjusted consistently when the requested motion approaches actuator capability.

A second independent saturation layer is still valuable after optimization. This final command limiter protects against implementation errors, corrupted solver outputs, invalid numerical values, communication faults, and unexpected software behavior. The optimization constraint and final safety clamp serve different purposes: the first preserves control feasibility, while the second provides a last deterministic hardware-protection boundary.

Torque-rate limiting is equally important because a command can remain within its absolute magnitude limit while changing too rapidly between control cycles. Large torque steps can generate mechanical shock, excite structural vibration, increase gearbox stress, and produce abrupt contact-force changes. A rate limiter constrains the difference between consecutive commands so that actuator loading evolves within a defined temporal envelope.

The allowable torque rate should reflect actuator dynamics and task requirements. Excessively restrictive rate limits can prevent rapid disturbance rejection or touchdown stabilization, while overly permissive limits provide little protection against command discontinuities. Different joints may require different limits because hip, thigh, knee, and arm actuators can have different motor, transmission, inertia, and structural characteristics.

Filtering can further smooth torque commands, but it introduces phase delay. A low-pass filter suppresses high-frequency command noise and numerical oscillation, yet excessive filtering can reduce responsiveness and destabilize fast feedback loops. Filtering should therefore complement physically meaningful torque and torque-rate constraints rather than compensate for poorly conditioned optimization or unstable controller tuning.

Joint torque capability can vary with joint velocity. Electric actuators generally cannot provide the same torque across the complete speed range because motor voltage, back electromotive force, current limits, and transmission characteristics constrain the torque-speed envelope. A fixed torque bound may therefore overestimate available capability at high speed. Velocity-dependent limits provide a more realistic actuator constraint.

Battery voltage also affects available actuator performance. As battery voltage decreases or experiences transient sag under heavy loading, the motor drive may lose the ability to generate the torque predicted under nominal voltage. A safety-aware WBC can use voltage-dependent actuator limits or a supervisory derating factor so that motion objectives remain compatible with the electrical power actually available.

Thermal protection requires a longer time horizon than instantaneous torque limiting. A motor may safely generate high peak torque for a short interval but overheat if the same load persists. Motor winding temperature, inverter temperature, gearbox temperature, and estimated thermal state can therefore be used to reduce allowable torque progressively. This process is commonly treated as thermal derating rather than abrupt shutdown.

Continuous and peak torque limits should be handled differently. Peak torque can support brief events such as impact recovery, stepping, or disturbance rejection, whereas continuous torque defines sustainable operation. A supervisory layer can monitor how long each actuator remains above its continuous rating and reduce the available peak envelope when thermal or electrical energy budgets are being exhausted.

Mechanical limits may be more restrictive than motor limits in some configurations. Gear teeth, shafts, bearings, link structures, and joint housings experience loads resulting from both actuator torque and external contact forces. Consequently, a command that is electrically feasible may still create excessive structural loading. Conservative joint torque bounds can incorporate transmission and structural safety factors where direct load estimation is unavailable.

Torque safety must also consider joint position. Near a mechanical joint stop, even a moderate torque directed further into the limit can create damaging loads. Position-dependent safety logic can reduce allowable torque as a joint approaches its boundary and prevent commands that drive it deeper into the stop. This protection complements joint-position and velocity constraints inside WBC.

Velocity-dependent directional limiting can provide additional protection near joint boundaries. If a joint is approaching its upper limit rapidly, positive torque may be restricted more strongly than negative torque because negative torque assists recovery. Such asymmetric limits preserve the ability to move away from unsafe regions while suppressing commands that increase risk.

Contact conditions strongly influence safe torque commands. A stance leg transmits joint torque into ground reaction force, while a swing leg primarily accelerates its own links. The same numerical torque can therefore have very different physical consequences depending on contact state. Incorrect contact classification may cause a controller to apply large support torques to a foot that is not actually supporting the robot.

Touchdown is particularly sensitive because the transition from swing motion to force support can generate sharp torque changes. Contact detection, force ramping, impedance behavior, and torque-rate limiting should work together so that the leg does not instantaneously apply full stance effort at impact. Smooth load transfer reduces mechanical shock and improves contact stability.

Foot slip requires another safety response. Continuing to increase tangential contact force after friction capability has been exceeded can worsen slipping and destabilize the robot. WBC should maintain contact forces inside friction constraints, while safety supervision can monitor unexpected foot velocity or contact inconsistency and reduce aggressive torque commands when the assumed stance condition becomes unreliable.

External disturbances can legitimately require torque near the actuator limits. Safety limiting should therefore avoid blindly suppressing every large command. The controller must distinguish a necessary short recovery action from sustained overload or abnormal behavior. Peak-duration monitoring, body-state information, contact confidence, and thermal margin can help determine whether high torque should remain temporarily available.

Payload carrying changes the torque safety margin because additional mass increases gravitational and inertial loading. A robot that has substantial torque reserve when unloaded may operate close to continuous limits while carrying cargo. Payload estimation can therefore be connected to safety supervision so that speed, acceleration, body height, gait, and manipulation force are reduced before actuator saturation becomes persistent.

Manipulation arms create similar coupling. A heavy object held far from the body can generate large moments that must ultimately be balanced by arm joints, trunk dynamics, leg torques, and ground reaction forces. Torque limiting should therefore be applied across the complete quadruped-manipulator system rather than protecting the arm and locomotion subsystems independently.

Power limiting provides another safety layer because simultaneous high torque across many joints can exceed battery or inverter capability even when each actuator individually remains within its limit. Electrical power can be approximated from joint torque, velocity, motor current, and drive efficiency. A supervisory controller can reduce task aggressiveness when predicted or measured total power approaches the system limit.

Regenerative operation also requires consideration. During deceleration or downhill locomotion, actuators can return electrical energy toward the DC bus or battery. If the battery cannot accept sufficient regenerative power, bus voltage can rise. Drive-level protection remains essential, while higher-level control can reduce aggressive regenerative braking or distribute deceleration over time and across joints.

Command validation should reject non-finite or corrupted numerical values before they reach the actuator interface. NaN, infinity, uninitialized memory, invalid timestamps, or implausibly large discontinuities should trigger a deterministic safety response. Numerical validation is computationally inexpensive and should be performed even when the upstream solver is considered reliable.

Communication freshness is another part of torque-command safety. A valid torque computed from an old robot state can become unsafe if transmission is delayed or the control process stalls. Commands should therefore carry timing information, and actuator or supervisory watchdogs should detect stale updates. Loss of fresh commands should transition the system toward a predefined safe behavior rather than indefinitely holding arbitrary torque.

Watchdog behavior must be designed according to the robot's physical state. Instantly setting every torque to zero may be inappropriate for a standing quadruped because the body can collapse. Depending on the actuator architecture, a safer response may involve controlled torque reduction, transition to local impedance control, body lowering, or another validated fallback mode before disabling actuation.

Independent safety supervision is valuable because failures can occur inside the primary WBC itself. A separate monitor can check body orientation, joint position and velocity, torque magnitude, torque rate, actuator temperature, battery condition, contact consistency, communication health, and collision indicators. Related quadruped control architectures similarly place joint/torque limits, thermal and electrical protection, fault detection, and emergency-stop supervision around WBC and low-level joint control. Quadruped Stair-Climbing Contro... Quadruped Stair-Climbing Contro...

Safety intervention should be graded rather than purely binary. Small violations may require command clipping or task scaling, shrinking margins may trigger gait reduction or acceleration limits, persistent overload may require body lowering or controlled stopping, and severe faults may require emergency shutdown. A staged response preserves useful robot capability while escalating protection according to actual risk.

The safety layer should communicate limiting information back to WBC and higher-level planners. If a joint is repeatedly torque-limited, the optimizer should not continue requesting the same infeasible motion indefinitely. Saturation flags, thermal derating factors, available torque envelopes, and power margins can be fed back so that gait, foothold, body trajectory, or manipulation objectives are adapted.

Logging is essential for validating torque-command safety. Requested torque, optimized torque, final limited torque, rate-limit activation, actuator temperature, current, voltage, contact state, joint velocity, and safety events should be recorded with synchronized timestamps. Comparing these signals reveals whether limiting occurs only during exceptional events or is continuously compensating for an unsuitable controller or mechanical design.

Simulation and hardware-in-the-loop testing can exercise extreme torque conditions without immediately exposing the robot to damage. Tests should include abrupt disturbances, contact errors, payload changes, joint-limit approaches, communication delays, solver failures, high-speed motion, low battery voltage, and thermal derating. Safety monitoring and staged validation are consistent with the broader simulation-to-HIL-to-real-robot deployment structure used in the supporting quadruped control architecture. Quadruped Stair-Climbing Contro...

Real-robot validation should gradually approach torque and power boundaries while monitoring structural loads, temperature, tracking performance, and stability. Tests should verify not only that commands remain numerically within limits, but also that saturation does not create unexpected balance loss, contact slip, oscillation, or discontinuous behavior. Worst-case transitions are more informative than steady-state operation alone.

Torque-command safety limiting should ultimately be treated as a layered architecture rather than a single clipping function. WBC constraints maintain dynamically consistent commands, rate and magnitude limits suppress dangerous transients, thermal and electrical derating reflect changing hardware capability, watchdogs protect against communication and software faults, and independent supervision provides final system-level protection.

A well-designed torque safety architecture allows a quadruped to use its actuator capability aggressively when necessary without treating maximum performance as permanently available. The controller continuously respects instantaneous torque, rate, thermal, electrical, mechanical, contact, and timing limits while adapting higher-level objectives when margins shrink. Safe torque control therefore becomes an active part of whole-body behavior rather than merely a final saturation operation.

06.10. WBC Validation Simulation and Field Test

Whole-Body Control (WBC) validation determines whether a quadruped controller remains dynamically consistent, stable, safe, and computationally reliable beyond ideal nominal conditions. Validation must examine not only trajectory tracking but also contact-force feasibility, torque limits, timing behavior, disturbance recovery, constraint handling, and transitions between locomotion states. A controller that performs well in one simulated gait is not yet ready for field deployment.

Validation should progress through increasingly realistic environments rather than beginning directly on the physical robot. Mathematical checks and unit tests establish basic correctness, simulation exposes the controller to controlled dynamic conditions, hardware-in-the-loop testing evaluates real interfaces and timing, and physical experiments confirm behavior under actual contact uncertainty. Each stage should define measurable criteria for advancement to the next stage.

The first validation layer concerns the internal consistency of the WBC formulation. Dimensions, coordinate conventions, reference frames, Jacobians, dynamics terms, inequality directions, and contact definitions should be verified independently. Small sign or frame errors can remain hidden during simple standing tests yet become severe during dynamic locomotion. Automated tests should therefore evaluate model quantities over many randomly generated robot configurations.

Rigid-body dynamics can be checked through consistency relationships between forward dynamics, inverse dynamics, and numerical integration. Given identical states and forces, independent computational paths should produce compatible accelerations or torques within defined tolerances. Energy and momentum behavior can provide additional diagnostic information, especially when investigating unexpected motion that appears before optimization or feedback control is introduced.

Kinematic validation should examine forward kinematics, foot positions, body transforms, task Jacobians, and Jacobian derivatives. Numerical differentiation can be compared with analytical Jacobians over randomly sampled configurations. Because WBC converts desired Cartesian behavior into generalized motion through these quantities, even a small kinematic error can appear later as inaccurate foot tracking, incorrect body stabilization, or unexpected contact forces.

Optimization validation should initially use deterministic test cases whose expected behavior is known. Standing on level terrain, symmetric four-foot support, zero desired acceleration, and simple body translations provide useful baselines. The resulting contact forces, generalized accelerations, and torques should satisfy equations of motion, friction constraints, actuator limits, and task requirements before more complex locomotion scenarios are introduced.

Constraint residuals should be measured directly rather than inferred from visible robot behavior. Equality residuals quantify dynamic and contact consistency, while inequality margins reveal proximity to friction, torque, force, and joint limits. Solver success alone does not prove that a solution is physically useful. Validation should therefore record primal residuals, constraint violations, slack variables, and available safety margins for every control cycle.

Simulation provides a controlled environment for testing combinations that would be expensive or dangerous on hardware. Gait speed, terrain slope, friction coefficient, payload mass, center-of-mass offset, external disturbances, sensor noise, and actuator limits can be varied systematically. Parameter sweeps reveal boundaries of the feasible operating region and identify conditions where tracking quality, stability, or optimization reliability begins to degrade.

The simulation model should not be identical to the controller model in every detail. If WBC and simulation share exactly the same masses, inertias, friction assumptions, actuator response, and contact model, validation can become unrealistically favorable. Introducing model mismatch is essential because the physical robot will never match its mathematical representation perfectly. Robustness should be evaluated against plausible uncertainty rather than ideal model agreement.

Contact modeling deserves particular attention because quadruped performance depends strongly on foot-ground interaction. Simulation should include variations in friction, compliance, restitution, surface inclination, and contact timing. Tests should intentionally create early touchdown, delayed touchdown, partial support, and slipping. The objective is to determine whether contact estimation and WBC constraints remain stable when actual contact differs from the planned contact schedule.

Disturbance testing evaluates recovery capability. External forces or impulses can be applied to the trunk from different directions and at different phases of the gait. The magnitude should increase gradually until recovery boundaries are identified. Useful measurements include body displacement, orientation error, recovery time, peak joint torque, contact-force margin, number of recovery steps, and whether the controller changes gait or activates fallback behavior.

Payload validation should vary both mass and load location. A centered payload primarily changes total weight and inertia, while an offset payload creates asymmetric contact requirements and gravitational moments. Tests should evaluate whether load estimation, force redistribution, torque limiting, and motion scaling respond correctly. Sudden pickup or release events are especially useful for examining transient adaptation of the WBC model.

Manipulation-capable quadrupeds require combined locomotion and arm validation. Arm extension, object lifting, pushing, pulling, and end-effector tracking can shift the center of mass and generate external reaction forces. Simulation should evaluate whether leg forces and body posture compensate appropriately. Tasks that are feasible for the arm alone may become infeasible when whole-body contact and friction constraints are considered.

Monte Carlo testing can expose combinations of uncertainty that individual parameter sweeps may miss. Robot mass, inertia, friction, sensor bias, terrain geometry, communication delay, and external disturbances can be randomized across repeated trials. Statistical distributions of tracking error, solver failure, slip occurrence, torque saturation, and recovery success provide a broader measure of robustness than a small number of hand-selected demonstrations.

Real-time computational validation must occur alongside dynamic validation. A WBC designed for 1 kHz operation should be tested under worst-case computational conditions rather than only nominal standing. Contact transitions, active torque constraints, manipulation tasks, near-singular configurations, payload adaptation, and recovery behavior can increase computation. Mean execution time alone is insufficient; maximum and high-percentile latency must also remain within the control budget.

Software stress testing should introduce delayed sensor packets, missing messages, stale timestamps, invalid numerical values, solver non-convergence, and temporary communication interruptions. These failures may not appear in ordinary simulation but are realistic in deployed systems. The controller should respond deterministically by rejecting corrupted data, preserving bounded commands, activating fallback logic, or entering a controlled safe state.

Hardware-in-the-loop (HIL) testing provides an intermediate stage between software simulation and unrestricted robot experiments. The WBC software can execute on the intended onboard computer while communicating through real interfaces and timing paths, with simulated robot dynamics or selected hardware components replacing the full machine. This exposes communication latency, scheduling jitter, driver behavior, synchronization errors, and computational bottlenecks before dynamic hardware risk is introduced.

Actuator-level validation should precede full-body dynamic testing. Individual joints or limbs can be tested for torque tracking, position sensing, velocity estimation, communication timing, saturation behavior, thermal response, and emergency shutdown. Commanded and measured torque should be compared across representative operating ranges. Unexpected actuator behavior should be resolved before relying on WBC to coordinate the complete robot.

Initial whole-robot hardware tests should begin with constrained and low-energy conditions. Supported standing, reduced torque limits, low body height, slow posture changes, and conservative contact-force targets reduce risk while validating sign conventions and force distribution. Mechanical support systems or safety tethers may be appropriate during early tests, provided they do not introduce unmodeled forces that invalidate the measurements.

Static standing is a useful first WBC hardware benchmark because expected force distribution can be estimated directly. On level ground with a centered center of mass, vertical forces should exhibit physically reasonable sharing among the feet. Controlled body translations and orientation changes can then verify whether force distribution moves predictably while torque, friction, and joint limits remain satisfied.

Dynamic testing should progress gradually from slow stepping to walking, trotting, turning, slopes, irregular terrain, and disturbance recovery. Only one major source of difficulty should be increased at a time when practical. This progression helps isolate failures. Jumping immediately from static standing to aggressive dynamic locomotion can make it difficult to determine whether a failure originates from estimation, dynamics, contact handling, optimization, or hardware.

Terrain validation should include surfaces with different friction, stiffness, geometry, and inclination. Flat laboratory floors are insufficient for field-oriented quadrupeds. Ramps, small obstacles, uneven ground, compliant surfaces, and controlled low-friction regions expose weaknesses in contact assumptions. Terrain difficulty should increase only after baseline locomotion demonstrates repeatable stability and adequate safety margins.

Field testing introduces uncertainty that is difficult to reproduce completely in simulation. Terrain geometry may be poorly observed, friction can vary between individual footholds, environmental disturbances can occur unexpectedly, and sensing quality can change with vibration, lighting, temperature, dust, or moisture. Field validation therefore examines whether the complete perception, estimation, planning, WBC, and actuator chain operates reliably as an integrated system.

Field experiments should be organized around explicit operating envelopes rather than informal demonstrations. Speed, payload, slope, terrain roughness, manipulation force, temperature, battery condition, and disturbance level should have defined ranges. Successful operation inside these ranges establishes a validated envelope, while failures near its boundaries provide information for improving controller limits and supervisory safety policies.

Repeatability is essential. A robot completing a difficult maneuver once does not establish reliable performance. The same scenario should be repeated sufficiently to characterize variation in tracking error, contact behavior, solver timing, and recovery success. Repeated trials are particularly important for contact-rich locomotion because small differences in foothold location or surface condition can produce substantially different dynamic responses.

Validation metrics should connect directly to WBC objectives and constraints. Useful quantities include body position and orientation error, swing-foot tracking error, ground reaction force error, friction margin, joint torque margin, joint-limit margin, solver residuals, computation time, energy consumption, slip distance, recovery time, and constraint-relaxation magnitude. No single metric is sufficient to characterize whole-body performance.

Safety metrics should be evaluated separately from nominal performance. The controller should record torque saturation, thermal derating, watchdog activation, communication faults, infeasible optimization events, fallback transitions, excessive body attitude, unexpected contact, and emergency stops. A system that achieves excellent trajectory tracking but repeatedly approaches unsafe limits should not be considered successfully validated.

Failure cases should be preserved as regression tests. Once a specific combination of terrain, contact timing, payload, or disturbance produces a failure, the scenario should be reproduced in simulation whenever possible and added to the automated validation suite. After controller modifications, previously solved failures should be retested to ensure that improvements in one area have not introduced regressions elsewhere.

Simulation-to-reality discrepancies should be treated as engineering information rather than merely as imperfections. Differences in actuator bandwidth, structural compliance, backlash, friction, sensor delay, contact deformation, and state estimation can explain why physical behavior differs from simulation. Recorded field data can be replayed or used to refine simulation parameters, creating an iterative loop between modeling, control development, and experimental validation.

Acceptance criteria should be defined before final field qualification. Required tracking accuracy, maximum allowable torque usage, minimum friction margin, solver success rate, deadline compliance, recovery success, payload capability, and fault-response behavior should be specified quantitatively wherever possible. This prevents subjective judgments based only on visually impressive demonstrations and creates a repeatable engineering basis for release decisions.

WBC validation is therefore not a final test performed after controller development but a continuous process spanning mathematical verification, simulation, software stress testing, HIL evaluation, controlled hardware experiments, and field trials. Each stage exposes different classes of failure and feeds evidence back into controller design. The result is a progressively validated operating envelope rather than an assumption that successful simulation automatically implies reliable physical performance.

A mature validation process ultimately demonstrates that the quadruped can maintain dynamically consistent behavior when reality differs from the nominal plan. Contact uncertainty, model mismatch, payload variation, actuator limitations, computational jitter, disturbances, and environmental complexity must all be represented in testing. Reliable WBC emerges when performance, feasibility, timing, and safety are validated together across simulation and real-world operation.


Author: Richard Minsoo Go

LinkedIn: Richard Minsoo Go

Top comments (0)