Robot Control Software — Ch.10: Legged Robot Control [EN]

STATERA Library · Series 08 · Volume 04 · Chapter 10 · EN
Source: https://github.com/Statera-Library/08_Vol_04_Robot_Control_Software/blob/main/Chapter_10_Legged_Robot_Control/Chapter_10_Legged_Robot_Control_EN.md

Volume 04 Robot Control Software

10. Legged Robot Control

10.01 Legged Robot Control Overview: Contact, Balance, Gait

Legged robot control differs fundamentally from control of wheeled platforms because locomotion is created through intermittent contacts between the robot and the environment. The controller must continuously coordinate joint motion, ground reaction forces, body posture, and contact transitions while maintaining dynamic stability. In the provided structure, these concerns form the foundation for later treatments of ZMP, DCM, gait generation, contact detection, terrain adaptation, swing-leg optimization, and learning-based control.

A useful abstraction divides legged locomotion into three tightly coupled elements: contact, balance, and gait. Contact defines which feet can exchange forces with the environment, balance determines whether those forces can support the desired body motion, and gait determines how contacts change over time to produce locomotion. None can be designed completely independently because changing a footstep modifies the support geometry, feasible contact forces, and future balance conditions.

Contact is more than a binary indication that a foot touches the ground. A controller must reason about contact position, surface orientation, normal force, tangential force, friction limits, and whether the contact is stable or slipping. Contact constraints therefore connect robot dynamics directly to environmental geometry. During locomotion, the software maintains an estimated contact state and uses it to decide which limbs may generate support forces and which limbs are free to execute swing motion.

Ground reaction forces are the physical mechanism through which a legged robot controls its floating base. Unlike a fixed manipulator, the robot body is not directly attached to the world, so joint torques alone cannot arbitrarily move the base. Instead, actuator torques produce limb forces that interact with the terrain, and the resulting reaction forces generate linear and angular acceleration of the body. Control software must distribute these forces among active contacts while respecting actuator, friction, and contact constraints.

Balance control regulates the robot\'s center of mass, body orientation, momentum, and support forces so that commanded motion remains dynamically feasible. Static balance can often be interpreted geometrically using the support region, but walking and running require dynamic criteria that account for momentum and acceleration. Consequently, practical controllers combine state estimation with dynamic models and predictive or feedback control to continuously correct disturbances, modeling errors, imperfect terrain information, and unexpected contact behavior.

The center of mass provides a compact representation of overall translational dynamics, while centroidal momentum describes the combined linear and angular motion of the complete robot. Controlling these quantities allows a high-level balance controller to reason about whole-body behavior without directly specifying every joint trajectory. Desired center-of-mass acceleration and momentum change can then be converted into contact-force objectives and ultimately into joint-level torque, position, or hybrid commands.

Different stability representations are useful at different operating conditions. The Zero Moment Point provides a classical framework for reasoning about dynamically stable support during walking, while Capture Point and Divergent Component of Motion formulations describe how unstable center-of-mass dynamics relate to recoverable foot placement. The chapter structure intentionally develops these concepts after the general overview, with ZMP followed by Capture Point and DCM control.

Gait describes the temporal and spatial organization of leg contacts. In a biped, walking alternates between single-support and double-support phases, whereas quadrupeds can use patterns such as walk, trot, pace, bound, or dynamically generated contact sequences. A gait controller determines when each leg should support the body, when it should leave the ground, how long it remains in swing, and where it should establish the next contact according to velocity commands and stability requirements.

Gait generation can be represented through phase variables, contact schedules, finite-state machines, oscillators, trajectory generators, or optimization-based planners. A nominal gait supplies predictable timing, but real terrain requires the schedule to remain adaptable. The controller may extend a support phase, shorten a swing phase, modify a landing location, or change the next contact when state estimation indicates that the original plan is no longer appropriate. This makes gait generation both a planning and feedback problem.

The stance and swing phases require different control objectives. During stance, the leg primarily contributes to supporting and accelerating the body through controlled ground reaction forces. During swing, the same leg must clear obstacles and move toward a target foothold without generating ground force. A robust software architecture therefore changes constraints and objectives according to contact state while preserving continuity around lift-off and touchdown, where abrupt control changes can excite the mechanical system.

Foot landing is a particularly important transition because predicted contact time rarely matches physical contact exactly. Terrain height uncertainty, compliance, joint tracking error, and state-estimation error can cause early or late touchdown. Contact detection based on force, torque, joint behavior, inertial measurements, or fused signals allows the controller to identify actual landing and transition safely into stance. The provided chapter consequently treats contact detection and foot-landing control as a dedicated topic.

Foot placement links gait generation directly to balance recovery. A nominal foothold may be calculated from desired velocity and gait geometry, but feedback corrections shift that location according to body velocity, center-of-mass error, angular motion, or predicted instability. On uneven terrain, perception further constrains candidate footholds according to surface height, slope, roughness, clearance, and expected support quality. Locomotion therefore becomes an interaction between control, state estimation, and terrain-aware planning.

Swing-leg control connects consecutive support configurations. The trajectory must provide sufficient ground clearance, acceptable joint velocity and acceleration, accurate terminal placement, and a touchdown condition compatible with stable force establishment. Polynomial curves, splines, Bézier trajectories, or optimization methods may be used. More advanced implementations modify the trajectory online when foothold targets change, enabling the robot to respond to disturbances or newly perceived terrain during the same step.

At the software level, legged control is naturally hierarchical. State estimation supplies body pose, velocity, joint states, contact states, and terrain information. A gait or locomotion planner determines contact timing and footholds, while a balance or whole-body controller computes feasible body motion and contact forces. Joint-level controllers then translate these objectives into actuator commands. This hierarchy connects directly with the preceding whole-body-control material in the volume, including contact modeling, CoM control, and quadruped or humanoid WBC structures.

Model-based control and learning-based control increasingly coexist within this hierarchy. Classical models provide explicit physical constraints, interpretable stability quantities, and predictable interfaces, while reinforcement-learning policies can learn highly nonlinear coordination and adaptation from large-scale simulation. A practical architecture can retain deterministic estimation, safety, contact handling, and actuator interfaces while using a learned policy for selected locomotion decisions or command generation. The structure explicitly introduces real-time RL policy integration before complete quadruped and humanoid cases.

Real-time execution is critical because errors in contact and balance propagate rapidly. State estimation, gait phase updates, force optimization, whole-body control, and joint commands must execute with bounded latency and sufficiently consistent timing. Sensor synchronization and actuator communication are equally important because delayed contact information or stale inertial measurements can make an otherwise correct controller respond to a physical state that no longer exists. Control-loop scheduling is therefore part of locomotion performance, not merely software infrastructure.

Ultimately, contact, balance, and gait form a closed locomotion cycle. Gait determines the intended contact sequence; contact defines the forces that can physically be generated; balance control determines how those forces should regulate the floating body; and the resulting robot motion changes the state from which the next gait decision is made. This coupled view provides the conceptual foundation for progressing from ZMP and DCM stability methods to gait generation, landing control, terrain-adaptive foot placement, trajectory optimization, RL integration, and full legged-robot control stacks.

10.02 ZMP-Based Stability Control [w/Code]

Zero Moment Point (ZMP) based stability control is a classical framework for maintaining dynamic balance in legged robots, particularly during biped walking. Instead of requiring the Center of Mass (CoM) projection to remain statically inside the support region, ZMP considers the combined effects of gravity and inertial forces. This allows the controller to reason about stability while the robot accelerates, decelerates, transfers weight, and changes support contacts during locomotion.

The ZMP is defined as the point on the supporting surface where the resultant moment generated by gravity and inertial forces has no horizontal component. When the ZMP remains inside the feasible support region, the ground reaction force can generally produce the required balancing moment without causing the supporting foot or contact configuration to rotate about its boundary. The concept therefore provides a practical relationship between robot dynamics, contact forces, and available support geometry.

For a robot standing on a single flat foot, the feasible ZMP region approximately corresponds to the contact area of that foot. During double support, the admissible region expands according to the combined geometry of both supporting contacts and is commonly represented by their support polygon. A stability controller compares the desired and estimated ZMP against this region and maintains an appropriate margin from its boundaries to improve robustness against disturbances, modeling errors, and contact uncertainty.

The Center of Pressure (CoP) and ZMP are closely related but arise from different viewpoints. CoP represents the effective location at which the distributed pressure beneath a contact surface acts, while ZMP is obtained from the dynamic equilibrium of the robot. Under common assumptions such as rigid contact with a flat stationary surface, their locations can coincide. In practical software, force and torque measurements can therefore provide valuable information for estimating contact pressure behavior and validating predicted ZMP motion.

A simplified ZMP model can be derived using the Linear Inverted Pendulum Model (LIPM). The robot\'s distributed mass is approximated by a point mass at the CoM, the vertical CoM height is assumed approximately constant, and angular momentum effects are simplified. Under these assumptions, horizontal CoM acceleration becomes directly related to the difference between CoM position and ZMP position, producing a compact dynamic model suitable for walking pattern generation and real-time feedback control.

For one horizontal direction, the LIPM relationship is commonly expressed as p = x - (z/g)ẍ, where p is the ZMP position, x is the horizontal CoM position, z is the assumed CoM height, g is gravitational acceleration, and ẍ is horizontal CoM acceleration. The same principle applies to the orthogonal horizontal direction. This equation shows that ZMP depends not only on CoM position but also on acceleration, explaining why dynamic balance differs fundamentally from static balance.

ZMP-based walking control usually begins with a planned footstep sequence and corresponding support phases. The gait generator defines when each foot enters or leaves contact and constructs a desired ZMP trajectory within the resulting support regions. During single support, the reference typically remains within the supporting foot, while during double support it moves between the two feet. Smooth transitions are important because discontinuous ZMP commands can demand unrealistic CoM acceleration or abrupt contact-force changes.

Once the desired ZMP trajectory is defined, a CoM trajectory must be generated that is dynamically consistent with it. Preview control is a widely used solution because future ZMP references are already available from the planned footsteps. Rather than responding only to instantaneous tracking error, the controller considers a finite preview of upcoming references and calculates CoM motion that anticipates future support transitions. This improves smoothness and reduces tracking error during repetitive walking.

A practical ZMP feedback loop combines the reference trajectory with estimated robot state and contact measurements. The desired ZMP is generated by the gait planner, while the actual or estimated ZMP is derived from force sensing, dynamic estimation, or both. Their difference becomes a balance error that can modify CoM acceleration, body posture, ankle torque, joint references, or whole-body-control objectives. Feedback is necessary because an ideal planned trajectory cannot fully represent real contact conditions.

Ankle control provides one important mechanism for correcting moderate ZMP errors. By changing ankle torque and foot-ground pressure distribution, the controller can shift the effective support point without immediately changing the planned footstep. Hip and whole-body strategies can additionally regulate angular momentum and body orientation when ankle authority is insufficient. Modern implementations therefore treat ZMP regulation as part of coordinated whole-body behavior rather than as an isolated ankle-control algorithm.

Contact-force constraints must remain consistent with the ZMP objective. A mathematically valid reference is not physically realizable if it requires negative normal force, excessive tangential force, violation of friction limits, or actuator torque beyond available capacity. Whole-Body Control (WBC) or Quadratic Programming (QP) can distribute desired forces among active contacts while enforcing friction cones, torque limits, joint constraints, and motion objectives. ZMP can consequently operate as a higher-level stability reference within a constrained controller.

State estimation quality strongly affects ZMP control performance. The controller requires reliable estimates of body orientation, CoM position and velocity, joint configuration, and contact state. IMUs, joint encoders, foot force sensors, and force/torque sensors may be fused to obtain these quantities. Sensor noise and communication delay must be filtered carefully, because excessive filtering introduces phase delay while insufficient filtering can cause unstable or oscillatory corrections in a high-bandwidth balance loop.

Foot-contact transitions are especially challenging because the support region changes rapidly during lift-off and touchdown. If the software switches contact constraints too early or too late, the calculated ZMP region may not represent the actual physical support condition. Robust implementations therefore coordinate gait phase, contact detection, measured normal force, and transition logic. Gradually transferring desired force between contacts can reduce discontinuities and improve stability during double-to-single and single-to-double support transitions.

ZMP stability margins provide a useful supervisory measure. Instead of merely testing whether the ZMP is mathematically inside the support polygon, the controller can evaluate its distance from the polygon boundary. A decreasing margin indicates that available corrective authority is being consumed. The system may then reduce walking velocity, modify CoM motion, increase double-support duration, adjust the next foothold, or invoke another recovery strategy before the current contact configuration becomes unrecoverable.

The assumptions behind basic ZMP control also define its limitations. Flat rigid terrain, known contacts, approximately constant CoM height, and moderate angular momentum simplify the method, but dynamic locomotion can violate these assumptions. Running, jumping, strongly uneven terrain, narrow contacts, rapid body rotation, and significant vertical acceleration require richer dynamic representations. In such conditions, centroidal dynamics, contact-force optimization, Model Predictive Control (MPC), Capture Point, or Divergent Component of Motion (DCM) methods may complement or replace simplified ZMP formulations.

Within a complete legged-robot control stack, ZMP should therefore be understood as both a stability criterion and an interface between gait planning and dynamic control. Footstep planning determines future support geometry, the ZMP planner generates dynamically meaningful support references, CoM control produces compatible body motion, and lower-level whole-body and joint controllers realize the required forces and torques. This layered interpretation provides the foundation for progressing toward Capture Point and DCM based walking control, gait generation, contact-aware landing, and terrain-adaptive locomotion.

10.03 Capture Point and DCM-Based Walking Control [w/Code]

Capture Point (CP) and Divergent Component of Motion (DCM) based walking control provide dynamic stability frameworks that explicitly describe how the unstable component of a legged robot's motion evolves during walking. Unlike approaches that primarily regulate the Zero Moment Point (ZMP) within a support region, CP and DCM methods connect Center of Mass (CoM) position and velocity directly to future balance. They are especially useful for footstep adjustment, disturbance recovery, and dynamically responsive biped locomotion.

The underlying concept is commonly derived from the Linear Inverted Pendulum Model (LIPM), where the robot is approximated as a point mass located at a nearly constant CoM height above the ground. Assuming simplified angular momentum and vertical motion, the horizontal dynamics contain both stable and unstable components. The unstable component naturally diverges unless appropriate ground reaction forces or future contacts are selected, which motivates the definitions of the Capture Point and DCM.

For a constant CoM height z, the natural frequency of the inverted pendulum is commonly defined as ω = √(g/z), where g represents gravitational acceleration. The Capture Point can then be expressed in one horizontal direction as ξ = x + ẋ/ω, where x is CoM position and ẋ is CoM velocity. This equation combines position and velocity into a state indicating where support must ultimately be established to arrest unstable CoM motion under the simplified model.

The physical interpretation of the Capture Point is intuitive for walking control. If a robot is moving and begins to lose balance, merely examining the vertical projection of its CoM does not indicate where the next foot should land. CoM velocity must also be considered. The Capture Point estimates a ground location associated with bringing the unstable motion toward rest. A faster body motion shifts the required capture location farther in the direction of travel, naturally connecting balance recovery with foot placement.

The Divergent Component of Motion generalizes this idea as a continuously evolving dynamic state. Under the basic LIPM formulation, DCM is commonly defined using the same expression ξ = x + ẋ/ω. Rather than interpreting ξ only as a final capture location, DCM control treats it as a trajectory that can be planned and tracked throughout locomotion. This makes DCM particularly suitable for coordinating successive support phases, continuous walking, and transitions between multiple footsteps.

DCM dynamics can be related directly to the ZMP or another equivalent support input. For a basic LIPM, the relationship can be written as ξ̇ = ω(ξ − p), where p represents the ZMP. The equation reveals the divergent nature of the system: when the DCM differs from the support input, the difference tends to grow exponentially without corrective action. Stable walking therefore requires the controller to deliberately shape ZMP behavior, DCM evolution, or future contact locations.

A DCM walking planner typically begins with a sequence of desired footsteps and contact timings. Each support phase defines a feasible region in which the ZMP or equivalent contact-force resultant can be controlled. Working backward from desired terminal conditions, the planner can construct a DCM reference trajectory across future support phases. This backward planning is useful because the unstable dynamics require future contact information to determine a current trajectory that remains compatible with upcoming footsteps.

During single support, the DCM reference evolves according to the selected support location and timing. During double support, the reference can be transitioned smoothly from one support foot toward the next. Instead of commanding abrupt changes at contact transitions, practical trajectory generators use continuous interpolation or dynamically consistent transition models. Smooth DCM evolution reduces sudden changes in required ground reaction forces and improves compatibility with whole-body and joint-level controllers.

The DCM tracking controller compares the desired DCM with an estimated DCM calculated from measured or estimated CoM position and velocity. The resulting error can be converted into a corrective ZMP command or contact-force objective. Because DCM contains velocity information, deviations caused by pushes or tracking errors can become visible earlier than they would through position-only balance measures. This enables the walking controller to respond before CoM displacement becomes excessively large.

When sufficient control authority remains inside the current support region, ankle and whole-body actions can regulate the ZMP and drive the DCM toward its reference without changing the footstep plan. The controller may redistribute contact pressure, modify body acceleration, or adjust angular momentum. However, the available ZMP is constrained by the physical support area. Once the required correction approaches or exceeds that region, modifying the next foot placement becomes an important recovery mechanism.

Footstep adaptation is therefore a central advantage of Capture Point and DCM based control. A measured DCM error can be propagated forward to estimate how the next foothold should change. The controller may shift the step forward, backward, or laterally and may also modify step timing. Position adaptation changes the future support geometry, while timing adaptation changes how long the unstable dynamics evolve before the next contact. Combining both provides greater recovery authority than either mechanism alone.

Step timing has a strong effect because DCM divergence is exponential with respect to time. A small disturbance early in a support phase can grow significantly if the robot waits too long before establishing the next contact. Conversely, accelerating touchdown can limit divergence, provided the swing leg can physically reach the required location. Practical control must therefore consider swing-leg kinematics, joint velocity and acceleration limits, terrain availability, collision constraints, and actuator capability before accepting a modified step.

Capture Point reasoning can also support multi-step recovery. A disturbance may be too large to compensate using the current support polygon or a single adjusted step. Instead of forcing an infeasible immediate correction, the controller can distribute recovery across several future footsteps. Each new contact changes the support geometry and provides additional control authority. This allows the robot to gradually reduce unstable motion while maintaining feasible step length, timing, contact forces, and joint motion.

Accurate state estimation is essential because DCM explicitly depends on CoM velocity, which is generally more sensitive to sensor noise and estimation errors than position. IMU measurements, joint encoders, kinematic models, contact sensing, and sometimes external perception are fused to estimate floating-base motion and CoM state. Filtering must balance noise suppression against delay, since excessive latency can cause the controller to react to an outdated DCM state in rapidly evolving locomotion.

DCM commands are normally integrated with Whole-Body Control (WBC) rather than sent directly to individual joints. The walking controller generates desired CoM behavior, ZMP or contact-force objectives, foot trajectories, and posture targets. A WBC or optimization layer then calculates feasible joint torques or accelerations while respecting contact constraints, friction limits, actuator limits, and kinematic objectives. This separates high-level dynamic stability regulation from detailed multi-joint coordination.

Capture Point and DCM control extend naturally beyond nominal walking into disturbance rejection and terrain-adaptive locomotion. Perception can constrain candidate footholds to stable terrain while the balance controller determines which candidates satisfy dynamic recovery requirements. When no feasible foothold exists within the nominal gait pattern, the system can alter step length, direction, timing, or contact sequence. Balance control consequently becomes tightly coupled with contact planning and environmental perception.

Within the legged-robot control stack, CP and DCM methods form a bridge between ZMP-based stability concepts and more advanced gait and foot-placement control. ZMP describes how support forces can be regulated within existing contacts, while CP and DCM expose how current CoM motion influences future balance and required contacts. Together they provide a structured foundation for gait pattern generation, contact-aware landing, terrain-adaptive foot placement, swing-leg optimization, and more advanced model-based or learning-assisted locomotion.

10.04 Gait Pattern Generator (GPG) SW Design [w/Code]

A Gait Pattern Generator (GPG) is the software component that organizes leg contacts and motion references into a coordinated locomotion pattern. It converts high-level commands such as desired forward velocity, lateral velocity, turning rate, and locomotion mode into time-dependent support phases, contact schedules, foothold references, and swing-leg trajectories. In a legged control stack, the GPG bridges motion intent and lower-level balance, whole-body, and joint control.

The central responsibility of a GPG is to determine which legs are in stance and which are in swing at every instant. A contact schedule represents this information over time and defines events such as lift-off, touchdown, single support, double support, or multi-leg support. The schedule must remain synchronized with the physical robot because balance and contact-force controllers depend on accurate knowledge of which feet can currently exchange forces with the environment.

Gait phase provides a compact representation of progression through a locomotion cycle. A normalized phase variable can evolve from zero to one and reset or wrap when a cycle is completed. Individual legs may use phase offsets so that their stance and swing intervals occur at different times. This representation is especially useful for periodic locomotion because walking, trotting, pacing, and other gait patterns can be described by changing phase relationships, duty factors, and cycle periods.

The duty factor defines the fraction of a gait cycle during which a leg remains in contact with the ground. A larger duty factor generally produces longer support periods and greater overlap among supporting legs, while a smaller value produces more dynamic locomotion with longer relative swing intervals. GPG software can modify duty factor together with gait frequency and step length to adapt locomotion speed while preserving feasible contact timing and sufficient time for swing-leg motion.

For a biped robot, the gait generator typically coordinates alternating left and right support with carefully managed double-support transitions. A walking cycle can contain left single support, double support, right single support, and another double-support interval. The generator must produce timing information that remains compatible with ZMP, Capture Point, or DCM references so that foot transitions do not violate the assumptions used by the dynamic balance controller.

Quadruped gait generation introduces additional combinations of contact states. A walk normally maintains several supporting legs and emphasizes static or quasi-static stability, while a trot coordinates diagonal leg pairs. Pace coordinates legs on the same side, and bound patterns coordinate front and rear pairs. Rather than implementing every gait as unrelated logic, a reusable GPG can represent these patterns through phase offsets, duty factors, contact sequences, and transition parameters.

Command processing is another important GPG function. Desired body velocity cannot simply be mapped directly to joint motion because each future foothold must be compatible with the commanded translation and rotation. The generator converts velocity commands into nominal step displacement, heading change, stride period, and foothold locations. These references can then be corrected by balance controllers according to CoM velocity, DCM error, terrain conditions, or disturbance recovery requirements.

Footstep generation determines where each swing leg should establish its next contact. A nominal foothold can be calculated from commanded body motion, expected stance duration, robot geometry, and current base state. Feedback terms may shift this location to compensate for velocity error or instability. The GPG should therefore distinguish between nominal gait geometry and corrective foot placement so that higher-level balance algorithms can modify steps without destroying the underlying gait coordination.

Once a foothold and swing interval are defined, the GPG can provide boundary conditions to a swing-leg trajectory generator. The trajectory begins near the lift-off position, rises to provide terrain clearance, progresses toward the target foothold, and approaches the surface with an appropriate terminal velocity. Polynomial, spline, or Bézier representations are commonly suitable because their position, velocity, and acceleration profiles can be generated smoothly and modified online when the target changes.

Stance references require a different interpretation. A supporting foot is normally treated as stationary relative to the terrain while the body moves with respect to it. The GPG therefore maintains contact constraints and support-phase timing rather than commanding the stance foot through a free-space trajectory. Lower-level whole-body control uses these constraints to calculate joint motion and ground reaction forces while limiting unwanted foot translation, rotation, or slip.

Finite-State Machines (FSMs) provide a straightforward software structure for gait sequencing. States may represent standing, gait initialization, stance, swing, transition, stopping, or recovery, while events trigger changes between them. FSM logic is particularly useful for enforcing deterministic transition rules. However, phase-based representations can provide smoother periodic coordination, so practical GPG implementations often combine an FSM for locomotion modes with continuous phase variables inside each active gait.

Gait transitions require special treatment because directly switching phase relationships can create simultaneous lift-off events, discontinuous footholds, or insufficient support. A transition manager can wait for a suitable synchronization point and gradually modify frequency, duty factor, phase offset, or contact sequence. For example, changing from walk to trot should preserve currently loaded contacts until the robot reaches a configuration from which the new diagonal coordination can begin without an abrupt loss of support.

Contact detection closes the gap between scheduled gait and physical execution. The GPG may predict touchdown at a specific phase, but actual landing can occur earlier or later because of terrain height, compliance, tracking error, or disturbances. Force sensing, joint torque, kinematic residuals, or other contact estimators can confirm physical contact. The software can then terminate swing, establish stance constraints, reset phase information, or temporarily modify the schedule to maintain synchronization.

Terrain-adaptive gait generation extends this mechanism by using perception and local terrain models. Candidate footholds can be evaluated according to height, slope, roughness, obstacle clearance, and expected contact quality. When the nominal foothold is unsuitable, a terrain planner can provide a nearby feasible target while the GPG preserves the overall contact sequence. Large terrain changes may additionally require adjustment of step height, swing duration, body posture, or gait frequency.

The GPG must interact closely with balance control. ZMP-based controllers require support polygons and future contact timing, while Capture Point and DCM controllers may request changes to foot placement or touchdown timing. The gait generator therefore cannot be treated as a purely open-loop trajectory player. It must expose interfaces through which stability controllers can modify future contacts while enforcing kinematic reachability, minimum support duration, collision constraints, and gait consistency.

A robust software architecture separates gait definition, phase management, contact scheduling, foothold generation, trajectory generation, transition management, and safety supervision into clear modules. Parameters such as gait period, duty factor, phase offsets, step dimensions, clearance height, and transition thresholds should be centrally managed and traceable. This modularity allows the same control stack to support multiple robot configurations and makes simulation, calibration, debugging, and real-time tuning significantly easier.

Real-time implementation requires deterministic updates because phase errors propagate directly into contact and balance errors. A fast control loop updates measured state and joint commands, while gait planning may operate at a lower but synchronized rate. Time stamps and explicit state transitions help prevent inconsistencies between planned and measured contact. Safety logic should also detect impossible schedules, missed contacts, excessive tracking errors, or loss of sufficient support and request recovery or controlled stopping.

Within the complete legged-robot software stack, the GPG functions as the temporal coordinator of locomotion. High-level motion commands enter the gait generator, which creates contact phases, nominal footholds, and swing references. Balance and terrain modules modify these references according to dynamic and environmental conditions, while whole-body and joint controllers realize them through feasible forces and actuator commands. This architecture provides the foundation for contact-aware landing, terrain-adaptive foot placement, swing-leg optimization, and learning-assisted locomotion.

10.05 Contact Detection and Foot Landing Control [w/Code]

Contact detection and foot landing control manage one of the most critical transitions in legged locomotion: the change from a freely moving swing leg to a load-bearing stance leg. A gait planner can predict when touchdown should occur, but the actual event depends on terrain height, compliance, tracking error, body motion, and disturbances. Reliable locomotion therefore requires the controller to detect physical contact and adapt the landing process instead of relying only on scheduled gait timing.

During the swing phase, the foot is commanded through free space toward a target foothold while producing essentially no intentional ground reaction force. At touchdown, the control objective changes rapidly because the foot becomes part of the support structure. Position tracking must give way to contact-force regulation and constrained motion. If this transition occurs at the wrong time, the controller may command incompatible motion or forces, producing impact, slip, instability, or excessive actuator loading.

Contact detection estimates whether a foot is physically interacting with the environment. Direct methods use foot force sensors, load cells, or six-axis force/torque sensors to measure normal and tangential forces. A normal-force threshold provides a simple touchdown indicator, but practical implementations usually include filtering, hysteresis, and temporal confirmation. These mechanisms prevent sensor noise or short impact spikes from repeatedly switching the estimated contact state.

Joint torque sensing provides another source of contact information. When a swing foot encounters the ground, external forces propagate through the leg and change measured or estimated joint torques. The controller can compare expected torque from the robot model with measured actuator torque and calculate a residual associated with external contact. This approach can reduce dependence on dedicated foot sensors, although model uncertainty, friction, actuator dynamics, and transmission effects must be considered.

Kinematic contact detection uses discrepancies between expected and observed motion. If a commanded foot continues moving downward but its estimated Cartesian position or velocity no longer follows the expected trajectory, contact may have occurred. IMU information and floating-base estimation can improve this interpretation because apparent foot motion depends on both joint motion and body motion. Combining kinematic residuals with force or torque information generally produces a more reliable contact estimate than relying on a single signal.

A robust contact estimator can therefore use sensor fusion rather than a single threshold. Force, torque, joint velocity, motor current, foot acceleration, IMU data, and predicted gait phase can contribute evidence to a contact probability or discrete contact state. The gait phase provides useful prior information because contact is more likely near the expected touchdown interval. However, physical measurements should be able to override the schedule when early contact, late contact, or unexpected collision occurs.

Early touchdown occurs when the foot encounters terrain before the planned landing time. This can happen on an unexpected obstacle, an elevated surface, or incorrectly estimated terrain. Continuing the nominal downward swing trajectory after contact can generate a large impact or excessive joint force. Once early contact is confirmed, the controller should terminate or reshape the swing trajectory, establish the appropriate contact constraint, and transfer load gradually while informing the gait and balance layers of the changed timing.

Late touchdown occurs when the expected ground contact does not appear at the scheduled time. A depression, step-down, terrain estimation error, or insufficient leg extension may cause this condition. Immediately switching to stance despite the absence of contact would create a false support assumption. Instead, the controller can extend the search trajectory downward within safe kinematic limits, reduce landing velocity, delay force transfer, and update the support state until valid contact is detected or a failure condition is declared.

Foot landing control should minimize impact while establishing support quickly enough for dynamic balance. A high downward velocity at touchdown produces large impulsive forces, whereas an excessively slow approach may delay support and reduce gait performance. Swing trajectories therefore commonly use a controlled terminal position and velocity near the expected surface. Terrain uncertainty can be accommodated by defining a landing or search region rather than assuming that contact will occur at one exact vertical coordinate.

Compliance is particularly useful during the transition from swing to stance. Instead of enforcing a rigid position command immediately after touchdown, impedance control can regulate the relationship between foot displacement and contact force. Appropriate virtual stiffness and damping allow the leg to absorb impact energy and settle onto the surface. The controller can then increase support stiffness or transition toward force control as contact becomes stable and the desired load is established.

Contact confirmation and load transfer should be treated as separate events. Detecting an initial force spike does not necessarily mean that the foot has achieved a stable supporting contact. After touchdown, the controller can verify sustained normal force, acceptable tangential force, low relative foot velocity, and consistent contact geometry. Only after these conditions are satisfied should the gait manager classify the foot as fully load-bearing and allow the balance controller to rely on it as a support contact.

Slip detection extends contact estimation beyond simple touchdown recognition. A foot may carry normal force while moving relative to the terrain, making the assumed rigid-contact constraint invalid. Tangential force approaching the friction limit, unexpected foot velocity, or disagreement between kinematic and inertial estimates can indicate slipping. The controller may respond by reducing tangential force demand, modifying body motion, redistributing load to other feet, or initiating a recovery step.

The estimated contact state directly changes the support geometry used by balance control. In biped walking, a confirmed touchdown may transform a single-support configuration into double support, while lift-off produces the opposite transition. In quadrupeds, each contact event changes the available support polygon and force-distribution problem. ZMP, Capture Point, DCM, or whole-body controllers must therefore receive contact-state updates with minimal delay and with enough confidence to avoid unstable switching.

Force transfer between contacts should be continuous whenever possible. During a planned double-support transition, the outgoing foot can gradually reduce its normal-force reference while the incoming foot increases its load. This avoids sudden redistribution of ground reaction forces and reduces discontinuities in body acceleration. The contact scheduler, landing controller, and Whole-Body Control (WBC) layer should share consistent transition timing so that motion constraints and force objectives change coherently.

Whole-Body Control integrates landing information with the dynamics of the complete robot. Before touchdown, the swing foot is typically represented as a motion task. After confirmed contact, it becomes a contact constraint capable of generating ground reaction force. A QP-based WBC can activate the new constraint, enforce friction limits, distribute forces among supporting feet, regulate CoM and posture, and respect joint torque limits. Smooth task weighting can reduce discontinuity during this mode change.

Terrain perception can improve landing control by predicting surface height, orientation, and local geometry before contact occurs. The foot trajectory can approach the surface along an appropriate direction and orient the sole according to estimated terrain normal. Nevertheless, perception is imperfect, so contact sensing remains necessary as a final physical confirmation. Combining predictive terrain information with reactive contact detection provides both efficient approach motion and robustness against local modeling errors.

Safety supervision must handle abnormal landing events that ordinary feedback cannot resolve. Examples include missing ground beyond the leg workspace, unexpectedly large impact force, repeated contact loss, severe slip, joint-limit approach, or collision with an obstacle during swing. The controller can freeze gait progression, redistribute support forces, retract or reposition the leg, lower the body, initiate a recovery step, or request a controlled stop according to the available support configuration.

Real-time implementation requires careful coordination among sensing, estimation, gait scheduling, trajectory generation, balance control, and actuator interfaces. Contact events occur quickly, so excessive filtering or communication delay can make the software respond after a significant impact has already developed. Event timestamps, deterministic state transitions, bounded filtering delays, and high-rate force or torque processing help maintain synchronization between the estimated contact state and the actual physical interaction.

Within the complete legged-robot control stack, contact detection and foot landing control form the physical synchronization layer between planned gait and real terrain interaction. The gait generator predicts touchdown, perception estimates the expected surface, sensors determine when contact actually occurs, landing control manages impact and load transfer, and balance or whole-body control incorporates the new support condition. This foundation enables terrain-adaptive foot placement, swing-leg optimization, disturbance recovery, and robust locomotion over uncertain environments.

10.06 Terrain Adaptive Foot Placement Planning [w/Code]

Terrain-adaptive foot placement planning enables a legged robot to select safe and dynamically useful footholds when the ground cannot be treated as flat or uniform. Instead of executing nominal footsteps generated only from commanded velocity and gait geometry, the planner incorporates terrain perception, robot state, contact constraints, and balance requirements. The resulting foothold must simultaneously support locomotion intent, physical reachability, contact stability, and future body motion.

The planning process begins with a representation of the local terrain surrounding the robot. Depth cameras, stereo vision, LiDAR, or other ranging sensors can provide three-dimensional measurements that are transformed into a robot-centered or world-fixed coordinate frame. These measurements may be represented as point clouds, elevation maps, height maps, surface patches, or traversability grids. The representation must preserve enough geometric information to distinguish usable support surfaces from gaps, obstacles, steep slopes, and irregular regions.

An elevation map is particularly useful because it associates horizontal locations with estimated terrain height and can be updated continuously as the robot moves. Additional layers can describe surface slope, roughness, uncertainty, obstacle distance, or semantic properties. Before foothold evaluation, the terrain data should be filtered to reduce sensor noise and isolated artifacts. Uncertainty should remain explicitly represented where possible because an apparently precise but incorrect terrain estimate can be more dangerous than a known uncertain region.

The gait generator provides a nominal foothold derived from desired velocity, heading, gait timing, and robot geometry. On flat terrain, this nominal location may already be acceptable. On irregular terrain, however, it becomes the center of a local search rather than an unconditional command. The terrain-adaptive planner examines candidate locations around the nominal foothold and determines which positions provide suitable support without excessively changing the intended gait.

Candidate footholds must first satisfy geometric constraints. The surface should provide sufficient area for the foot, avoid sharp discontinuities, and remain within acceptable slope limits. A foothold near the edge of a step, gap, or rock may be rejected even when its center point appears valid because part of the foot could remain unsupported. For robots with finite-size feet, evaluating the complete contact patch rather than a single terrain point significantly improves landing reliability.

Surface orientation also influences contact quality. When the terrain is inclined, the desired foot orientation can be aligned with the estimated local surface normal within the available ankle or leg range. Excessive inclination may exceed joint limits, reduce friction margin, or create unfavorable loading. The planner therefore evaluates both foothold position and orientation, allowing downstream swing and whole-body controllers to prepare the foot for physically compatible contact.

Kinematic reachability limits the candidate region according to the current body pose and leg configuration. A terrain location may be geometrically excellent but unusable if the leg cannot reach it without violating joint limits or approaching singular configurations. Practical planners use workspace models, inverse kinematics, reachability maps, or simplified geometric bounds to reject such candidates before expensive optimization. A safety margin can preserve additional joint range for disturbance rejection after touchdown.

Collision constraints must also be considered throughout the step. Selecting a valid landing location is insufficient if the swing leg collides with terrain while moving toward it. The planner can evaluate obstacles around the foot and lower leg and provide information to the swing trajectory generator for adjusting clearance height or approach direction. Narrow passages, stairs, rocks, and vegetation may require a trajectory that differs substantially from the nominal smooth arc used on flat terrain.

Each feasible candidate can be assigned a terrain quality cost. Typical terms penalize surface slope, roughness, height discontinuity, proximity to edges, obstacle distance, perception uncertainty, or limited contact area. Rather than optimizing terrain quality alone, the cost should also penalize excessive displacement from the nominal foothold. This creates a compromise between selecting the safest local surface and preserving the gait geometry required to achieve the commanded body velocity.

Dynamic stability introduces another set of constraints. The selected foothold determines the future support geometry and therefore influences ZMP, Capture Point, DCM, and contact-force feasibility. A candidate close to the nominal position may still be unsuitable if it cannot capture the robot\'s current motion. Balance feedback can shift the preferred foothold according to CoM velocity or DCM error, while terrain evaluation determines which nearby positions are physically available. Foot placement consequently becomes a coupled balance and perception problem.

Step timing can be optimized together with foothold location. If the best terrain patch lies slightly farther away, additional swing time may be necessary for the leg to reach it safely. Conversely, a growing balance error may require earlier touchdown even if the terrain choice is less ideal. The planner must coordinate spatial and temporal decisions while respecting swing velocity, acceleration, joint limits, and minimum support durations imposed by the gait pattern.

When no valid foothold exists near the nominal target, the system should not simply choose the least invalid candidate. The planner can expand the search region within reachable limits, shorten or redirect the step, modify gait timing, request a different contact sequence, or reduce commanded velocity. If a safe solution still cannot be found, locomotion should transition toward recovery or controlled stopping rather than committing to a contact that is predicted to fail.

Terrain adaptation must remain responsive to new perception during the swing phase. A foothold selected at lift-off may become invalid as additional terrain is observed or state estimation improves. Online replanning can move the target while the foot is already in motion, provided the revised location remains reachable within the remaining swing time. The trajectory generator should therefore accept target updates and smoothly reshape the remaining path without introducing discontinuous position, velocity, or acceleration commands.

Contact detection provides final verification of the terrain model. Even a carefully selected foothold may encounter the surface earlier or later than predicted because of perception error or local compliance. The landing controller detects actual touchdown, manages impact, and confirms stable support before the gait system fully relies on the new contact. The difference between predicted and measured terrain height can also be fed back to update the local terrain representation and improve subsequent foothold decisions.

For quadruped robots, terrain-adaptive planning must consider the interaction among several supporting and moving legs. Changing one foothold alters the support polygon, body posture, and reachable region of later steps. Sequential planning can evaluate one swing leg at a time, while optimization-based methods can consider several future contacts jointly. Looking beyond the immediate step is particularly valuable on stairs, stepping stones, rubble, or other terrain where locally attractive footholds may lead to poor future configurations.

For bipeds, foothold selection is strongly coupled with dynamic balance because each step often creates the primary support region for the following phase. Forward and lateral placement influence walking velocity, DCM evolution, and recovery capability. Terrain constraints may prevent the dynamically preferred location from being used, requiring the controller to modify CoM motion, step timing, or subsequent footsteps. Multi-step planning can distribute these corrections over several contacts instead of demanding an infeasible immediate step.

Real-time implementation benefits from separating terrain processing, candidate generation, feasibility filtering, scoring, and trajectory updates into modular stages. Fast geometric tests can eliminate obviously invalid candidates before more expensive kinematic or dynamic evaluation. Parallel processing may evaluate multiple candidate footholds simultaneously. The planner should also expose confidence and failure information so that gait, balance, and safety modules understand when terrain constraints are reducing available locomotion options.

Within the complete legged-robot control stack, terrain-adaptive foot placement planning connects perception directly to locomotion and balance. Perception constructs the local terrain model, the gait generator supplies nominal footsteps and timing, the foothold planner selects feasible contacts, and balance control introduces dynamic corrections. Swing-leg and landing controllers then execute and verify the selected contact, while whole-body control coordinates the resulting forces and body motion. This closed-loop structure enables robust locomotion across slopes, stairs, rocks, gaps, and other uncertain terrain.

10.07 Swing Leg Trajectory Optimization [w/Code]

Swing-leg trajectory optimization determines how a leg should move from lift-off to the next foothold while satisfying locomotion timing, terrain clearance, kinematic limits, and landing requirements. Unlike a fixed geometric swing curve, an optimized trajectory considers the robot state and environmental constraints together. The objective is not merely to reach the target position, but to produce a smooth, feasible, energy-conscious motion that prepares the foot for reliable contact.

The swing phase begins when the supporting role of a leg is removed and its contact constraint is released. At this moment, the trajectory generator receives the current foot position, velocity, and sometimes acceleration as initial boundary conditions. The desired foothold, foot orientation, touchdown time, and terminal velocity define the final conditions. Maintaining consistency with the measured initial state prevents discontinuities that could otherwise generate sudden joint acceleration or torque commands.

A swing trajectory is commonly represented in Cartesian space because the desired behavior is naturally described by the foot position and orientation relative to the terrain. Horizontal motion determines progression toward the foothold, while vertical motion provides clearance. Polynomial functions, cubic or quintic splines, Bézier curves, and piecewise trajectories can generate continuous profiles. Higher-order representations allow position, velocity, and acceleration constraints to be imposed simultaneously at important transition points.

Vertical clearance is a primary trajectory requirement. The foot must rise sufficiently above the terrain to avoid collision, but excessive lifting increases joint motion, energy consumption, and required swing velocity. On flat terrain, a predefined clearance height may be adequate. On uneven terrain, the trajectory should use terrain geometry along the entire swing path rather than considering only the height of the destination, because intermediate rocks, steps, or surface protrusions can create collision risks.

Terrain-aware optimization can formulate clearance as an inequality constraint between the predicted foot path and the local terrain model. A safety margin is added to account for perception uncertainty, tracking error, and finite foot dimensions. Instead of forcing a uniformly high trajectory, the optimizer can raise only the portions of the path that pass over obstacles. This reduces unnecessary motion while maintaining sufficient clearance and improves efficiency when repeatedly traversing irregular environments.

Kinematic feasibility constrains the trajectory according to the reachable workspace of the leg. Every point along the path should correspond to a valid joint configuration without violating joint position limits or approaching undesirable singularities. Inverse kinematics can evaluate candidate trajectories, while reachability maps or simplified workspace constraints can accelerate real-time planning. Maintaining a margin from joint limits also preserves corrective authority for disturbances and late foothold adjustments.

Joint velocity and acceleration constraints are equally important because a geometrically reachable trajectory may still demand motion faster than the actuators can execute. Short swing duration, large step displacement, or high obstacle clearance can produce excessive joint rates. Optimization can penalize or explicitly constrain Cartesian and joint-space velocity and acceleration. If no feasible solution exists, the gait planner may need to increase swing duration, shorten the step, or modify the target foothold.

Smoothness objectives reduce mechanical shock and improve tracking quality. Common cost functions penalize squared velocity, acceleration, jerk, or combinations of these quantities over the trajectory. Minimizing acceleration can reduce dynamic effort, while minimizing jerk produces smoother transitions and may reduce vibration in the mechanical structure. The weighting of these objectives depends on robot morphology, actuator characteristics, gait speed, and whether efficiency or highly dynamic motion has priority.

Energy-related objectives can further improve swing efficiency. Moving a leg rapidly requires actuator torque to accelerate and decelerate link inertia, and unnecessary vertical motion increases mechanical work. A simplified optimizer may minimize trajectory length or joint acceleration, while a more detailed formulation can estimate torque, mechanical power, or electrical energy using the robot dynamics. Energy optimization must remain subordinate to collision avoidance, balance, timing, and safe landing constraints.

Foot orientation should be optimized together with position when the terrain is not horizontal. Terrain perception can estimate the local surface normal at the target foothold, allowing the sole to approach with an orientation compatible with the expected contact surface. Orientation interpolation must respect ankle and leg limits and should avoid abrupt rotational motion near touchdown. Preparing orientation before contact can reduce edge impacts and improve the initial distribution of ground reaction force.

Touchdown conditions strongly influence the final segment of the swing trajectory. A large downward velocity can produce excessive impact force, whereas zero velocity too far above the actual terrain may make the foot approach inefficiently. The optimizer can specify a controlled terminal velocity and an approach direction based on terrain uncertainty. Near the expected surface, the trajectory may transition into a slower landing or search mode so that contact detection can safely determine the actual touchdown instant.

Swing timing is coupled with dynamic balance. Capture Point and DCM based controllers may request earlier touchdown when instability grows, while terrain constraints may require additional time to reach a distant or elevated foothold. The trajectory optimizer must therefore determine whether a requested contact time is physically achievable. Timing adaptation can be performed jointly with spatial optimization, subject to actuator capability, joint limits, gait synchronization, and minimum clearance requirements.

Online trajectory replanning is essential when the target foothold changes during swing. Terrain-adaptive foot placement or disturbance recovery may shift the landing position after lift-off. Rather than restarting the trajectory from its original initial condition, the planner uses the current measured or commanded foot state as a new boundary condition and optimizes the remaining segment. Continuity of position, velocity, and preferably acceleration prevents abrupt commands when the updated trajectory is activated.

Collision avoidance should consider more than the foot alone. The shin, knee, and other leg links may collide with stairs, rocks, or structural obstacles even when the foot trajectory itself is clear. A more complete optimizer incorporates robot-link geometry and minimum-distance constraints against the local environment. Self-collision constraints may also be required for large lateral steps, turning motions, crossed-leg maneuvers, or robots with closely spaced limbs.

Optimization methods vary according to computational resources and control requirements. Closed-form polynomial trajectories are extremely fast and predictable but provide limited flexibility. Numerical optimization can directly incorporate nonlinear kinematics, collision constraints, terrain geometry, and dynamic costs, although computational latency must be bounded. Practical systems often combine a fast nominal trajectory generator with local optimization or constraint-based correction, providing predictable behavior while retaining environmental adaptability.

The optimized Cartesian foot trajectory must eventually be converted into commands compatible with Whole-Body Control (WBC). Rather than independently solving inverse kinematics for the swing leg and ignoring the rest of the robot, WBC can treat foot position, velocity, and acceleration as motion objectives while simultaneously regulating CoM, posture, stance contacts, and ground reaction forces. This allows swing motion to remain consistent with the dynamic requirements of the complete robot.

Tracking performance should be monitored throughout execution because an optimized reference is useful only if the physical leg can follow it. Large position error, actuator saturation, unexpected contact, or delayed motion can make the remaining trajectory infeasible. The controller can reduce swing speed, modify the path, request a timing adjustment, or initiate landing and recovery logic. Contact detection must override free-space tracking when the foot encounters terrain earlier than predicted.

Real-time software benefits from separating nominal trajectory generation, constraint evaluation, optimization, online replanning, and execution monitoring. Terrain and foothold modules provide environmental and target information, while the gait generator provides phase and timing. The optimizer produces a feasible reference that is consumed by WBC and joint control. Explicit validity flags and failure states allow the locomotion supervisor to respond safely when optimization cannot satisfy all required constraints.

Within the complete legged-robot control stack, swing-leg trajectory optimization connects terrain-adaptive foothold planning with physical contact execution. The foothold planner determines where the next support should occur, while trajectory optimization determines how the leg can reach it safely and smoothly within the available time. Landing control then confirms contact and transfers load to the new support. This coordinated process enables efficient, collision-aware, dynamically consistent locomotion over uncertain and irregular terrain.

10.08 RL Policy Real-Time Inference Integration [w/Code]

Reinforcement Learning (RL) policy integration allows a legged robot to convert learned locomotion behavior into real-time actuator or control references during physical operation. A trained policy maps observations of the robot and environment to actions that influence joint motion, torque, or higher-level control targets. Real-time integration therefore requires more than deploying a neural network: sensing, preprocessing, inference, safety supervision, and deterministic control interfaces must operate as one coordinated execution pipeline.

The RL policy is normally trained before deployment using simulation, recorded experience, or a combination of simulated and physical data. During training, the policy learns a mapping from an observation vector to an action vector by maximizing a reward related to velocity tracking, balance, energy efficiency, contact behavior, posture, or terrain traversal. Once training is complete, the resulting neural network parameters are exported and integrated into the robot software without requiring online gradient updates during ordinary inference.

The observation vector defines what information the policy receives at every inference step. Typical inputs include joint positions, joint velocities, base orientation, angular velocity, estimated gravity direction, commanded body velocity, previous actions, and gait-related information. Terrain-aware policies may additionally consume height samples, depth features, or latent representations of local geometry. Observation definitions used during deployment must remain consistent with those used during training to prevent distribution mismatch.

State estimation therefore forms an essential upstream component of RL inference. Encoder measurements provide joint states, while an Inertial Measurement Unit (IMU) contributes body orientation and angular motion information. Contact estimators and kinematic models may provide additional support-state information. These signals are synchronized, filtered, and transformed into the coordinate conventions expected by the policy. Incorrect sign conventions, frame definitions, scaling, or sensor timing can severely degrade an otherwise well-trained controller.

Observation normalization must also reproduce the training configuration precisely. Neural policies are commonly trained using scaled or normalized inputs so that quantities with different physical units remain within numerically suitable ranges. Deployment software applies the same offsets, scale factors, clipping limits, and ordering before inference. Normalization parameters should therefore be treated as part of the policy artifact rather than as independent tuning values that can be modified without validation.

The policy inference loop executes the neural network at a predefined control frequency. The inference rate may be lower than the low-level actuator control frequency because the policy output can be held or interpolated between updates. For example, a high-rate motor loop can continue regulating torque or position while an RL policy generates new references at a slower rate. This multi-rate architecture separates computationally intensive neural inference from deterministic actuator regulation.

Policy actions can represent several different control interfaces. A policy may directly generate desired joint torques, but many practical systems output desired joint positions, position offsets, joint velocities, or references consumed by a lower-level impedance or PD controller. Higher-level policies may instead generate desired foot positions, body motion, contact parameters, or gait commands. The selected action space strongly influences training difficulty, control bandwidth, safety architecture, and transfer from simulation to hardware.

When joint position targets are used, the policy output is often scaled around a nominal pose before being passed to a joint controller. The resulting command can be expressed conceptually as a nominal joint configuration plus a bounded policy-generated offset. A PD or impedance controller then converts the reference into actuator torque using measured position and velocity feedback. This structure preserves a deterministic inner control loop while allowing the learned policy to coordinate whole-body locomotion.

Real-time execution requires bounded inference latency and predictable scheduling. If observations are sampled at one instant but actions are applied after an excessive or variable delay, the policy effectively operates on an outdated robot state. Timestamped sensor data, preallocated memory, fixed tensor dimensions, asynchronous logging, and controlled execution priorities help reduce jitter. Worst-case latency is often more important than average inference time because occasional delays can destabilize highly dynamic locomotion.

The neural network runtime must be selected according to the deployment processor and timing requirements. A training model may originate in PyTorch and then be exported through formats such as TorchScript or ONNX, depending on the software architecture. Hardware-specific runtimes can further optimize execution through graph optimization, kernel fusion, reduced precision, or accelerator-specific compilation. Every conversion step must be validated because numerical differences can change policy behavior.

Reduced-precision inference can improve throughput and decrease memory bandwidth. FP16 or other supported representations may provide substantial acceleration on GPU-based embedded computing platforms, but precision changes should be evaluated against the original policy. Comparison tests can measure action differences over representative observation sequences before hardware deployment. Performance optimization is useful only when the resulting policy preserves stable and sufficiently consistent locomotion behavior.

The RL policy should not be allowed unrestricted access to physical actuator commands. An action processing layer can enforce joint position, velocity, torque, and rate limits before commands reach the robot. Sudden changes may be filtered or rate-limited, while invalid numerical outputs such as NaN or infinity must immediately trigger fallback logic. These constraints create a safety boundary between learned behavior and the deterministic actuator interface.

Safety supervision operates independently from the learned policy. The supervisor monitors body orientation, joint limits, motor status, communication health, contact state, tracking error, temperature, and other critical variables. If predefined limits are violated, the system can reduce command magnitude, switch to a safer controller, stop gait progression, or initiate a controlled shutdown. The RL policy may contribute locomotion intelligence, but it should not be the sole mechanism responsible for detecting hazardous system states.

Policy confidence and execution validity can also be monitored indirectly. Large deviations from expected observation ranges may indicate that the robot has entered a state poorly represented during training. Out-of-distribution detection can use simple observation bounds, statistical measures, or additional learned estimators. When abnormal inputs persist, the locomotion supervisor can reduce speed or transition to a fallback mode rather than assuming that the policy will generalize reliably.

Simulation-to-real transfer is a major concern because the physical robot differs from its training simulator. Differences in mass, inertia, joint friction, motor response, communication delay, contact stiffness, sensor noise, and terrain friction can produce significant policy degradation. Domain randomization exposes the policy to variations in these parameters during training, while system identification improves the simulator model. Both approaches reduce dependence on a single idealized simulation configuration.

Latency and actuator dynamics should also be represented during training when possible. A policy trained with instantaneous actuation may exploit behavior that cannot be reproduced by physical motors and communication networks. Randomized observation delay, action delay, actuator strength, control gains, and motor dynamics can improve robustness. Deployment measurements of real timing characteristics should therefore feed back into simulation configuration and subsequent policy training.

RL inference can coexist with conventional model-based control rather than replacing the entire locomotion stack. A learned policy can generate residual joint commands, foothold corrections, gait parameters, or adaptive gains while a Whole-Body Control (WBC) or model-based controller preserves contact and dynamic constraints. This hybrid architecture combines learned adaptation with explicit physical structure and can simplify safety supervision compared with completely end-to-end actuator control.

Terrain-aware RL policies may consume perception information directly or receive compact terrain features from a separate perception pipeline. Raw depth observations provide rich environmental information but increase computational cost and training complexity. Height maps or encoded terrain features reduce input dimensionality and can separate perception from locomotion. The inference architecture must ensure that terrain information is temporally aligned with proprioceptive state and expressed in the coordinate frame expected by the policy.

Policy switching is required when multiple learned controllers support different behaviors such as standing, walking, running, recovery, or terrain-specific locomotion. Directly replacing one policy with another can generate discontinuous actions. A policy manager can select appropriate transition states, blend commands, interpolate latent variables, or temporarily use a conventional stabilizing controller. Transition logic should consider current contact configuration and body state rather than relying only on high-level mode commands.

Logging and replay are essential for validating real-time RL integration. Observations, normalized policy inputs, raw network outputs, processed actions, actuator commands, estimated state, contact events, and inference timing should be recorded with synchronized timestamps. A recorded sequence can later be replayed through the policy offline to determine whether unexpected behavior originated from sensing, preprocessing, inference, action processing, communication, or physical execution.

The complete real-time architecture therefore forms a closed loop from sensors to state estimation, observation construction, neural inference, action processing, low-level control, actuators, and physical robot dynamics. Safety supervision surrounds this pipeline while logging captures its internal states. By preserving training-time interfaces, bounding inference latency, validating model conversion, constraining actions, and maintaining deterministic fallback behavior, RL policies can be integrated as reliable components of advanced legged-robot locomotion software.

10.09 Quadruped Full SW Stack Integration Case

A quadruped full software stack integrates perception, state estimation, locomotion planning, balance control, Whole-Body Control (WBC), joint control, safety supervision, and hardware interfaces into one real-time system. Each module solves a different part of locomotion, but successful operation depends on consistent states, timing, coordinate frames, and interfaces across the entire pipeline. Integration therefore focuses as much on deterministic data flow and failure handling as on individual control algorithms.

The software architecture is commonly divided into hardware abstraction, real-time control, locomotion, estimation, perception, planning, and supervisory layers. Hardware abstraction provides standardized interfaces to motors, encoders, IMUs, force sensors, cameras, and communication buses. Higher layers should operate on well-defined physical quantities rather than device-specific protocols, allowing sensors or actuators to change without redesigning the complete locomotion software.

At the lowest level, actuator interfaces exchange commands and feedback with each joint drive. Joint position, velocity, torque, motor temperature, fault state, and communication status are collected at high frequency, while torque, position, velocity, or impedance references are transmitted to the actuators. This loop must remain deterministic because delays or dropped commands directly affect stability. Hardware timestamps and synchronized clocks help preserve temporal consistency across distributed devices.

State estimation converts raw measurements into a coherent representation of the robot. Joint encoders describe leg configuration, while the IMU measures angular velocity and linear acceleration. Kinematic constraints and detected foot contacts provide information about body motion relative to the ground. The estimator produces base position, orientation, linear and angular velocity, joint state, and contact-related quantities required by locomotion and balance controllers.

Contact estimation is tightly coupled with state estimation because quadruped locomotion continuously switches supporting legs. Force sensors, joint torque information, motor currents, foot velocity, and kinematic residuals can be fused to determine whether each foot is in stance, swing, touchdown, or uncertain contact. Reliable contact states improve base estimation and prevent controllers from applying inappropriate force or motion objectives to a leg whose physical contact differs from the planned gait.

The gait layer determines the temporal organization of the four legs. A gait scheduler defines stance and swing intervals, phase variables, duty factors, and phase offsets for walking, trotting, pacing, bounding, or transitional behavior. High-level velocity commands are converted into nominal body motion and footstep timing. The scheduler also coordinates gait transitions so that contact sequences change without creating sudden loss of support or conflicting commands.

Foot placement planning determines where each swing leg should establish its next contact. Nominal footholds are derived from desired velocity, body state, gait phase, and robot geometry, while balance feedback can shift them to compensate for disturbances. On irregular terrain, perception supplies height, slope, roughness, and obstacle information so that the planner can search for reachable and stable footholds instead of blindly executing nominal steps.

Once a foothold is selected, the swing-leg trajectory generator constructs a continuous path from lift-off to touchdown. The trajectory must provide sufficient terrain clearance while respecting joint workspace, velocity, acceleration, and collision constraints. Online replanning can modify the target during swing when terrain information changes or balance recovery requires a different step. Near touchdown, the motion transitions toward controlled landing behavior and contact detection.

Balance control computes body and contact objectives that preserve dynamic stability while tracking commanded motion. Depending on the architecture, this layer can use centroidal dynamics, Model Predictive Control (MPC), ZMP, Capture Point, DCM, or related methods. For quadrupeds, the controller must exploit changing combinations of supporting feet and determine how the available contacts can generate forces and moments that regulate CoM motion and body orientation.

Model Predictive Control is particularly useful for quadruped locomotion because future contact schedules can be incorporated into force planning. Given predicted body motion and known stance intervals, MPC can optimize ground reaction forces over a finite horizon while respecting friction and contact constraints. The first part of the optimized solution is applied before the problem is solved again with updated state information, producing a receding-horizon feedback process.

Whole-Body Control translates locomotion objectives into commands consistent with the complete robot dynamics. Typical tasks include body position and orientation regulation, swing-foot tracking, stance-contact maintenance, force distribution, and posture control. A Quadratic Programming (QP) formulation can combine these objectives with equations of motion, friction constraints, torque limits, and contact conditions. Task priorities or weights determine how conflicting objectives are resolved.

The output of WBC is converted into actuator-level references by joint controllers. Torque-controlled robots may receive feedforward torque combined with feedback terms, while impedance-controlled systems use desired joint positions, velocities, stiffness, and damping. High-rate local control compensates for tracking errors between slower locomotion updates. The separation between planning, dynamic control, and actuator regulation enables each layer to operate at an appropriate computational frequency.

Perception runs alongside the control pipeline rather than directly inside every high-rate loop. Depth cameras, stereo systems, or LiDAR can generate point clouds, elevation maps, obstacle representations, and traversability information. Because perception often has higher and less deterministic latency than proprioceptive sensing, terrain products are timestamped and transformed into consistent coordinate frames before locomotion modules use them. Stale terrain information must be detected rather than silently accepted.

A high-level locomotion manager connects operator or autonomous navigation commands to the control stack. Requested forward velocity, lateral velocity, yaw rate, body height, gait mode, and behavior state are validated before reaching lower layers. The manager coordinates standing, walking, terrain traversal, recovery, stopping, and other modes through explicit state transitions. This prevents independent modules from making incompatible decisions during behavioral changes.

Learning-based modules can be integrated without replacing the complete model-based stack. An RL policy may generate joint references, residual corrections, foothold adjustments, gait parameters, or recovery behavior. Its observations are constructed from synchronized robot states and optionally terrain features, and its outputs pass through scaling and safety limits. Hybrid integration allows learned adaptation to operate together with deterministic contact, dynamics, and actuator constraints.

Safety supervision surrounds the complete locomotion pipeline. It monitors body attitude, joint limits, actuator saturation, communication health, battery condition, temperature, contact consistency, estimator validity, and controller execution timing. Fault severity determines whether the system limits velocity, changes gait, enters a stable stance, switches to a recovery controller, disables actuator commands, or performs an emergency stop. Safety logic should remain operational even if a higher-level module fails.

Real-time scheduling is a system-level design problem because different modules require different update rates. Motor and joint loops may execute at the highest frequency, followed by state estimation and WBC, while MPC, gait planning, perception, and high-level planning operate more slowly. Data exchange should use timestamps, bounded queues, shared-memory structures, or other deterministic mechanisms. Blocking operations, dynamic memory allocation, and synchronous logging should be avoided in critical control threads.

Software integration also requires explicit management of coordinate frames and units. Sensor measurements may originate in IMU, body, joint, foot, odometry, map, or world frames, while controllers require precisely defined transformations among them. Position, velocity, force, torque, angle, and time units must remain consistent across module boundaries. Interface definitions should specify reference frames, sign conventions, update rates, validity flags, and timestamps rather than relying on undocumented assumptions.

Logging, visualization, and replay provide observability into the complete stack. State estimates, contact states, gait phases, footholds, swing trajectories, desired and measured joint states, ground reaction forces, controller outputs, safety events, and computation times should be recorded with synchronized timestamps. Offline replay allows engineers to reproduce failures and determine whether a problem originated in perception, estimation, planning, control, communication, or hardware execution.

Integration testing should progress from individual modules to the complete physical system. Sensor and actuator interfaces can first be verified independently, followed by estimator tests, standing control, single-leg swing tests, controlled stepping, treadmill or flat-ground walking, and progressively more difficult terrain. Simulation and Hardware-in-the-Loop (HIL) testing can expose timing, interface, and failure-handling problems before risky experiments are performed on the physical robot.

The resulting quadruped architecture forms a closed-loop hierarchy from environment and sensors through perception and state estimation to gait planning, foot placement, balance control, WBC, joint control, and actuators. Contact events and physical motion continuously return feedback to the estimator and planners, while safety supervision monitors every critical stage. Robust quadruped locomotion emerges from this coordinated software stack rather than from any single controller, planner, or learned policy.

10.10 Humanoid Biped Walking Control SW Case

Humanoid biped walking control integrates state estimation, gait planning, dynamic balance, footstep generation, swing-leg control, Whole-Body Control (WBC), joint regulation, and safety supervision into a coordinated real-time software system. Unlike quadrupeds, a biped frequently relies on a single supporting foot and must transfer support between only two contacts. Small errors in foot placement, momentum, or contact timing can therefore strongly influence whole-body stability.

The control architecture begins with synchronized sensing and state estimation. Joint encoders provide limb configuration, while an Inertial Measurement Unit (IMU) measures body angular velocity and linear acceleration. Foot force or force-torque sensors provide contact and Center of Pressure (CoP) information. These measurements are fused with robot kinematics to estimate pelvis pose, Center of Mass (CoM) position and velocity, joint states, support phase, and quantities required by the balance controller.

Walking is organized as an alternating sequence of single-support and double-support phases. During single support, one foot maintains contact while the opposite leg swings toward its next foothold. During double support, both feet contact the ground and the load is transferred from the outgoing foot to the incoming foot. The gait scheduler defines these intervals and provides phase timing to the footstep planner, balance controller, swing controller, and contact-state logic.

A high-level walking command typically specifies desired forward and lateral velocity, turning rate, body orientation, or destination. The footstep planner converts this request into a sequence of left and right footholds with associated timing. Nominal step length and width are constrained by the humanoid\'s kinematic workspace, while feedback corrections modify foot placement according to the current balance state. The planner must preserve both locomotion intent and future recoverability.

Zero Moment Point (ZMP) control provides a classical framework for generating dynamically balanced walking motion. Desired footsteps and support phases define a feasible ZMP trajectory inside the available support region. A simplified model such as the Linear Inverted Pendulum Model (LIPM) can then generate a corresponding CoM trajectory. During execution, measured or estimated states are compared with the planned motion and corrective control is applied to keep the dynamic response consistent with the support geometry.

Capture Point (CP) and Divergent Component of Motion (DCM) methods provide an additional representation of the unstable component of biped dynamics. CoM position and velocity are combined into a state indicating where support should be established to control divergent motion. DCM tracking can adjust the desired ZMP within the current support region, while larger disturbances can modify the next footstep position or timing. This creates a direct connection between balance recovery and foot placement.

Model Predictive Control (MPC) can extend this approach by optimizing future CoM, ZMP, DCM, contact forces, or footsteps over a finite horizon. The controller uses the planned sequence of left-foot and right-foot contacts while considering stability, motion tracking, and physical constraints. Only the first portion of the optimized solution is applied before the problem is solved again using updated state information, allowing the walking plan to adapt continuously to disturbances and modeling errors.

The swing-leg controller moves the non-supporting foot from lift-off to the planned touchdown location. The trajectory should maintain terrain clearance, remain within joint limits, avoid self-collision, and arrive with an appropriate position, orientation, and velocity. Smooth polynomial, spline, or optimized trajectories can be used. If the balance controller modifies the target foothold during swing, online replanning reshapes the remaining trajectory while maintaining continuity.

Foot orientation is particularly important for humanoid walking because stable support normally requires a sufficiently large sole contact area. On flat terrain, the sole is generally prepared approximately parallel to the support surface before touchdown. On slopes or stairs, terrain information can modify the target orientation. Excessive orientation error at initial contact can produce edge loading, impact, or unexpected moments that disturb the upper body before stable support is established.

Contact detection determines the actual transition from swing to stance. Foot force sensors, joint torque information, kinematic residuals, or fused estimators can identify touchdown even when the physical terrain height differs from the planned value. Early contact requires the swing controller to terminate or modify free-space tracking, while late contact may require a controlled downward search. Stable contact should be confirmed before the new foot is treated as a fully reliable support.

Double-support control manages the transfer of load between the two feet. The outgoing support force is gradually reduced while the incoming foot assumes a larger portion of the robot weight and required control wrench. Abrupt force transfer can create body acceleration or foot slip, so desired contact forces should change smoothly. Contact-force optimization can also maintain CoP and friction constraints while preparing the outgoing foot for lift-off.

Whole-Body Control coordinates walking objectives with the full humanoid dynamics. Desired CoM motion, pelvis orientation, torso posture, swing-foot trajectory, stance-foot constraints, and contact forces can be combined in a hierarchical controller or Quadratic Programming (QP) formulation. Equations of motion, friction limits, joint limits, torque limits, and contact constraints restrict the feasible solution. Task priorities determine which objectives are preserved when all requests cannot be satisfied simultaneously.

Upper-body control contributes directly to walking performance rather than serving only a visual or manipulation role. Torso orientation and arm motion influence angular momentum and the overall mass distribution. Arm swing may counterbalance leg motion, while deliberate torso or arm adjustments can contribute to disturbance recovery. When the humanoid simultaneously performs manipulation, WBC must coordinate hand tasks with locomotion without sacrificing essential support and balance constraints.

The output of WBC is translated into actuator commands through high-rate joint control. Torque-controlled humanoids may combine model-based feedforward torque with position and velocity feedback, whereas impedance-controlled systems use desired joint position, velocity, stiffness, and damping. The local control loop must track references with sufficiently low latency while rejecting mechanical disturbances. Saturation limits prevent higher-level commands from requesting physically unavailable actuator effort.

Disturbance recovery uses multiple strategies according to disturbance magnitude. Small errors can be handled through ankle torque and ZMP adjustment without changing the planned step. Larger disturbances may require hip or upper-body momentum regulation, while still larger errors can trigger CP/DCM-based footstep modification. Severe disturbances may require an additional recovery step or transition toward a protective state. The supervisor selects responses according to available support and actuator capability.

Terrain-adaptive walking adds perception to the footstep and swing-planning process. Depth cameras, stereo vision, or LiDAR can generate elevation maps and identify stairs, slopes, obstacles, gaps, and candidate support surfaces. The footstep planner evaluates reachable terrain regions and adjusts both foot position and orientation. Because perception can contain uncertainty and latency, physical contact sensing remains necessary to verify the actual surface during touchdown.

Real-time scheduling must accommodate control loops with different computational requirements. Joint regulation and state estimation generally operate at high rates, WBC and balance control at intermediate or high rates, and MPC, footstep planning, and perception at lower rates. Timestamped messages and deterministic data exchange maintain temporal consistency. Critical control threads should avoid blocking operations, unbounded queues, and synchronous logging that can introduce unpredictable execution delays.

Safety supervision continuously monitors body inclination, CoM behavior, foot contact, joint limits, actuator status, communication, estimator validity, controller timing, and thermal or electrical conditions. When the walking state approaches predefined limits, the supervisor can reduce commanded velocity, shorten steps, increase double-support duration, stop walking, enter a stable posture, or activate recovery behavior. Emergency logic must remain independent of optional planning or learning modules.

Logging and replay provide the information required to diagnose integrated walking behavior. Important signals include estimated CoM and DCM, desired and measured ZMP or CoP, support phase, footsteps, swing trajectories, contact forces, joint states, actuator commands, controller outputs, safety events, and execution timing. Synchronized logs allow engineers to distinguish planning errors from state-estimation errors, contact failures, control limitations, or hardware problems.

Validation progresses from static balance to increasingly dynamic behavior. Standing and weight-shift tests verify state estimation and force distribution before stepping is introduced. Controlled single steps, repeated walking on flat ground, turning, disturbance recovery, slopes, and stairs can then be evaluated progressively. Simulation and Hardware-in-the-Loop (HIL) testing allow timing, interface, and failure scenarios to be examined before equivalent experiments are attempted on the physical humanoid.

The complete humanoid walking software stack forms a closed loop from sensing and state estimation through gait scheduling, footstep planning, ZMP/CP/DCM or MPC-based balance control, swing-leg generation, WBC, joint control, and physical contact. Touchdown and body-motion feedback continuously update the next control decision. Reliable biped walking emerges from synchronized management of contact, balance, timing, whole-body dynamics, and safety across this integrated architecture.

---

Author: Richard Minsoo Go

LinkedIn: Richard Minsoo Go