DEV Community

Richard Minsoo Go
Richard Minsoo Go

Posted on

Robot Control Software — Ch.07: Whole Body Control [EN]

STATERA Library · Series 08 · Volume 04 · Chapter 07 · EN
Source: https://github.com/Statera-Library/08_Vol_04_Robot_Control_Software/blob/main/Chapter_07_Whole_Body_Control/Chapter_07_Whole_Body_Control_EN.md

Volume 04 Robot Control Software

07. Whole Body Control

07.01 Whole Body Control (WBC) Concept: Task Space Priorities

Whole Body Control (WBC) is a unified control framework that coordinates all available degrees of freedom of a robot to achieve multiple motion and interaction objectives simultaneously. Instead of controlling the base, torso, arms, and legs as independent subsystems, WBC treats the robot as one coupled mechanical system. This approach is especially important for humanoids, mobile manipulators, quadrupeds, and other robots whose tasks require coordinated motion across several joints and contact points.

The central idea of WBC is to convert high-level behavioral objectives into consistent commands for the complete robot. A navigation module may request base motion while a manipulation module specifies an end-effector pose, and a stabilization module simultaneously requires balance. Whole Body Control combines these objectives while respecting the robot\'s kinematic structure, dynamic behavior, actuator capabilities, joint limits, and environmental contact conditions.

Task space provides a natural representation for expressing many WBC objectives. Rather than specifying every joint angle directly, the controller describes desired quantities such as hand position, foot pose, center-of-mass location, torso orientation, or interaction force. These quantities are defined in Cartesian or other operational spaces that correspond directly to physical behaviors. The controller then determines the joint-level motion or torque required to realize them.

A task can generally be represented by a function of the robot configuration, such as x = f(q), where q denotes generalized coordinates and x represents a task-space variable. The differential relationship is commonly written as x_dot = J(q)q_dot, where J is the task Jacobian. Through this mapping, desired task-space velocities or accelerations can be transformed into coordinated commands involving many joints rather than a single actuator.

Because a robot often needs to satisfy several objectives simultaneously, WBC requires a mechanism for managing competing tasks. For example, a humanoid reaching toward an object must preserve balance while moving its hand, maintaining suitable foot contacts, and avoiding joint limits. These requirements cannot always be satisfied perfectly at the same time. Task priorities therefore define which objectives must be preserved when conflicts occur.

A strict hierarchical approach assigns different priority levels to individual tasks. Safety-critical constraints such as maintaining valid contacts, preventing joint-limit violations, or preserving balance typically occupy higher levels, while manipulation accuracy, posture optimization, or secondary configuration objectives may occupy lower levels. Lower-priority tasks are executed only within motion directions that do not interfere with the solutions established by higher-priority tasks.

Null-space projection is a common method for implementing this hierarchy in kinematic controllers. After a high-priority task determines part of the robot motion, the remaining degrees of freedom can be used for secondary objectives. A lower-priority command is projected into the null space of the higher-priority task Jacobian. Consequently, redundant joints can improve posture or manipulability without significantly disturbing the primary end-effector or balance objective.

Task priorities may also be implemented through hierarchical quadratic programming. In this formulation, each control level solves an optimization problem subject to constraints inherited from higher levels. Equality constraints can represent desired motion or contact relationships, while inequalities describe joint limits, torque bounds, friction cones, collision margins, or acceleration limits. Hierarchical optimization provides a systematic mechanism for coordinating complex robots under numerous physical restrictions.

Not every application requires strict priorities. Weighted optimization combines several objectives into a common cost function and assigns numerical weights according to their relative importance. This method can provide smooth compromises when objectives compete, but the resulting behavior depends strongly on weight selection and scaling. Strict hierarchy is preferable when one task must never be degraded by another, whereas weighted formulations are useful when controlled trade-offs are acceptable.

Dynamic Whole Body Control extends the task-space concept beyond geometry by explicitly considering robot dynamics. A typical rigid-body model relates generalized acceleration, inertia, Coriolis and centrifugal effects, gravity, actuator forces, and external contact forces. The controller solves for quantities such as joint torques, accelerations, and contact forces that produce the requested task behavior while remaining dynamically feasible.

Contact management is particularly important for legged and physically interactive robots. Feet, wheels, grippers, or support surfaces create constraints that modify the available motion of the entire body. WBC must account for whether contacts are fixed, sliding, rolling, or transitioning between states. Contact forces must also remain physically valid, including requirements associated with unilateral contact and friction limits, otherwise the mathematically calculated motion may not be executable.

For humanoid and legged robots, balance-related tasks frequently occupy high positions in the control hierarchy. Center-of-mass regulation, centroidal momentum control, support-region constraints, and foot-contact conditions can be coordinated with manipulation tasks. The robot may therefore move its torso and unused joints to compensate for arm motion, allowing an end-effector to reach a target while maintaining dynamically consistent whole-body stability.

Mobile manipulators introduce another form of redundancy because both the mobile base and manipulator contribute to end-effector motion. WBC can distribute a Cartesian command between these subsystems according to task requirements. Small adjustments may primarily use arm joints, while distant targets can cause coordinated base movement. Secondary objectives can maintain favorable arm configurations, improve manipulability, reduce energy consumption, or keep the platform away from obstacles.

Task activation and transition management are essential for practical implementations. Robotic behavior frequently changes between standing, walking, reaching, grasping, carrying, and releasing. Abruptly adding or removing tasks can generate discontinuities in velocity, acceleration, or torque commands. Smooth activation functions, trajectory blending, and controlled priority transitions help maintain stable behavior as the active task set changes during operation.

Whole Body Control usually operates within a layered software architecture. Perception and planning modules determine environmental state and desired behavior, while trajectory generators convert those decisions into time-dependent references. WBC receives these references together with estimated robot state and produces joint-level commands. Fast motor controllers then execute position, velocity, or torque targets while sensors continuously provide feedback for the next control cycle.

State estimation quality directly affects WBC performance because task-space calculations depend on accurate knowledge of joint states, body pose, velocity, and contacts. Encoders, inertial sensors, force-torque sensors, vision, and other measurements may be fused to estimate the current configuration. Errors in base orientation or contact state can propagate through the whole-body model, making robust estimation and synchronized sensor data important components of the control system.

Real-time implementation requires careful management of computational complexity and numerical conditioning. Jacobians, dynamic matrices, constraint equations, and optimization problems may need to be updated hundreds or thousands of times per second. Singular configurations and nearly dependent tasks can make numerical solutions unstable. Regularization, damping, task scaling, solver warm-starting, and appropriate matrix factorization techniques are therefore important for reliable execution.

The practical value of WBC lies in separating behavioral intent from actuator coordination. Higher software layers can request meaningful objectives such as "keep the torso upright," "move the hand to this pose," or "maintain support," while the whole-body controller determines how available joints and contact forces should cooperate. This abstraction reduces the need for every application module to understand the detailed coordination of the complete mechanical system.

A robust WBC design ultimately combines task-space control, priority management, robot dynamics, physical constraints, contact reasoning, state estimation, and real-time optimization. Task priorities provide the structure needed to resolve conflicts, while redundancy allows unused degrees of freedom to support secondary objectives. Together, these mechanisms enable complex robots to perform coordinated motion and interaction while preserving feasibility, stability, and safety across the entire body.

07.02 Hierarchical Quadratic Programming (HQP) [w/Code]

Hierarchical Quadratic Programming (HQP) is an optimization framework widely used in Whole Body Control to coordinate multiple robot objectives while preserving explicit priority relationships. Unlike a single weighted optimization problem, HQP organizes tasks and constraints into ordered levels. A solution at a lower level is permitted only when it does not degrade objectives already established at higher-priority levels.

The motivation for HQP arises from the fact that complex robots must satisfy many objectives simultaneously. A humanoid may need to maintain balance, preserve foot contacts, track hand motion, regulate torso orientation, and optimize posture at the same time. When these objectives conflict, simply assigning numerical weights may produce undesirable compromises. HQP instead represents importance directly through a mathematical hierarchy.

At each hierarchy level, a quadratic program minimizes a cost function subject to equality and inequality constraints. A typical quadratic objective can be written as minimizing 1/2 xᵀHx + fᵀx, where x contains decision variables and H and f define the optimization objective. Depending on the controller architecture, x may include generalized accelerations, joint torques, contact forces, task errors, or slack variables.

Equality constraints are commonly used to represent relationships that should be satisfied precisely or with controlled relaxation. Examples include rigid contact conditions, equations of motion, desired Cartesian acceleration, and kinematic relationships between robot bodies. These constraints can be expressed in a linearized form such as Ax = b, allowing the quadratic programming solver to incorporate them directly into the optimization problem.

Inequality constraints are particularly important because physical robots operate within finite limits. Joint positions, velocities, accelerations, actuator torques, contact forces, friction conditions, and collision distances all impose admissible ranges on the solution. These conditions can typically be represented as lower and upper bounds or linear inequalities, enabling HQP to prevent commands that violate physical or safety-related restrictions.

The hierarchy distinguishes HQP from ordinary Quadratic Programming (QP). Suppose balance preservation is assigned to the highest task level, end-effector tracking to the second level, and posture optimization to the third. The controller first obtains the best feasible balance solution. The manipulation task is then optimized without worsening that result, and posture is finally improved only within the remaining freedom available after both higher levels are protected.

This structure is closely related to the concept of null-space control. In classical redundancy resolution, a secondary task is projected into the null space of a higher-priority Jacobian. HQP generalizes this idea by handling equality constraints, inequality constraints, actuator limits, contacts, and dynamically feasible quantities within a common optimization framework. It therefore supports control problems that are difficult to manage through Jacobian projection alone.

HQP can operate at the kinematic level using joint velocities or accelerations as optimization variables. A desired task-space motion is mapped through a Jacobian relationship, and the optimization determines robot motion that best satisfies the ordered objectives. This formulation is useful for trajectory generation, posture control, and manipulation when detailed force dynamics are not the primary concern or are handled by lower control layers.

Dynamic HQP incorporates the full equations of motion and is particularly useful for humanoids, quadrupeds, and robots interacting strongly with their environment. Generalized acceleration, actuator torque, and contact force may be optimized simultaneously. The resulting solution must satisfy rigid-body dynamics while also respecting contact conditions, friction constraints, torque limits, and task-space acceleration requirements.

Contact constraints introduce an important set of inequalities into dynamic HQP. A supporting foot or wheel cannot generally pull on the ground, and tangential contact forces must remain consistent with available friction. Friction cones are therefore represented directly or approximated using linear friction pyramids. These constraints allow the optimizer to calculate contact forces that support desired whole-body motion without requesting physically impossible interactions.

Slack variables provide controlled relaxation when a task cannot be satisfied exactly. Instead of declaring the complete optimization problem infeasible, the controller introduces an error variable and minimizes its magnitude at the corresponding priority level. High-priority safety constraints may remain hard constraints, while tracking objectives can use bounded slack. This distinction enables the system to continue operating gracefully when environmental or mechanical limitations prevent perfect execution.

Constraint feasibility must be considered carefully because conflicting hard constraints can make an HQP level unsolvable. For example, a requested acceleration may be incompatible with joint limits, torque limits, and fixed contact conditions simultaneously. Controller designers therefore distinguish immutable physical constraints from behavioral objectives that may be relaxed. Feasibility restoration strategies can progressively modify noncritical requirements while preserving essential safety conditions.

Task scaling provides another method for maintaining feasibility. When a desired motion exceeds the robot\'s available velocity, acceleration, or torque capability, the controller can reduce the magnitude of the task while preserving its direction or geometric intent. In hierarchical control, scaling must respect priority relationships so that modifying a lower-level task does not alter the solution quality guaranteed to higher-priority objectives.

The ordering of tasks strongly influences robot behavior and should reflect functional requirements rather than arbitrary software structure. Physical feasibility, collision avoidance, critical contact constraints, and balance commonly receive high priority. Manipulation or locomotion tracking may occupy intermediate levels, while posture, manipulability, energy reduction, and preferred joint configurations are often placed lower. The exact hierarchy depends on the robot and operating scenario.

HQP is especially valuable for redundant robots because additional degrees of freedom can be systematically allocated to secondary objectives. A humanoid can maintain foot contacts and center-of-mass behavior while using its arms for manipulation and its torso for additional reach. A mobile manipulator can coordinate base and arm motion while preserving obstacle clearance. Remaining redundancy can then improve posture, joint-limit margins, or manipulability.

Task activation must be handled smoothly when the hierarchy changes during robot operation. Walking introduces and removes foot-contact constraints, grasping establishes new interaction relationships, and manipulation tasks may appear or disappear according to mission state. Abrupt changes in active constraints can produce discontinuous acceleration or torque commands. Transition functions and task blending are therefore commonly used around contact and behavioral transitions.

Real-time execution is a major engineering requirement because HQP problems may need to be solved at high control frequencies. Every control cycle can require updated Jacobians, dynamic matrices, contact models, constraint bounds, and task references. Efficient sparse matrix operations, warm-starting, active-set methods, interior-point methods, and specialized hierarchical solvers can reduce computation time while maintaining sufficiently accurate solutions.

Numerical conditioning also affects controller reliability. Tasks with substantially different physical units or magnitudes can create poorly scaled optimization matrices. Nearly singular Jacobians and redundant constraints may further degrade solver performance. Appropriate normalization, regularization, damping, constraint preprocessing, and rank detection help prevent numerical problems from becoming unstable robot commands, particularly near singular configurations.

State estimation and model accuracy remain essential even when the optimization itself is mathematically correct. HQP depends on estimated joint states, floating-base motion, contact states, inertial parameters, and external forces. Errors in these quantities can cause the optimizer to solve the wrong physical problem. Robust estimators, synchronized sensing, model calibration, and feedback control are therefore required around the optimization layer.

In a practical Whole Body Control architecture, planners provide desired task trajectories while the HQP layer converts them into dynamically or kinematically consistent commands. The optimizer receives the current robot state, model information, contact configuration, task hierarchy, and physical limits. Its output may consist of desired joint acceleration, torque, and contact force commands that are subsequently executed by high-frequency actuator controllers.

HQP provides a systematic bridge between high-level robotic intent and low-level physical feasibility. Its primary strength is not merely solving several optimization objectives, but guaranteeing that their priority structure remains explicit during conflict resolution. By combining hierarchy, constraints, redundancy resolution, feasibility management, and real-time optimization, HQP enables sophisticated robots to coordinate whole-body behavior while respecting safety and physical limitations.

07.03 Contact Modeling and Friction Cone Constraints [w/Code]

Contact modeling is a fundamental component of Whole Body Control because robots frequently exchange forces with their environment through feet, wheels, hands, grippers, or other surfaces. A controller must determine not only how the robot should move but also whether the required interaction forces can physically exist. Contact models provide mathematical relationships that connect robot motion, environmental geometry, and admissible forces.

A contact is usually characterized by its position, orientation, surface normal, and allowable relative motion. Depending on the application, it may be modeled as a point contact, point contact with friction, line contact, planar surface contact, or more complex distributed contact. The selected representation determines which forces and moments can be transmitted between the robot and environment and therefore directly affects the control solution.

For a rigid contact, the contacting robot body is assumed to have no relative motion with respect to the environment along constrained directions. At the velocity level, this condition can be expressed using a contact Jacobian as Jc(q)q_dot = 0. At the acceleration level, the corresponding relationship includes both Jc q_ddot and its time derivative, providing a constraint that can be incorporated into inverse dynamics or optimization-based control.

Contact forces enter the robot equations of motion through the transpose of the contact Jacobian. A common floating-base dynamic model can be expressed as M(q)q_ddot + h(q,q_dot) = Sᵀτ + Jcᵀfc, where M is the inertia matrix, h represents nonlinear effects, τ contains actuator torques, and fc represents contact forces. Whole Body Control must find these quantities consistently with the desired robot motion.

A fundamental property of ordinary ground contact is unilateral behavior. A rigid floor can push against a robot but cannot normally pull its foot toward the surface. If the contact normal points outward from the surface, the normal force must therefore remain nonnegative. This unilateral contact condition distinguishes environmental support from mechanically attached joints and becomes an essential inequality constraint in optimization-based controllers.

Tangential contact forces are limited by friction. Under the classical Coulomb friction model, the magnitude of the tangential force must not exceed the coefficient of friction multiplied by the normal force. This relationship is commonly expressed as ||ft|| ≤ μfn, where ft is the tangential force, fn is the normal force, and μ is the friction coefficient. The resulting admissible force region forms the friction cone.

The friction cone provides an intuitive physical interpretation of contact feasibility. A contact force vector lying inside the cone can theoretically be supported without sliding under the assumed friction coefficient. A force vector reaching the cone boundary corresponds to impending slip, while a vector outside the cone requires more tangential friction than the surface can provide. Whole Body Control therefore constrains optimized contact forces to remain inside this region.

The exact Coulomb friction cone is nonlinear because its cross section is circular. Real-time optimization systems often approximate it using a polyhedral friction cone or friction pyramid composed of linear inequalities. For example, tangential force components may be bounded independently according to the normal force. This approximation converts friction requirements into linear constraints that can be handled efficiently by Quadratic Programming and Hierarchical Quadratic Programming.

The quality of a friction-pyramid approximation depends on the number and orientation of its faces. A small number of faces reduces computational cost but may conservatively restrict feasible forces or approximate the true cone poorly. Increasing the number of faces improves geometric accuracy at the expense of additional inequalities. Controller design therefore balances physical fidelity against the computational requirements of the real-time optimization loop.

Planar contacts, such as a humanoid foot resting on the ground, can transmit both forces and moments. In this case, friction constraints alone are insufficient. The resultant wrench must also satisfy conditions related to the finite support area. Center of pressure, allowable moments, and support polygon boundaries can be incorporated into the contact wrench constraints so that the calculated interaction remains physically realizable across the contact surface.

The center of pressure represents the effective location at which the resultant ground reaction force acts on a support surface. For stable planar contact, this point generally needs to remain within the valid support region of the foot or contact patch. Optimization-based Whole Body Control can impose corresponding inequalities on contact forces and moments, preventing solutions that would require the foot to generate an impossible rotational support condition.

Contact modeling becomes more complex when multiple contacts are active simultaneously. A standing humanoid may distribute its weight between two feet, while a robot climbing or manipulating heavy objects may use hands and feet together. The optimizer must determine how total forces and moments should be distributed among contacts while satisfying individual friction cones, unilateral conditions, actuator limits, and whole-body dynamic equations.

Force distribution is not unique when several contacts provide redundant support. This redundancy can be exploited to improve stability, reduce actuator effort, increase friction margins, or avoid excessive loading at a particular contact. Optimization objectives may therefore regulate contact-force distribution in addition to motion tracking. Such objectives are usually assigned priorities that preserve fundamental feasibility and balance requirements.

The assumed coefficient of friction strongly influences the feasible contact-force region. An optimistic value may allow the controller to request forces that cause real-world slipping, whereas an excessively conservative value can unnecessarily reduce robot capability. Practical systems may use experimentally identified coefficients, environment-dependent estimates, or conservative design values. Some advanced controllers can adapt friction estimates from measured contact behavior.

Contact state estimation is equally important because the controller must know which constraints are currently valid. Force-torque sensors, joint torque estimates, tactile sensors, inertial measurements, and kinematic consistency can help determine whether a foot is firmly supported, beginning to slip, or losing contact. Incorrect contact classification can cause significant control errors because the optimization model may assume forces or constraints that do not physically exist.

Contact transitions require special handling in locomotion and manipulation. During walking, a foot changes from swing motion to impact and then to stance, while another foot may later leave the ground. Similarly, a hand transitions from free motion to grasping or pushing. Instantly switching between unconstrained and rigid-contact models can create discontinuities, so controllers often use planned transition phases, force ramps, and smooth task activation.

Impact introduces dynamics that differ from continuous contact. When a moving body establishes contact, velocity can change rapidly and impulsive forces may occur over a short interval. Although high-level Whole Body Control often treats the post-impact state as a new constrained phase, trajectory planning should reduce excessive impact velocity. More advanced systems may explicitly model impact impulses and momentum changes during contact establishment.

Rigid-contact assumptions are useful but do not represent every environment accurately. Soft terrain, compliant feet, deformable objects, tires, and elastic grippers can exhibit measurable displacement under load. Compliance may be modeled using spring-damper relationships or more detailed contact mechanics. Controllers can also incorporate impedance or force-control behavior so that interaction remains stable despite uncertainty in environmental stiffness and contact geometry.

Contact constraints interact directly with task priorities in Hierarchical Quadratic Programming. Maintaining a valid support contact and satisfying friction limits typically receive higher priority than end-effector tracking or posture optimization. If a requested manipulation motion would require a ground reaction force outside the friction cone, the controller should modify the lower-priority motion rather than violate the physical contact condition.

Real-time implementation requires the contact model to remain computationally manageable. Contact Jacobians, surface frames, friction inequalities, support constraints, and force bounds must be updated as the robot moves. Sparse matrix structures and linearized constraints are widely used because they allow optimization solvers to process multiple contacts at high frequency. Numerical scaling is also important because forces, moments, positions, and accelerations have different units.

Robust contact control must account for uncertainty in friction, surface orientation, state estimation, and robot parameters. Safety margins can shrink the nominal friction cone so that optimized forces remain away from the predicted slip boundary. Similar margins may be applied to center-of-pressure limits and contact-force bounds. Although conservative margins reduce the theoretical operating envelope, they can improve reliability when the physical environment is imperfectly known.

Contact modeling ultimately provides the physical foundation that connects Whole Body Control calculations to real environmental interaction. By combining kinematic contact constraints, unilateral force conditions, friction cones, support-region limits, contact-state estimation, and dynamic equations, the controller can distinguish mathematically attractive motions from physically executable ones. This enables humanoids, legged robots, and mobile manipulators to move and interact while maintaining stable, feasible contact with their surroundings.

07.04 Inertia Property Matrix Computation and Update [w/Code]

Inertia property computation is a fundamental part of Whole Body Control because dynamic control depends on an accurate representation of how robot mass is distributed throughout the mechanism. Each link contributes mass, center-of-mass position, and rotational inertia to the complete system. These properties determine how strongly the robot responds to joint torques, external forces, gravity, acceleration, and changes in contact configuration.

For each rigid link, the basic inertial parameters consist of mass m, center of mass c, and a 3 × 3 rotational inertia tensor I. The inertia tensor describes resistance to angular acceleration about different axes and includes both moments and products of inertia. These quantities are normally defined in a link-fixed reference frame and transformed appropriately when constructing the dynamics of the complete articulated robot.

The rotational inertia tensor is symmetric and can be expressed using three diagonal moments of inertia and three independent products of inertia. When the coordinate frame is aligned with the principal inertia axes, the off-diagonal terms become zero and the tensor becomes diagonal. Robot CAD models frequently provide these properties, but their coordinate conventions and reference points must be verified before they are transferred into the control model.

When inertia is known about one point but required about another, the parallel-axis theorem is used. If the inertia tensor is specified about the center of mass, it can be translated to another reference point using the link mass and displacement between the two locations. Correct application of this transformation is essential because even accurate CAD inertia values become incorrect if they are associated with the wrong origin.

Spatial vector formulations provide a convenient representation for modern rigid-body dynamics algorithms. A spatial inertia matrix combines translational mass properties, center-of-mass offset, and rotational inertia into a unified 6 × 6 matrix. This representation allows linear and angular quantities to be processed consistently and supports efficient recursive algorithms for inverse dynamics, forward dynamics, and composite rigid-body calculations.

For an articulated robot, individual link inertias are combined according to the current kinematic configuration. The resulting generalized mass matrix M(q), sometimes called the joint-space inertia matrix, maps generalized acceleration to inertial generalized force. Because the relative orientation and position of robot links change with joint configuration q, the numerical values of M(q) generally change continuously as the robot moves.

The Composite Rigid Body Algorithm (CRBA) is widely used to compute the generalized mass matrix efficiently. Starting from the outer links and moving toward the base, the algorithm recursively accumulates the spatial inertias of connected bodies. These composite inertias are then projected through joint motion subspaces to construct M(q). CRBA avoids repeatedly evaluating dynamics independently for every generalized coordinate.

Alternative dynamics algorithms may obtain equivalent information without explicitly constructing every intermediate matrix. The Recursive Newton-Euler Algorithm efficiently computes inverse dynamics, while the Articulated Body Algorithm is commonly used for forward dynamics. Whole Body Control software may select among these algorithms depending on whether it requires the full inertia matrix, inverse dynamics terms, acceleration prediction, or combinations of these quantities.

The generalized inertia matrix has important mathematical properties. For a physically valid rigid-body model, M(q) should be symmetric and positive definite for independent generalized coordinates. Symmetry reflects the energy structure of mechanical systems, while positive definiteness ensures positive kinetic energy for nonzero generalized velocity. Violations often indicate modeling errors, numerical problems, or invalid inertial parameters.

Floating-base robots require special treatment because the base is not rigidly attached to the world. Humanoids, quadrupeds, and many mobile robots therefore include six unactuated base degrees of freedom together with actuated joints. Their generalized inertia matrix represents coupling between base translation, base rotation, and internal joint motion. This coupling is essential for predicting whole-body reactions to limb movement and contact forces.

The inertia matrix is closely related to other terms in the rigid-body equations of motion, commonly represented as M(q)q_ddot + h(q,q_dot) = Sᵀτ + Jcᵀfc. Here h includes Coriolis, centrifugal, and gravitational effects, while contact forces enter through the contact Jacobian. Accurate computation of M(q) is therefore necessary for inverse dynamics, torque optimization, momentum regulation, and dynamically consistent task control.

Whole Body Control generally updates configuration-dependent dynamic quantities during every control cycle. Joint encoders and state estimation provide the latest generalized configuration and velocity, after which forward kinematics, transforms, Jacobians, inertia quantities, and nonlinear dynamic terms are recomputed. At high control frequencies, these calculations must be efficient enough that optimization and actuator command generation still complete within the available cycle time.

Not all inertial properties change at the same rate. The intrinsic mass and inertia tensor of a rigid robot link remain constant unless hardware changes, whereas their representation in a common coordinate frame changes continuously with robot configuration. Separating constant model parameters from configuration-dependent transformations avoids unnecessary computation and provides a cleaner software architecture for real-time dynamics updates.

Payload handling introduces an important exception to fixed inertial parameters. When a manipulator grasps a tool, package, or other object, the effective mass distribution of the robot changes. The payload mass, center of mass, and inertia tensor should therefore be incorporated into the dynamic model. Ignoring a substantial payload can cause incorrect torque prediction, degraded tracking, unstable interaction, or inaccurate contact-force allocation.

Payload inertia can be represented as an additional rigid body attached to the end-effector or merged into the inertial parameters of the terminal link. The appropriate transformation must account for the payload pose relative to the attachment frame. If the payload moves internally or changes configuration, its inertia cannot be treated as a single constant addition and a more detailed articulated or time-varying model may be required.

Online payload estimation can improve performance when object properties are not known beforehand. Measurements of joint torque, acceleration, force-torque sensing, and robot motion can be used to estimate payload mass and center-of-mass location. More advanced identification methods can estimate additional inertial parameters. Updated estimates can then be introduced gradually into the controller to prevent sudden changes in commanded torque.

Inertial parameter identification is also useful for correcting differences between CAD models and manufactured robots. Motors, cables, covers, fasteners, sensors, batteries, and assembly tolerances can cause the actual mass distribution to differ from nominal design data. Excitation trajectories and measured joint responses can be used to identify dynamically relevant parameter combinations and improve agreement between predicted and observed behavior.

Physical consistency must be preserved when inertial parameters are estimated or modified. Mass must remain positive, and the inertia tensor must correspond to a realizable mass distribution. Its principal moments must satisfy appropriate relationships, while the combined spatial inertia should remain physically valid. Constrained identification methods are often preferable because unconstrained numerical estimation can produce parameters that fit data but violate mechanical physics.

Numerical conditioning is important when computing and using inertia matrices. Robots may contain links whose masses and inertias differ by several orders of magnitude, producing poorly scaled dynamic equations. Near-singular kinematic configurations can further complicate task-space calculations derived from M(q). Proper units, matrix factorization, regularization where appropriate, and careful avoidance of unnecessary explicit matrix inversion improve numerical reliability.

Task-space control often requires an operational-space inertia matrix that describes the apparent inertia observed at an end-effector or another task coordinate. In simplified form, this quantity is related to the joint-space inertia and task Jacobian through expressions involving J M⁻¹ Jᵀ. It captures how the complete robot configuration affects resistance to acceleration in task space and is important for dynamically consistent control and force interaction.

Centroidal dynamics also depend directly on the distribution of link inertias. The centroidal momentum matrix maps generalized velocity to the robot\'s total linear and angular momentum about the center of mass. As the configuration changes, limb motion redistributes inertia and modifies momentum behavior. Humanoid and legged controllers use these relationships to coordinate balance, locomotion, manipulation, and recovery motions across the complete body.

Model updates must be synchronized with state estimation and the optimization cycle. Using joint states from one timestamp with inertia or Jacobian quantities computed from another can introduce inconsistency, especially during fast motion. Real-time control architectures therefore maintain coherent state snapshots and deterministic update sequences so that kinematics, dynamics, contact models, and optimization constraints represent approximately the same physical instant.

Software implementations frequently use established rigid-body dynamics libraries to calculate inertia matrices and related quantities. Regardless of the library, model validation remains necessary. Unit tests can verify matrix symmetry, positive definiteness, conservation properties, known static configurations, and agreement between alternative dynamics calculations. Simulation comparisons and measured robot responses provide additional validation before deployment on physical hardware.

Efficient inertia computation and update ultimately provide the dynamic foundation required by Whole Body Control. Accurate link parameters are transformed and assembled into configuration-dependent whole-body quantities, while payload and identification updates adapt the model to changing physical conditions. By maintaining physically consistent and computationally efficient inertia information, the controller can generate feasible torques, contact forces, accelerations, and coordinated motions across the entire robot.

07.05 Task Space Control: CoM and Joint Control [w/Code]

Task-space control allows a robot to express motion objectives in coordinates that directly represent useful physical behavior rather than controlling every joint independently. Typical task variables include end-effector position, orientation, center of mass, torso pose, and contact-point motion. Whole Body Control combines these objectives and converts them into coordinated joint commands while respecting the robot\'s kinematic and dynamic structure.

A task variable is generally defined as a function of the generalized robot configuration, x = f(q). Its velocity is related to generalized velocity through x_dot = J(q)q_dot, where J is the task Jacobian. At the acceleration level, x_ddot = J(q)q_ddot + J_dot(q,q_dot)q_dot. These relationships form the basic mathematical interface between desired Cartesian behavior and joint-space motion.

Task-space tracking commonly begins by defining a reference trajectory containing desired position, velocity, and acceleration. A feedback controller can then generate a commanded task acceleration using position and velocity errors. A typical form combines feedforward acceleration with proportional and derivative feedback, allowing the robot to follow planned motion while correcting deviations caused by disturbances, modeling errors, or imperfect actuator response.

Position and orientation tasks require different mathematical representations. Cartesian position can be described directly using three coordinates, whereas orientation may be represented by rotation matrices, quaternions, or other minimal coordinates. Orientation error must be computed consistently with the chosen representation because simple subtraction of orientation parameters can produce incorrect behavior. The resulting translational and rotational errors are then mapped through the corresponding Jacobian.

Center-of-mass control is particularly important for humanoids and legged robots because whole-body stability depends strongly on mass distribution relative to environmental contacts. The robot CoM is calculated from the masses and positions of all links. Its velocity and acceleration can be related to generalized motion through a CoM Jacobian, allowing the controller to regulate global body motion using coordinated contributions from many joints.

A CoM task may command horizontal motion for balance or locomotion while regulating vertical height according to the desired posture and gait. During standing, the controller can keep the CoM within an appropriate support region. During walking, the reference may move according to a planned dynamic trajectory. The resulting joint and contact-force commands coordinate the legs, torso, and other body segments to achieve the requested global motion.

CoM control alone does not completely describe dynamic balance. Angular momentum generated by arm, torso, and leg motion can significantly influence whole-body behavior. Centroidal dynamics therefore complement CoM regulation by considering total linear and angular momentum. Advanced Whole Body Control can simultaneously track CoM acceleration and regulate centroidal momentum while satisfying foot-contact and friction constraints.

End-effector task-space control is used when a robot must position or orient a hand, foot, tool, sensor, or other body frame. A manipulator may track a six-dimensional pose consisting of three translational and three rotational components. For redundant robots, many joint configurations can produce the same end-effector pose, allowing the remaining degrees of freedom to support balance, posture, collision avoidance, or manipulability objectives.

Joint-space control remains essential even when primary behavior is expressed in task space. Joint tasks can regulate individual positions, velocities, or accelerations and provide a preferred configuration for redundant degrees of freedom. A nominal posture task is often placed at a lower priority than Cartesian manipulation or balance tasks, allowing unused motion capability to keep joints away from limits and maintain mechanically favorable configurations.

The relationship between task-space and joint-space objectives is managed through task priorities or optimization. A high-priority CoM or contact task can first reserve the motion necessary for stability, while an end-effector task uses the remaining feasible freedom. A lower-priority joint posture task then shapes the residual configuration. This hierarchy prevents secondary joint preferences from interfering with essential whole-body behavior.

Null-space projection provides one classical method for combining prioritized tasks. After solving a primary task, a secondary joint command can be projected into the null space of the primary Jacobian. Motion inside this null space does not affect the primary task to first order. This approach is computationally attractive for kinematic control, although inequality constraints and complex contact conditions are often handled more naturally through optimization.

Optimization-based Whole Body Control formulates task-space and joint-space objectives within Quadratic Programming or Hierarchical Quadratic Programming. Desired task accelerations become equality objectives, while joint limits, torque bounds, friction conditions, and collision constraints can be represented as inequalities. The optimizer determines generalized accelerations, torques, or contact forces that best satisfy the requested tasks within physical limits.

Dynamically consistent task-space control incorporates the generalized inertia matrix rather than relying only on geometric Jacobian relationships. The operational-space inertia describes the apparent resistance of the complete robot to acceleration in a selected task coordinate. This allows task forces and joint torques to be mapped in a way that respects robot dynamics, which becomes particularly important for high-speed motion and physical interaction.

Joint limits require explicit consideration because task-space commands can otherwise drive redundant joints toward mechanically unavailable configurations. Position limits can be converted into velocity or acceleration bounds over the control horizon, while soft avoidance objectives can begin acting before a hard boundary is reached. This combination preserves task performance when possible while preventing the optimizer from requesting unsafe joint motion.

Singularities occur when a task Jacobian loses rank and the robot cannot generate arbitrary motion in some task directions. Near singular configurations, small task-space commands can require very large joint velocities or torques. Damped pseudoinverses, regularization, manipulability objectives, and optimization constraints can reduce this sensitivity and guide the robot toward configurations with better control authority.

Multiple task-space objectives frequently operate simultaneously. A humanoid may regulate CoM position, maintain both foot poses, orient the torso, move one hand toward an object, and keep the head directed toward a visual target. Whole Body Control assembles the corresponding Jacobians and references while resolving competition among them. The resulting motion emerges from coordinated use of the entire available kinematic structure.

Mobile manipulators provide another important example of task-space coordination. The same end-effector displacement may be generated by the arm, mobile base, or a combination of both. A task-space controller can distribute motion according to available redundancy, while secondary objectives regulate base orientation, arm posture, obstacle clearance, or manipulability. This avoids treating navigation and manipulation as completely independent control problems.

Contact tasks require special treatment because a body frame touching the environment may need to remain stationary or follow a constrained motion. A stance foot can be assigned zero velocity or acceleration relative to the ground, while a rolling wheel allows motion in selected directions. Contact Jacobians therefore define allowable motion subspaces and connect task-space control directly to contact-force and friction constraints.

Task transitions must be smooth when objectives appear, disappear, or change priority. During walking, a swing-foot tracking task becomes a contact constraint after touchdown. During grasping, a free hand transitions into a force or pose-constrained interaction task. Abrupt switching can create discontinuous joint acceleration or torque commands, so activation weights, reference interpolation, and transition trajectories are used to maintain continuity.

State estimation provides the configuration and motion information required to evaluate every task. Joint encoders supply internal coordinates, while inertial sensing, vision, force sensors, and contact estimation may determine floating-base pose and environmental interaction. Since task-space errors and Jacobians depend on these states, estimation delays or inconsistencies can directly degrade CoM regulation, end-effector accuracy, and contact stability.

Real-time implementation requires all task quantities to be updated in a deterministic sequence. Forward kinematics determines body poses, Jacobians map generalized motion, dynamics provide inertia and force relationships, and the task controller computes references before optimization. These calculations may execute hundreds or thousands of times per second, making efficient model updates and predictable solver execution essential for stable control.

The final output of task-space Whole Body Control depends on the actuator interface. Position-controlled robots may receive joint position or velocity references, while torque-controlled platforms can execute dynamically optimized torque commands directly. Hybrid architectures may combine task-space acceleration generation with joint-level impedance control, providing compliance and disturbance rejection while preserving coordinated whole-body objectives.

Effective task-space, CoM, and joint control therefore form complementary layers rather than competing approaches. Task coordinates express meaningful external behavior, CoM and momentum objectives organize global body stability, and joint-space tasks manage internal configuration. Integrated through priorities, dynamics, constraints, and feedback, these mechanisms allow complex robots to transform high-level motion intent into stable and physically feasible whole-body action.

07.06 Mobile Manipulator Simultaneous Loco-Manipulation WBC [w/Code]

A mobile manipulator combines a mobile base and one or more robotic manipulators into a single physical system capable of locomotion and manipulation. Simultaneous locomotion-manipulation Whole Body Control treats base motion and arm motion as coordinated degrees of freedom rather than independent subsystems. This enables the robot to move through the environment while continuously positioning, orienting, or interacting with objects using its end-effector.

Traditional mobile manipulation often separates navigation and manipulation into sequential phases. The robot first drives to a selected base pose, stops, and then executes an arm trajectory. Although this simplifies planning and control, it restricts the usable workspace and can introduce unnecessary stopping and repositioning. Simultaneous loco-manipulation instead allows the base and arm to move together, increasing reachability, continuity, and operational efficiency.

The complete robot configuration can be represented by generalized coordinates containing both base and manipulator states. For a planar mobile platform, these coordinates may include base position and heading together with arm joint angles. The end-effector pose is therefore a function of the entire configuration, x_e = f(q_b,q_a), where q_b represents base coordinates and q_a represents manipulator coordinates. Both subsystems contribute to the same task-space motion.

Differential kinematics expresses this coupling through a whole-body Jacobian. End-effector velocity can be written as x_dot_e = J_b q_dot_b + J_a q_dot_a, where J_b describes the effect of base motion and J_a represents arm motion. The controller can therefore distribute a desired Cartesian end-effector velocity across the base and manipulator instead of requiring the arm alone to generate the complete motion.

Mobile-base kinematics introduce constraints that depend on the platform architecture. An omnidirectional base may generate planar translation and rotation relatively independently, whereas differential-drive and Ackermann-steered platforms have nonholonomic constraints that restrict instantaneous lateral motion. Whole Body Control must preserve these mobility constraints so that optimized generalized velocities or accelerations correspond to commands the physical base can actually execute.

Task allocation between the base and arm is central to simultaneous loco-manipulation. Small and rapid end-effector corrections may be assigned primarily to the arm because of its responsiveness, while large workspace changes can be supported by base motion. The controller may continuously redistribute contributions according to joint configuration, base accessibility, actuator capability, obstacle geometry, manipulability, and the dynamics of the requested task.

Redundancy provides an important advantage because the same end-effector pose can often be achieved using many combinations of base position and arm configuration. Whole Body Control can exploit this redundancy to avoid joint limits, maintain favorable manipulator posture, improve manipulability, reduce energy consumption, or keep the base in a safe region. Secondary objectives can be optimized without significantly disturbing higher-priority manipulation tasks.

A common hierarchy places physical feasibility and safety constraints above motion-tracking objectives. Collision avoidance, actuator limits, mobile-base constraints, and required environmental contacts must remain valid before accurate end-effector tracking is considered. Manipulation and locomotion objectives can occupy intermediate priority levels, while preferred base orientation, arm posture, energy efficiency, and manipulability can be treated as secondary optimization goals.

Hierarchical Quadratic Programming provides a systematic method for solving these coupled objectives. Generalized base and arm velocities, accelerations, or torques can be optimized subject to equality and inequality constraints. High-priority requirements are solved first, and lower-priority objectives use the remaining feasible freedom. This structure is particularly useful when environmental obstacles, joint limits, and interaction forces change continuously during robot motion.

Dynamic Whole Body Control becomes important when the mobile manipulator performs rapid motion, carries heavy payloads, or applies significant forces to the environment. The dynamic model must represent inertia and coupling between the base, manipulator, payload, and contact forces. Arm acceleration can generate reaction forces on the base, while base acceleration changes the motion observed at the end-effector. Ignoring these effects can reduce tracking accuracy and stability.

For wheeled mobile manipulators, wheel-ground interaction forms part of the whole-body model. Rolling constraints define allowable wheel motion, while friction limits determine the forces that can be transmitted without slipping. If the platform uses independently driven wheels, the controller must map desired base forces or velocities into wheel commands. Wheel saturation, steering limits, traction conditions, and uneven surfaces can further constrain feasible motion.

End-effector control can involve position, orientation, velocity, force, or combinations of these quantities. During free-space motion, Cartesian pose tracking may dominate the manipulation objective. Once contact with an object or surface occurs, force or impedance control may become necessary. Whole Body Control can coordinate base motion with compliant arm behavior so that the robot maintains desired interaction forces while repositioning its mobile platform.

Simultaneous loco-manipulation is especially useful for tasks involving extended trajectories. A robot wiping a large surface, inspecting equipment, opening a long cabinet, handling material along a production line, or following an object may need to move beyond the static workspace of its arm. Continuous base motion effectively extends the manipulator workspace while avoiding repeated stop-plan-move-manipulate sequences.

Trajectory planning must consider the complete mobile-manipulator system rather than generating unrelated base and arm trajectories. A whole-body planner can produce desired end-effector paths together with suitable base motion, while the controller handles local tracking and constraint enforcement. Alternatively, a higher-level planner may specify task-space goals and allow the WBC optimizer to determine how much motion should be produced by each subsystem in real time.

Collision avoidance becomes more challenging because both the base and manipulator move simultaneously. The controller must consider robot-environment collisions as well as self-collision between links. Distance constraints can be constructed from selected collision points or geometric primitives and converted into velocity or acceleration inequalities. These constraints allow the optimizer to modify redundant motion before the robot reaches unsafe configurations.

Manipulability is often used to guide base positioning relative to the task. If the base remains too far from an object, the arm may approach full extension and lose directional control authority. If it moves too close, joints may become folded or collision risk may increase. A manipulability objective can encourage configurations in which the arm retains useful motion capability, while base movement continuously maintains a favorable working relationship with the target.

Joint-limit avoidance serves a similar purpose. During long manipulation trajectories, an arm-only controller may gradually drive one or more joints toward their mechanical limits. Simultaneous base motion can relieve this condition by shifting the entire manipulator relative to the task. The controller can use joint-limit margins as secondary objectives, causing the base to reposition before the arm reaches a configuration that restricts further task execution.

Base motion must also account for localization and environmental perception. Odometry, inertial sensing, LiDAR, cameras, and other sensors estimate the mobile platform pose and surrounding geometry. Manipulator encoders provide joint states, while force-torque sensing may characterize interaction. These measurements must be synchronized because end-effector pose depends jointly on the estimated base state and the measured arm configuration.

Navigation and manipulation planners often operate at slower rates than Whole Body Control. Global navigation may determine a collision-free route, while a manipulation planner specifies object-related goals or desired tool trajectories. WBC operates at a higher frequency and reconciles these references with current robot state and local constraints. This layered architecture allows global planning intent to coexist with fast physical corrections and coordinated actuator behavior.

Task transitions require careful management. The robot may change from navigation-dominant motion to approach, reaching, grasping, carrying, placing, or physical interaction. Stopping the base at every transition is not always necessary, but abruptly changing task priorities can generate discontinuous commands. Smooth activation functions, trajectory blending, velocity matching, and controlled priority transitions allow locomotion and manipulation to evolve continuously.

Payload handling changes the dynamics of the complete system. A heavy object modifies manipulator inertia, center of mass, actuator loading, and potentially the traction requirements of the mobile base. If the payload is known, its inertial properties can be incorporated into the robot model. If it is uncertain, online estimation or conservative bounds can help maintain feasible acceleration, torque, and stability limits during transportation.

Real-time computational performance is essential because the controller may need to update whole-body kinematics, collision constraints, dynamics, contact models, and optimization variables every cycle. Efficient Jacobian computation, sparse optimization, warm-starting, and predictable solver timing reduce latency. The control frequency must be sufficient to coordinate the faster manipulator dynamics with the typically slower but larger-scale motion of the mobile platform.

Failure handling should preserve safe behavior across both locomotion and manipulation. Wheel slip, localization degradation, joint saturation, unexpected contact, obstacle intrusion, or solver infeasibility can invalidate planned motion. The controller should detect these conditions and reduce task demands, freeze selected degrees of freedom, retreat from contact, or transition to a safe state according to system-level safety requirements.

Simultaneous locomotion-manipulation WBC ultimately transforms the mobile base and robotic arm from two cooperating devices into one integrated redundant mechanism. By combining whole-body kinematics, dynamics, task priorities, mobility constraints, collision avoidance, contact control, state estimation, and real-time optimization, the robot can continuously reposition itself while manipulating. This capability significantly expands the workspace and flexibility of autonomous mobile manipulation in complex environments.

07.07 Quadruped WBC Software Structure [w/Code]

Quadruped Whole Body Control software coordinates the floating body, four articulated legs, ground contacts, and joint actuators as one dynamically coupled system. Unlike fixed-base manipulation, the robot body is not directly constrained to the environment and must be stabilized through intermittent foot contacts. The WBC software therefore converts locomotion objectives into feasible joint and contact-force commands while continuously maintaining balance and physical consistency.

A typical software structure separates high-level behavior generation from real-time whole-body control. Navigation and mission software determine where the robot should travel, while locomotion planning generates gait patterns, body trajectories, footholds, and swing-leg references. The WBC layer receives these references and reconciles them with measured robot state, active contacts, actuator limits, terrain conditions, and the dynamic model.

The robot state is commonly represented using a floating-base formulation. Generalized coordinates contain the six-degree-of-freedom base pose together with all leg joint coordinates, while generalized velocity contains base linear and angular velocity plus joint velocities. Because the floating base has no direct actuator, body motion must be produced indirectly through forces transmitted between the feet and the environment.

State estimation is therefore a fundamental WBC software service. Joint encoders provide leg configuration, an IMU measures body angular velocity and acceleration, and kinematic contact information constrains base motion relative to supporting feet. Vision, LiDAR, GNSS, or external localization may provide additional corrections. The estimator combines these measurements to deliver synchronized base pose, velocity, joint state, and contact information.

Contact estimation determines which feet can safely be treated as supporting constraints. A gait scheduler may predict stance and swing phases, but actual touchdown or liftoff can differ because of terrain height, compliance, or disturbances. Force sensors, joint torque estimates, foot velocity, and terrain observations can refine contact state. Reliable contact information prevents the WBC solver from applying physically invalid support assumptions.

The robot-model module maintains the kinematic and dynamic quantities required by control. Forward kinematics computes body and foot poses, while Jacobians relate generalized motion to individual task frames. The generalized inertia matrix, gravity vector, Coriolis and centrifugal terms, and contact Jacobians are updated from the current state. Efficient rigid-body algorithms are important because these calculations execute every control cycle.

The floating-base dynamics can be expressed as M(q)q_ddot + h(q,q_dot) = Sᵀτ + J_cᵀf_c. Here, M is the generalized inertia matrix, h contains gravity and nonlinear dynamic terms, τ represents actuated joint torques, and f_c contains contact forces. The selection matrix S reflects the fact that only the leg joints are directly actuated while the floating base is controlled through environmental reaction forces.

Locomotion planning provides references that define the intended whole-body behavior. These references may include desired body position, orientation, linear and angular velocity, footstep locations, swing-foot trajectories, and contact schedules. More advanced planners may also provide desired momentum or ground reaction forces. WBC transforms these planner-level quantities into commands compatible with instantaneous robot dynamics and constraints.

Body control regulates the pose and motion of the floating trunk. Desired body acceleration can be generated from position, orientation, linear-velocity, and angular-velocity tracking errors. Since the body has no direct actuator, the optimizer determines combinations of stance-foot forces and joint torques that produce the required body acceleration. This mechanism forms the core relationship between locomotion objectives and physical actuation.

Swing-foot control manages legs that are temporarily disconnected from the ground. A swing trajectory specifies foot position, velocity, and acceleration from liftoff to touchdown. The corresponding foot Jacobian maps this task into generalized motion. Tracking accuracy is important for terrain clearance and precise foothold placement, but the controller must avoid excessive accelerations near touchdown that could produce large impact forces.

Stance feet are treated differently because they define the robot\'s support structure. Under a rigid-contact assumption, stance-foot acceleration relative to the terrain is constrained near zero. The corresponding contact forces become optimization variables or are computed from the dynamic solution. These forces must remain physically feasible while producing the body accelerations and moments required for balance and locomotion.

Friction constraints prevent the optimizer from requesting impossible ground reaction forces. The normal component of each stance force must remain compressive, while tangential forces are bounded according to the available coefficient of friction. The nonlinear friction cone is commonly approximated by linear inequalities forming a friction pyramid, allowing the constraints to be incorporated efficiently into real-time Quadratic Programming.

Contact-force distribution becomes especially important when several feet simultaneously support the body. Multiple combinations of forces can produce the same net wrench on the robot. WBC can distribute these forces according to stability margins, actuator capability, friction utilization, terrain geometry, or energy objectives. Smooth force redistribution also reduces abrupt loading changes when feet enter or leave the support set.

Quadratic Programming provides a practical computational framework for quadruped WBC. Decision variables may include generalized accelerations, joint torques, contact forces, or combinations of these quantities. Tracking objectives are represented as quadratic costs, while dynamics, stance contacts, friction limits, torque limits, and acceleration bounds are expressed as equality or inequality constraints. The solver selects a physically feasible compromise among competing objectives.

Hierarchical Quadratic Programming can be used when strict task priorities are required. Dynamic consistency, contact feasibility, and safety constraints can occupy the highest levels, followed by body stabilization and essential foot tasks. Posture regulation, force regularization, energy reduction, and other secondary objectives can operate at lower priorities. This prevents convenience objectives from degrading critical balance or contact requirements.

Joint-space posture control provides internal configuration regulation that complements body and foot tasks. A nominal posture can keep the legs away from joint limits and configurations with poor mechanical leverage. During redundant support phases, posture objectives use remaining degrees of freedom without significantly changing higher-priority body motion. Joint velocity, acceleration, and torque bounds provide additional protection for the actuators.

Different gaits produce different WBC constraint structures. During a trot, diagonal pairs of feet alternate between stance and swing, whereas a crawl gait may maintain three supporting feet for much of the cycle. Bounding and running introduce shorter contact periods and potentially aerial phases. The controller must update contact Jacobians, force variables, and task activation according to the current gait phase without destabilizing the optimization problem.

Transitions between contact states require careful software handling. At touchdown, a swing foot changes from a motion-tracking task into a support constraint and begins carrying ground reaction force. At liftoff, its force must decrease before the foot becomes unconstrained. Reference interpolation, force ramping, task activation functions, and impact-aware control can reduce discontinuities in acceleration and torque commands during these transitions.

Terrain adaptation introduces additional constraints and references. Foot orientation may need to align with local surface geometry, while contact-force limits depend on estimated friction and terrain quality. Rough terrain can also cause unexpected contact timing or body disturbances. The WBC software must accept updated footholds, terrain normals, and contact estimates without requiring a complete restructuring of the real-time control architecture.

Low-level joint control executes the commands generated by WBC. Torque-controlled robots can apply optimized joint torques directly, while other platforms may use position, velocity, or impedance interfaces. Joint-level impedance control is frequently combined with feedforward torque to provide compliance and disturbance rejection. Motor drivers then translate these commands into electrical actuation while enforcing hardware-level current, temperature, and safety limits.

Real-time scheduling is critical because estimation, model computation, optimization, and actuator communication must remain synchronized. A typical cycle reads sensor data, updates state and contacts, computes kinematics and dynamics, refreshes task references and constraints, solves the WBC optimization, and sends actuator commands. Predictable execution time is often more important than maximizing average computational throughput because missed deadlines can directly affect stability.

Software architecture should isolate mathematical control logic from hardware-specific communication. Sensor interfaces, state estimation, robot modeling, locomotion planning, WBC optimization, joint control, and diagnostics can be implemented as clearly defined modules with explicit data interfaces. This modularity allows the same control concepts to be tested in simulation and transferred to different quadruped platforms with limited changes to hardware abstraction layers.

Simulation provides an essential environment for validating the WBC software before deployment. Developers can test gait transitions, external disturbances, friction changes, sensor noise, actuator saturation, and contact uncertainty without risking hardware. Logging generalized states, task errors, contact forces, solver status, and execution time enables systematic comparison between planned behavior, optimization results, and simulated or measured robot response.

Runtime monitoring should detect failures that cannot be resolved by normal optimization. Excessive body attitude error, persistent foot slip, joint limit approach, actuator saturation, estimator divergence, solver infeasibility, or missed real-time deadlines can trigger degraded control modes. Depending on severity, the system may reduce velocity, increase support duration, lower the body, stop locomotion, or enter a controlled safe state.

A robust Quadruped WBC software structure therefore forms a closed real-time loop connecting autonomy, locomotion planning, estimation, robot modeling, contact reasoning, optimization, and low-level actuation. Its purpose is not merely to compute joint commands, but to preserve dynamically feasible whole-body behavior as contacts and terrain continuously change. This architecture enables stable standing, agile locomotion, disturbance recovery, and adaptable movement across complex environments.

07.08 Humanoid WBC Software Structure [w/Code]

Humanoid Whole Body Control software coordinates the floating torso, articulated arms, articulated legs, head, and multiple environmental contacts as one dynamically coupled system. Unlike fixed-base manipulators, a humanoid must continuously regulate balance while its support configuration changes during standing, walking, reaching, carrying, and physical interaction. The WBC software therefore transforms high-level motion objectives into coordinated joint torques, accelerations, and contact forces while respecting kinematic, dynamic, and safety constraints.

A typical humanoid control architecture separates mission-level behavior, motion planning, whole-body optimization, state estimation, and low-level actuator control. High-level software determines behaviors such as standing, walking, reaching, grasping, carrying, or recovery. Motion planners generate body, hand, foot, and gaze references, while the WBC layer resolves these references against the current robot state, active contacts, dynamics, actuator limits, and environmental constraints.

The humanoid state is commonly represented using a floating-base model. Generalized coordinates contain the six-degree-of-freedom torso or pelvis pose together with all actuated joint coordinates. Generalized velocity contains base linear and angular velocity and all joint velocities. Because the floating base is not directly actuated, its motion must be generated through coordinated joint torques and reaction forces transmitted through the feet, hands, or other contact surfaces.

State estimation is a central software service because accurate whole-body control depends on consistent knowledge of the floating-base state. Joint encoders provide internal configuration, while IMUs measure body angular motion and acceleration. Foot contacts, force-torque sensors, cameras, LiDAR, GNSS when available, and external localization can provide additional information. Sensor fusion produces synchronized estimates of base pose, velocity, joint state, contact state, and environmental information.

Contact estimation determines which parts of the humanoid are physically constrained by the environment. During standing, both feet may provide support, while walking may involve one or two supporting feet. During manipulation, a hand can establish an additional environmental or object contact. Contact detection can use force-torque measurements, joint torque estimates, foot velocity, tactile sensing, and motion consistency. Reliable contact information is essential because incorrect contact assumptions can make the WBC optimization physically inconsistent.

The robot-model module provides the kinematic and dynamic quantities required by the controller. Forward kinematics determines the pose of the pelvis, feet, hands, head, and other important frames. Jacobians map generalized motion to task frames, while the generalized inertia matrix, gravity vector, Coriolis and centrifugal terms, and contact Jacobians describe whole-body dynamics. These quantities must be updated efficiently at every control cycle because humanoid configurations change continuously.

Humanoid dynamics can be represented by M(q)q_ddot + h(q,q_dot) = Sᵀτ + J_cᵀf_c. The generalized inertia matrix M describes the configuration-dependent inertial behavior, while h contains gravity and nonlinear velocity-dependent effects. The actuator torque vector τ acts only on the actuated joints, whereas contact forces f_c act through the contact Jacobian J_c. The floating-base dynamics are therefore controlled indirectly through the combination of joint actuation and environmental reaction forces.

Task-space control provides the main interface between behavior planning and whole-body motion. Typical tasks include pelvis pose, center-of-mass position, hand pose, foot pose, head orientation, gaze direction, and torso orientation. Each task can define desired position, velocity, acceleration, or force behavior. The WBC optimizer combines these tasks according to priority while ensuring that the resulting motion remains compatible with contact, dynamics, joint limits, and actuator capabilities.

Center-of-mass and momentum control are particularly important for humanoid balance. The controller can regulate the horizontal and vertical CoM trajectory while simultaneously controlling centroidal linear and angular momentum. During standing, these objectives help maintain the body within a dynamically feasible support region. During walking, they coordinate trunk and limb motion with foot placement so that the humanoid can generate appropriate momentum while transitioning between support configurations.

Foot control changes according to gait phase. A stance foot is treated as a constrained contact frame whose motion relative to the ground is approximately zero. A swing foot follows a planned trajectory with specified position, velocity, and acceleration. At touchdown, the swing task must smoothly transition into a contact constraint, while at liftoff the corresponding contact force must decrease before the foot becomes unconstrained. The software must manage these transitions without producing excessive acceleration, torque, or impact forces.

Hand and arm control introduces additional redundancy and interaction capability. A humanoid may simultaneously maintain balance, walk, and move one or both hands toward objects. Hand tasks can specify Cartesian pose, velocity, force, or impedance behavior. If a hand grasps an object or contacts a surface, the corresponding interaction becomes part of the whole-body constraint structure. The controller can then use leg, torso, and free-arm motion to support the manipulation objective without sacrificing balance.

Task priorities are essential because humanoid objectives can conflict. Physical feasibility, contact consistency, collision avoidance, and actuator safety generally need to be preserved while body stabilization and essential manipulation or locomotion tasks are maintained. Lower-priority objectives can regulate posture, joint-limit margins, arm configuration, gaze, energy consumption, or manipulability. Hierarchical Quadratic Programming provides a practical framework for enforcing these priorities while preserving feasible solutions.

Contact-force optimization is required whenever the humanoid interacts strongly with the environment. Ground reaction forces must remain within friction limits, while hand contact forces must remain compatible with the contact geometry and object interaction. When several contacts are active, multiple force distributions may produce the same whole-body wrench. The WBC optimizer can select a distribution based on stability margins, actuator capability, friction utilization, force smoothness, and task requirements.

Humanoid joint limits and self-collision constraints require continuous monitoring. Large arm movements, deep squatting, crouching, or unusual recovery motions can bring joints close to mechanical boundaries or cause links to approach one another. Joint position, velocity, and acceleration limits can be incorporated into the optimization, while geometric distance constraints can prevent self-collision and environmental collision. Secondary posture objectives can further guide the robot away from undesirable configurations.

Walking introduces a continuously changing contact structure and therefore requires dynamic task management. A gait planner may specify footstep locations, support phases, swing trajectories, body motion, and desired walking velocity. The WBC software converts these references into dynamically consistent joint and contact commands. During single-support phases, the remaining stance foot carries the main support load; during double support, forces can be redistributed while both feet contribute to stabilization and motion generation.

Humanoid locomotion also requires careful handling of transitions between standing, walking, stopping, turning, stepping over obstacles, and recovery. Abrupt changes in task activation can produce discontinuous acceleration or torque commands. Reference interpolation, task blending, force ramping, and smooth priority transitions allow the controller to change behaviors without destabilizing the robot. Recovery behaviors may temporarily increase balance-related priorities while reducing nonessential manipulation objectives.

Physical interaction extends WBC beyond locomotion and free-space manipulation. A humanoid may push, pull, open a door, lift an object, support itself with a hand, or manipulate a tool. In such cases, force and motion objectives must be coordinated. Impedance control allows the robot to regulate the relationship between displacement and interaction force, while force constraints prevent the optimizer from requesting physically excessive contact loads.

Payload handling modifies the dynamic behavior of the complete humanoid. Holding an object changes the mass distribution, center of mass, inertia, required joint torques, and potentially the friction demand at the feet. The payload can be modeled explicitly when its properties are known, or estimated online when they are uncertain. WBC should update the dynamic model and task constraints so that lifting and carrying remain feasible without excessive actuator loading or loss of balance.

Real-time execution requires a deterministic control pipeline. A typical cycle reads sensors, estimates state and contacts, updates the robot model, generates or receives task references, constructs constraints and costs, solves the WBC optimization, and sends commands to the actuator layer. The entire process must complete within a predictable control period. Efficient rigid-body dynamics, Jacobian computation, sparse optimization, solver warm-starting, and memory-conscious software design are important for maintaining high control rates.

Low-level joint control executes the commands produced by the WBC layer. Torque-controlled humanoids can directly apply optimized joint torques, while position- or velocity-oriented platforms may use impedance or cascaded servo control. Feedforward dynamics can be combined with feedback control to improve tracking and disturbance rejection. Motor drivers and safety controllers enforce hardware-level current, temperature, communication, and emergency limits independently of the higher-level WBC optimization.

A modular software architecture separates hardware interfaces from estimation, modeling, planning, optimization, and control logic. Sensor drivers provide standardized measurements, the estimator produces a coherent robot state, the model layer computes kinematics and dynamics, and the WBC solver consumes these quantities through explicit interfaces. Diagnostics and logging operate alongside the real-time loop to monitor task errors, contact forces, solver status, actuator saturation, and execution timing without disturbing deterministic control execution.

Simulation and hardware-in-the-loop testing are essential for validating humanoid WBC software. Standing, walking, stepping, manipulation, external disturbances, contact transitions, payload changes, actuator saturation, and sensor failures can be tested before physical deployment. Recorded state trajectories and controller outputs allow developers to compare planned behavior with simulated and measured responses. Systematic testing is particularly important because many WBC failures occur at transitions between otherwise valid operating modes.

Runtime safety monitoring should detect conditions that cannot be resolved through normal optimization. Excessive body tilt, unexpected contact loss, persistent foot slip, actuator saturation, joint-limit proximity, estimator divergence, collision risk, solver infeasibility, or missed real-time deadlines can trigger degraded control modes. Depending on the failure condition, the robot may reduce motion speed, increase support, lower the body, release a manipulation task, stop walking, or enter a controlled safe posture.

A robust humanoid WBC software structure therefore forms a closed-loop architecture connecting behavior planning, locomotion and manipulation references, state estimation, contact reasoning, whole-body modeling, hierarchical optimization, and low-level actuation. Its purpose is to continuously reconcile multiple competing objectives within the physical capabilities of the robot. By treating the humanoid as one dynamically integrated mechanism rather than a collection of independent limbs, the architecture supports balanced locomotion, coordinated manipulation, physical interaction, disturbance recovery, and adaptive whole-body behavior.

07.09 WBC Real-Time Implementation: QP Solver Selection [w/Code]

Real-time implementation is one of the most important engineering aspects of Whole Body Control because an optimization problem that is mathematically correct but cannot be solved within the required control period is not suitable for a physical robot. WBC must repeatedly process sensor measurements, update kinematics and dynamics, construct task objectives and constraints, solve the optimization problem, and transmit actuator commands with predictable latency. Solver selection must therefore consider not only solution quality but also execution time, determinism, numerical robustness, memory usage, and hardware resources.

A typical WBC control cycle begins with synchronized sensor acquisition and state estimation. Joint positions and velocities, floating-base motion, contact states, force measurements, and external perception are converted into a coherent robot state. Forward kinematics and Jacobians are then updated, followed by dynamic quantities such as the inertia matrix, nonlinear force terms, and contact relationships. Task references and constraint boundaries are generated from this state before the optimization problem is assembled and passed to the selected QP solver.

The optimization variables depend on the WBC architecture and robot type. A kinematic controller may optimize joint velocities or accelerations, while a dynamic controller may include generalized accelerations, joint torques, and contact forces. Some implementations introduce slack variables to relax selected task constraints when exact tracking becomes infeasible. The variable dimension directly affects solver complexity, so unnecessary variables should be avoided without removing physical quantities required for dynamic consistency or safety.

Quadratic Programming is widely used because many WBC objectives and constraints can be represented in a form that is computationally efficient. A standard problem can be expressed as minimizing one-half xᵀHx plus fᵀx subject to equality and inequality constraints. Tracking errors, posture objectives, force regularization, and energy-related terms can be included in the quadratic cost, while dynamics, contact conditions, friction limits, joint bounds, torque limits, and collision constraints can be represented through appropriate constraints.

The choice between a conventional QP and Hierarchical Quadratic Programming depends on how strictly task priorities must be preserved. Weighted QP combines multiple objectives through numerical weights, which can be computationally efficient but requires careful weight selection. HQP explicitly preserves priority levels and prevents lower-priority objectives from degrading higher-priority solutions. For humanoid and quadruped systems with strict contact and balance requirements, hierarchical treatment can provide a clearer relationship between control requirements and solver behavior.

Active-set QP solvers are attractive for real-time WBC because consecutive control problems are usually similar. The active constraints from the previous cycle can provide useful information for the next optimization, allowing warm-starting and rapid convergence when the active constraint set changes slowly. Active-set methods can be particularly effective for moderate problem sizes with many inequality constraints, although frequent contact transitions or rapidly changing active sets may reduce this advantage.

Interior-point methods provide another important solver family. Instead of explicitly identifying the active constraint set, they solve a sequence of barrier-regularized problems and can provide robust performance for larger, dense optimization problems. Their computational requirements may be higher than those of an active-set method for small WBC problems, but their predictable treatment of many constraints can become attractive when the optimization dimension grows substantially.

Operator-splitting and first-order optimization methods can also be considered when extremely fast iterations or large sparse problems are involved. These methods may provide useful solutions with relatively simple numerical operations and can exploit problem sparsity effectively. However, they often require careful tuning of convergence tolerances and iteration limits. For safety-critical WBC, the controller should define acceptable residual thresholds and verify whether the returned solution satisfies the required physical constraints.

Warm-starting is an important technique regardless of solver family. The previous control-cycle solution is usually close to the next solution because robot states, task references, and constraints change continuously. Supplying this previous solution to the solver can substantially reduce computation. Warm-starting can be applied to primal variables, dual variables, active constraints, or other solver-specific internal states, provided that changes in contact configuration and problem dimensions are handled correctly.

Problem sparsity should be exploited whenever possible. Whole-body dynamics, contact relationships, and task Jacobians often produce structured matrices containing many zero or repeated patterns. Sparse matrix storage and sparse linear algebra can reduce memory consumption and computation time. However, excessive matrix transformations or repeated allocation can eliminate these benefits. Real-time implementations therefore commonly preallocate memory, reuse matrix structures, and update only values that change between control cycles.

Numerical conditioning has a direct influence on solver reliability. WBC problems combine quantities with different physical units and magnitudes, including positions, accelerations, forces, torques, and moments. Poor scaling can cause the solver to spend excessive effort resolving numerical errors rather than optimizing the physical problem. Appropriate variable scaling, constraint normalization, regularization, and consistent unit conventions can significantly improve convergence and solution quality.

Solver tolerances must be selected according to the physical requirements of the robot. Excessively tight tolerances may increase computation time without providing meaningful improvement in actuator commands, while overly loose tolerances can produce constraint violations or inaccurate task tracking. Practical systems often distinguish between feasibility tolerances, optimality tolerances, and termination criteria. These values should be validated against the actual dynamic and safety margins of the robot rather than chosen solely from theoretical numerical considerations.

Deterministic timing is often more important than average solver speed. A solver that normally completes in a very short time but occasionally requires several times longer may be unsuitable for a high-frequency balance controller. The WBC implementation should therefore monitor worst-case execution time, not only mean execution time. Maximum iteration limits, bounded memory allocation, fixed computational paths where practical, and graceful handling of solver timeouts help maintain predictable real-time behavior.

The required control frequency depends on the robot and application. A slow manipulation platform may tolerate a lower WBC frequency, whereas a dynamic humanoid or quadruped may require substantially faster updates for balance and contact regulation. The solver itself is only one part of the timing budget. Sensor communication, state estimation, model computation, collision processing, optimization assembly, solver execution, actuator communication, and operating-system scheduling must all fit within the complete control period.

Contact transitions create special challenges for real-time QP solving. When a foot touches or leaves the ground, the number of contact constraints and force variables can change. An implementation can either dynamically resize the optimization problem or maintain a fixed-size problem and deactivate unused contacts through bounds or activation parameters. Fixed-size structures can simplify memory management and improve timing determinism, while dynamic structures may reduce unnecessary computations. The appropriate choice depends on the solver and robot architecture.

Infeasibility handling is essential because a WBC optimization problem may become impossible to satisfy. Conflicting contact assumptions, excessive task demands, actuator saturation, unrealistic friction estimates, or sudden disturbances can make the nominal problem infeasible. Robust systems use slack variables for selected soft objectives, constraint relaxation strategies, task-priority degradation, or predefined fallback controllers. Hard safety constraints should not be relaxed merely to obtain a mathematically feasible solution.

Solver selection should therefore be evaluated using representative robot scenarios rather than a single nominal test. Standing, walking, turning, reaching, manipulation, contact transitions, external disturbances, joint-limit proximity, payload changes, and degraded sensing can produce very different optimization behavior. Benchmarking should measure solve time, worst-case latency, iteration count, constraint residuals, tracking performance, failure frequency, and sensitivity to changing active constraints.

Hardware architecture also influences solver selection. CPU-based implementations are common because modern CPUs provide strong floating-point performance, large memory capacity, and mature sparse linear-algebra libraries. GPU acceleration can become attractive for large batches or highly parallel numerical operations, but data-transfer latency and synchronization overhead can reduce its benefit for small, tightly coupled WBC problems. Dedicated embedded processors require additional attention to memory footprint, thermal limits, and deterministic execution.

Software architecture should isolate the optimization formulation from the solver backend. A WBC application can define tasks, constraints, variables, and priorities through a solver-independent interface and then select different QP engines for simulation, development, benchmarking, or deployment. This separation makes it easier to compare solvers without rewriting the control architecture. It also allows fallback solvers or simplified control modes to be introduced when the primary solver fails.

Real-time monitoring should continuously evaluate solver status and the physical validity of its output. Important indicators include optimization termination status, primal and dual residuals, constraint violations, iteration count, computation time, actuator saturation, contact-force limits, and task errors. A solver reporting numerical convergence does not automatically guarantee that the resulting robot command is safe. The control layer should therefore perform independent sanity checks before sending commands to the actuators.

A practical WBC system may use different optimization configurations for different operating modes. Standing can use a relatively compact problem with strong balance and contact priorities, while walking introduces changing foot constraints and swing tasks. Manipulation may add hand pose and force objectives, and recovery mode may reduce nonessential tasks to preserve balance. Solver settings, tolerances, task priorities, and iteration limits can be adapted while maintaining a common underlying real-time architecture.

The final solver-selection decision should therefore consider the complete control system rather than optimization performance alone. A suitable solver must provide sufficiently accurate solutions within the available control period, remain numerically stable under changing robot configurations, handle equality and inequality constraints efficiently, support warm-starting where beneficial, and provide reliable failure information. The best practical configuration is the one that consistently converts WBC objectives into physically valid commands under the timing and safety requirements of the target robot.

07.10 WBC Safety and Constraint Handling Methods

Whole Body Control safety depends on treating physical limits and environmental conditions as integral parts of the control problem rather than as independent checks performed after optimization. A WBC controller must generate commands that satisfy actuator capabilities, joint limits, contact conditions, collision restrictions, balance requirements, and task objectives simultaneously. Constraint handling therefore determines not only whether a requested motion is mathematically achievable but also whether it can be executed safely by the physical robot.

A typical WBC safety architecture separates hard constraints from soft objectives. Hard constraints represent conditions that must not be violated, such as actuator torque limits, joint position boundaries, contact consistency, friction limits, and collision avoidance margins. Soft objectives represent behaviors that may be degraded when necessary, such as posture preferences, trajectory accuracy, energy efficiency, or secondary manipulation tasks. This distinction allows the optimizer to sacrifice less critical objectives before violating fundamental physical safety conditions.

Joint position limits are among the most basic constraints in whole-body control. A controller should not wait until a joint reaches its mechanical stop before responding. Position limits can be converted into velocity and acceleration constraints using the current joint state and the remaining control horizon. Safety margins can also be introduced so that the controller begins reducing motion as a joint approaches its boundary. This predictive treatment helps prevent sudden saturation and allows smooth redirection of redundant motion.

Joint velocity and acceleration limits provide additional protection against mechanically aggressive commands. A desired task-space acceleration may mathematically require extremely high joint velocity or acceleration near a singular configuration. The WBC optimizer must therefore constrain these quantities according to actuator and transmission capabilities. Rate limits on command changes can also be applied to prevent abrupt torque or acceleration transitions that could excite structural vibration or produce undesirable mechanical loads.

Actuator torque and current constraints are essential because optimized whole-body motion must remain executable by the installed hardware. Each motor has limits related to continuous torque, peak torque, current, temperature, gearbox capability, and available electrical power. The controller can represent these limits directly in the optimization or derive conservative operating boundaries from actuator models. Maintaining sufficient margin below absolute hardware limits can improve reliability during disturbances and modeling uncertainty.

Contact constraints are particularly important for floating-base robots because environmental forces provide the support required to control the body. A stance foot may be constrained to remain stationary relative to the ground, while the corresponding contact force must satisfy unilateral and friction conditions. Hand contacts, wheel contacts, or other support surfaces require similarly appropriate models. Incorrect contact assumptions can cause the optimizer to generate forces or accelerations that cannot physically occur.

Friction constraints limit the tangential force that can be transmitted through a contact. The physical friction cone is commonly approximated by a linear friction pyramid so that it can be incorporated into a real-time QP. The normal force must remain consistent with the contact model, while tangential components are bounded by the estimated coefficient of friction. Conservative friction estimates can improve safety on uncertain terrain, although excessively conservative values may unnecessarily reduce mobility and task performance.

Collision avoidance can be formulated using geometric distance constraints between robot links and the environment. When the distance between two bodies approaches a safety threshold, the controller can impose a velocity or acceleration inequality that prevents further motion toward the obstacle. Self-collision constraints can be treated similarly between selected pairs of robot links. The safety margin should account for perception uncertainty, model error, control latency, and the physical dimensions of the robot.

Constraint margins should therefore reflect uncertainty rather than relying exclusively on nominal geometry. Sensor noise, localization error, calibration differences, flexible structures, actuator backlash, and unmodeled payloads can cause the actual robot state to differ from the estimated state. A nominal collision distance or joint limit may therefore be insufficient. Adaptive or conservative margins provide additional protection by accounting for the uncertainty associated with each constraint.

Balance constraints are closely related to contact and whole-body dynamics. For a standing or walking robot, the available contact forces must generate a feasible body wrench while remaining inside friction and support limitations. The controller may use center-of-mass, centroidal momentum, zero-moment, or support-region related constraints depending on the robot and control formulation. Balance should generally be protected as a higher-priority objective than secondary posture or manipulation preferences.

Torque feasibility and contact-force feasibility are strongly coupled. A contact-force distribution that produces the desired body acceleration may require joint torques beyond actuator capability. Conversely, torque limitations may restrict the amount of contact force that a particular leg or arm can generate. Whole Body Control should therefore solve these quantities together whenever dynamic consistency is important rather than optimizing contact forces independently from actuator commands.

Task constraints may need to be relaxed when the requested motion becomes physically impossible. A hand trajectory, body pose, or gaze target can become infeasible because of joint limits, obstacles, contact geometry, or actuator saturation. Slack variables allow selected task equations to be violated by a controlled amount while preserving higher-priority safety constraints. The resulting task error should be monitored so that the system recognizes when the commanded behavior cannot be fully achieved.

Constraint prioritization provides a systematic way to resolve conflicts. Safety-critical physical constraints normally occupy the highest level, followed by required contacts, balance, essential task tracking, and secondary objectives. Hierarchical Quadratic Programming can preserve these distinctions explicitly, while weighted QP can approximate them through carefully selected cost weights. The choice depends on the required strictness of the priorities and the computational characteristics of the target system.

Soft constraints are useful when temporary violation is preferable to complete controller failure, but they must be selected carefully. A posture preference can normally be relaxed, whereas a collision boundary or absolute torque limit should generally remain hard. Excessive use of soft constraints can make the optimizer produce apparently successful solutions that violate important physical requirements. Safety architecture should therefore define clearly which constraints are never relaxable.

Constraint activation and deactivation must also be handled smoothly. A foot that is approaching touchdown changes from a swing task to a contact constraint, while a hand releasing an object changes from a constrained interaction to free motion. Abruptly introducing or removing constraints can cause discontinuities in optimization variables and actuator commands. Activation weights, gradual force transitions, reference blending, and contact-state confirmation can reduce these effects.

Constraint feasibility should be checked independently of task performance. A solver may report mathematical convergence even though the solution contains physically unacceptable residuals, excessive forces, or numerical errors. The control layer should therefore examine equality residuals, inequality violations, torque margins, contact-force margins, joint-limit distances, collision distances, and solver status before commands are transmitted. These checks provide a second layer of protection beyond the optimization algorithm itself.

Real-time constraint generation requires efficient geometric and dynamic computation. Collision distances, Jacobians, friction limits, contact normals, joint boundaries, and actuator constraints may change every control cycle. Recomputing all quantities from scratch can consume a significant portion of the available timing budget. Efficient updates, preallocated memory, cached model structures, incremental collision processing, and sparse matrix operations help maintain deterministic execution.

Safety constraints should also account for control latency. A collision constraint based only on the current position may be insufficient if the robot can travel a significant distance before the next command takes effect. Predictive constraints can incorporate estimated velocity, acceleration, communication delay, and solver execution time. This approach creates a larger effective safety margin and reduces the probability that a rapidly moving link crosses a boundary before the next control update.

Emergency and degraded modes provide additional protection when normal WBC operation becomes unreliable. Solver infeasibility, estimator divergence, unexpected contact, actuator saturation, communication loss, excessive body tilt, or severe sensor disagreement may require a transition to a simpler control mode. Depending on the platform, the robot may reduce velocity, lower the body, increase support, release a manipulation task, stop locomotion, or execute a predefined safe posture.

Fallback control should be designed as part of the architecture rather than added after a failure occurs. A fallback mode may use conservative impedance control, predefined joint configurations, reduced task sets, or a simpler optimization problem. The transition conditions should be deterministic and based on measurable quantities such as solver timeout, constraint violation, state-estimation quality, or actuator status. Recovery to normal WBC should also require stable conditions rather than immediate automatic switching.

External disturbances require constraints to remain feasible under rapidly changing conditions. A push, unexpected collision, payload shift, foot slip, or terrain deformation can suddenly alter the forces available to the robot. Robust WBC can respond by reducing nonessential task demands and reallocating contact forces or joint motion. Disturbance handling should preserve critical constraints while allowing the robot to temporarily deviate from nominal trajectories.

Uncertainty-aware constraint handling becomes increasingly important for robots operating outside controlled environments. Terrain friction may be estimated with uncertainty, object mass may be unknown, and perception may provide incomplete geometry. Robust or conservative optimization can represent uncertain parameters through bounded constraints, safety margins, or worst-case approximations. The objective is not to eliminate all uncertainty but to prevent reasonable estimation errors from producing unsafe commands.

Constraint handling should be validated through systematic scenario testing. Normal operation should be evaluated together with joint-limit approaches, actuator saturation, friction reduction, contact loss, collision proximity, payload changes, external disturbances, sensor noise, and solver delays. Testing should measure not only whether the optimizer returns a solution but also whether physical safety margins remain positive and whether the controller transitions correctly when constraints become active.

Logging and diagnostics are essential for understanding safety behavior. Each control cycle can record active constraints, minimum joint-limit margin, minimum collision distance, contact-force margin, actuator saturation, solver residuals, computation time, and task errors. These records allow engineers to identify whether a failure originated from estimation, modeling, planning, optimization, hardware saturation, or insufficient safety margins. Historical logs also provide valuable data for improving constraint parameters.

The final safety layer should be independent enough to detect failures within the WBC implementation itself. Hardware emergency systems, actuator protection, communication watchdogs, thermal monitoring, and independent limit checks can prevent a software optimization error from becoming a physical hazard. WBC should operate inside this broader safety architecture rather than being treated as the sole protection mechanism.

A robust WBC safety and constraint-handling architecture therefore combines hard physical limits, prioritized objectives, contact and friction models, collision margins, uncertainty handling, real-time feasibility checks, degraded modes, and independent safety supervision. The central principle is that optimization should search only within the physically acceptable region whenever possible, while controlled relaxation should be reserved for objectives that are genuinely negotiable. This approach allows Whole Body Control to remain effective during normal operation while maintaining predictable and safe behavior when the robot encounters limits, disturbances, uncertainty, or unexpected environmental conditions.


Author: Richard Minsoo Go

LinkedIn: Richard Minsoo Go

Top comments (0)