Mode-Reactive Template-Based Control in Planar Legged Robots

Translating the center of mass (CoM) while fixing the orientation of a rigid body supported by relatively massless, actuated limbs is a common problem setting in legged robotics. This paper proposes a hierarchical approach to such maneuvers that decouples CoM task planning from body orientation control in sagittal-plane models, thereby exposing a well-studied and computationally effective low dimensional dynamical system that can be used for CoM task planning. The resulting algorithms directly address the control authority (degree of underactuation) available at a given contact mode, enabling a-priori plans with these intuitive, robust pendular dynamics to be formally embedded at the (virtual) CoM of planar floating-torso models, while focusing high gain posture stabilizing feedback upon the body orientation. A series of numerical and empirical examples address single- and multi-legged leaping — transitional maneuvers where only a single brief stance mode is available to load energy into the CoM and guide its direction. We compare our hierarchical method to a model-based model-predictive controller in one of the tasks, demonstrating similar performance with a significantly smaller computational footprint.


I. INTRODUCTION
Programming a robot's work with provably correct and physically grounded architecture requires an analytically tractable method of relating abstract behavioral specifications for independent low degree of freedom (DoF) body components to a high degree of freedom, coupled hybrid dynamical system whose control authority (degree of underactuation) varies with the contact mode [1]. For legged robots, achieving such command has been particularly challenging due to the complexity of realizing such abstract task specifications (e.g., ''leap on to the box'') in the face of numerous constraints arising from body design (limited actuator force and power) and variation of environmental affordance (limited traction at available footholds). This paper presents theory and experiments that achieve tunable, reliable, highly energetic maneuvers in the sagittal plane for legged robots whose ground The associate editor coordinating the review of this manuscript and approving it for publication was Luigi Biagiotti . contact modes may severely constrain their actuators' control authority.
Specifically, neglecting leg mass, we decompose the body's three degree of freedom sagittal plane dynamics into a virtual mass center and literal orientation in such a manner that a steady posture can be asserted with as high control authority as the contact mode and actuator endowment affords. The decomposition formally guarantees that whatever actuator affordance remains, if any, can be applied with no cross talk to very well understood models of virtual pendular dynamics such that the body's translational motion can be planned and executed with considerable precision, even in the face of substantial parametric uncertainty. This hierarchical approach to planning transitional maneuvers, with formal sequential composition [2] of appropriate submodules, empirically yields robust and repeatable dynamical ascents and descents on a quadrupedal robot tasked with leaping between otherwise unreachable handholds and footholds.  [3] robot executing a ''monkey bars'' task (cf. Sec. V-A) using the controller in this paper, with a superimposed image showing the model used to generate the stance phase reference dynamics.

A. RELATED WORK
We restrict our focus to a class of planar floating-torso [4]- [6] models with massless, kinematic-singularity-free legs, and sticking toe contacts (detailed definition in Assumption 1). These modeling assumptions place our analysis within the formal hybrid systems framework of [7]. We also restrict attention to ''pitch steady'' locomotion (prioritized control of body orientation) joining an extensive literature [8]- [10] as formalized in Def. 1.

1) WHOLE BODY CONTROL (WBC)
Given sufficient control affordance (enough toe contacts with suitable traction and an adequate number of actuators in poses far from kinematic singularity) whole-body control (WBC)-specified wrenches applied to the robot's full rigid body dynamics-can be achieved via feedback linearization (FL) [11], [12]. More recent literature has approached WBC through recourse to quadratic programming (QP)-minimizing, pointwise-in-time, the actuators' torque-affine deviation from the specified wrenches-to avoid strict inversion of the dynamics required by FL [5], [13], [14]. Although somewhat more computationally expensive, QP naturally incorporates traction and torque constraints while sidestepping numerical issues in non-invertible (underactuated) configurations. Of course, poorly structured or outright infeasible (e.g., underactuated, or slippery) configurations incur large tracking errors consequent upon the mismatch between desired and produced wrenches.
One method of mitigating (but not solving) these problems in small multilegged robots with light limbs is to rapidly change the contact mode by taking a transient step, or simply relying on high frequency gaits [15], [16]. Another approach to mitigating the underactuation issue in WBC is operational space control or null-space control [5], [13], [14], [17], where task requirements are prioritized and addressed according to the affordance at the current configuration.
In general, depending upon the degree of underactuation, WBC methods cannot guarantee task completion when coordinated control of the body is required. Moreover, even in modes and configurations with adequate control affordance, these direct or optimally approximated wrench control methods rely intimately on accurate robot and environment models and may not be sufficiently robust for long term operation in less than perfectly characterized environments.

2) OPTIMAL CONTROL
If the system is not fully actuated, feedback design is possible for controllable systems and has been pursued in the legged locomotion literature via optimal control methods such as LQR or SOS [18]. The imposition of an objective function on the state space relieves the burden of specifying WBC target wrenches. Its integration over a specified time horizon can potentially mitigate the limitations of direct WBC by leveraging the improved affordance of favorable contact modes to relax the passage through impoverished configurations while making optimal progress.
Model predictive control (MPC) is an increasingly popular approach to optimal control that can offer both computational efficacy and formal insight by rewarding reference motion sequences over a specified finite time horizon [19]. MPC has been successfully applied to direct WBC by transforming the control input space from joint torques to toe forces [20], and it has been used to generate reference wrench sequences for subsequent WBC by application to a simplified target dynamical control system [14], [21], [22].
Optimal control methods rely on accurate models due to their iterated appearance in the horizon-long integrated objective function. Hence, poorly modeled environments or imperfectly represented robot properties-inevitable sensorimotor infidelities and, particularly, sensitive model mismatch incurred by difficult to measure parameters [23]-can cause failure [24]. Moreover, all are computationally significantly more expensive (even MPC with linearized, approximated dynamics [20]) in comparison to pointwise-in-time WBC methods.

3) FROM REFERENCE TRAJECTORIES TO ANCHORED TEMPLATES
In general, optimal control methods are applied to regulate whole body motion around state space reference trajectories. These reference signals themselves often arise as optimized interpolants constrained to respect some simplified approximant of the complete hybrid dynamics [25], or they may be generated by numerically integrating forward from the current state some similarly simplified reference dynamical system [26], [27]. However, in all these approaches, by dint VOLUME 10, 2022 of being fixed ahead of time, the models used cannot adapt to unforeseen contact events.
This paper takes its place in a long tradition of dynamical model reduction associated with biological observations of low degree of freedom ''template'' dynamics emerging from complicated high degree of animal bodies that ''anchor'' the simpler behavior [28]. Their empirical validity and utility in control of robotic systems has been observed in numerous instances [29]- [31]. A complete formal account of hierarchical composition for classical dynamical systems can be found in [32] while its formal extension to the hybrid setting remains a work in progress [33]. In this paper, we use the terms ''template'' (reduced-order model that generates the reference dynamics) and ''anchoring'' (its embedding via WBC), as they are defined in Appendix VI. We use the term ''template planning'' to refer to the selection of reference dynamics whose controlled ''anchoring'' wrenches in the whole body dynamics will be integrated by the robot's physics to directly expose the desired motions in real time.
From the perspective of robot locomotion, our work follows a large body of prior walking control literature utilizing LIP (linear inverted pendulum) and ZMP (zero moment point)-based methods [9], [34], in that we prioritize the stabilization of body orientation over the regulation of CoM translation. Specifically, this paper replaces by algorithmic prescription of templates prior work in which the template (e.g. linear inverted pendulum [35], SLIP [36]) has been selected by intuition and domain expertise. Thus, our controller applies the appropriate anchored template composition in a mode reactive manner in real-time.

B. OUR APPROACH: MODE-REACTIVE TEMPLATES
This paper contributes new theory for sagittal plane control of legged machines along with empirical demonstrations of its efficacy for planning and executing highly energetic maneuvers requiring multiple hybrid transitions through variously underactuated modes. We introduce a family of pitch-steady anchoring controllers for algorithmically selected pendular templates that govern CoM translational dynamics with as much remaining control authority as contact mechanics and actuator endowment allow. We provide formal convergence guarantees for the both the pitch and the CoM subsystems assuming perfect traction. We illustrate the utility of these simply tunable pendular templates for planning transitional maneuvers by application to two different leap sequences requiring careful attention to foothold and handhold placements along the way. We demonstrate the resulting closed loop hybrid systems by implementing them on the Minitaur quadruped, [3], [15], depicted in Fig. 1, first in their idealized ''pinned toe'' form and second, a ''traction-aware'' version, using a pointwise-in-time QP to relax that naive anchoring into a WBC which is feasible relative to the available model of substrate coulomb friction. Both these empirically demonstrated versions are computationally-efficient enough to run in real time at 1KHz control rates on the embedded microcontroller on the robot. The anchorability of the embedded open-loop template controller in this paper (the accuracy of which is sensitive to modeling errors), as well as success in other applications as discussed in Sec. VI, are evidence toward the robustness of our proposed anchoring scheme.
We also implement (in simulation) an MPC-based WBC for one of the tasks as a representative state-of-the-art alternative, for comparison and to illustrate the benefits of our modular hierarchical controller composition. Our numerical experience is that such a conventional approach will often fail in that highly energetic underacatuated setting if the reference trajectory is chosen naively. Hence, we accord this MPC-based anchoring the benefit of our algorithmically chosen mode-reactive template as its infinitesimal reference trajectory generator. In other words, we present the ''best case'' alternative comparison performance by explicitly accounting for underactuation issues in a manner that has not been reported in the prior WBC approaches described above. As we report in our results (Sec. IV-B2.b), the state-of-the-art MPC-based WBC alternative yields performance similar to our proposed hierarchical anchoring but incurs a significantly more burdensome computational footprint.
In sum, this paper presents for the first time a direct correspondence between arbitrary configurations of a class of sagittal plane locomotion models, and dynamical template models that can accurately capture the available affordance as well as be utilized for computationally tractable template planning. The formal correspondence ensures that task execution plans created with these simplified models can be effectively anchored into the floating torso, and the simplicity of the models ensures that computationally-constrained legged robots can execute dynamically challenging behaviors as we demonstrate experimentally. Moreover, since these template models resemble well-studied dynamical systems like point particles and inverted pendula, template controllers leveraging momentum-or energy-based methods can be utilized directly (as we show in our empirical demonstrations). Lastly, The combinatorial explosion in the number of dynamics modes that need to be considered has motivated ''contactimplicit'' trajectory optimization techniques [26], and we hope that the computational tractability of the presented approach can pave the way for an online reactive analogue.
In Sec. II, we introduce a general class of floating-torso locomotion models for consideration, and proceed to then subdivide this class according to the type of anchoring the differing control affordances allow (Fig. 2, Prop. 2). We discuss the resulting behavior under these types of anchoring in Sec. III and summarize our results in Fig. 2C. In Sec. IV, we demonstrate template-based control of dynamic leaping behaviors on a simulated monoped with offset torso, and the physical Minitaur robot.

II. MODELING A CLASS OF PLANAR MECHANISMS
We introduce a general class of planar models (depicted in Fig. 2B) to which our analytical results apply. The joint configuration θ = (θ 1 , . . . , θ k ) includes all the limbs, where θ j is the configuration of limb j. While normally we would expect a subset of legs to be in contact, the focus of this paper is on the stance dynamics of a single contact mode, and so without making any assumptions about k ∈ N (i.e., it could one or greater), we assume all k contacts are active.
We use the notation D x f (x) to indicate the Jacobian (matrix of partial derivatives of the function f evaluated at state x), and sometimes omit the first subscript if it is the only argument for the function. To disambiguate, where possible, we use bold lowercase symbols to denote vectors, lowercase symbols to denote scalars, and uppercase symbols to denote matrices.
Assumption 1 Floating Torso Model: The model has 1) a single massive rigid body, and all other links are massless; 2) no kinematic singularity (Dg j are full rank); 3) all contacts are sticking contacts; 4) the body orientation φ ∈ S 1 is a cyclic variable in the Lagrangian, i.e. ∂L ∂φ = 0, though it has kinetic energy, ∂L ∂φ = 0. The physical interpretation is that the body has its mass distributed uniformly, so that there is no net moment due to gravity about the CoM. Based on Assumption 12, since each Dg j (θ j ) is full rank, each θ j ∈ j has dimension at least 2, and could be composed of revolute or prismatic joints. All of these conditions hold for (among others) the models depicted in Fig. 2B. For the physical robots shown, the massless leg assumption (Def. 11) relies on published evidence [37]- [39] and our conjecture of its effectiveness; the sticking contact assumption (Def. 13) relies on the specific operating conditions.
The full configuration space includes joints and body coordinates as in [40], such that the configuration space is the product of, respectively, joint and body configurations, ( j j ) × SE (2). Let n denote the total number of joints (adding up sizes for each θ j ), and let m denote the total number of actuated joints.

A. BACKGROUND: FLOATING TORSO KINEMATICS AND DYNAMICS
Let R : S 1 → R 2×2 be a function that maps an angle on to a rotation matrix. Each toe creates a contact constraint, written in the (inertial) world frame as where a j remains constant during stance (Assumption 13), and we denote by a(q) a stacked version with all the contacts j ∈ {1, . . . , k} in the contact set. The contact Jacobian iṡ where we define the matrices . . . . . . For a single massive rigid body with massless legs (Assumption 11), the unconstrained dynamics can be derived using a simple Lagrangian, where potential terms γ include gravity and compliance.
In the planar setting (unlike the spatial setting), the R (n+3)×(n+3) unconstrained inertia tensor is constant and so there is no Coriolis matrix. The dynamics can be derived using a constrained Lagrangian as in [40], where the upper n rows of the inertia tensor corresponding to the massless legs are zero, and the lower diagonal 3 × 3 block (corresponding to the ''body'' DoFs x) is The dimensions of various matrices are The columns of A x correspond to the SE(2) body configuration. In (5), B represents the mapping of actuator torques to the generalized coordinates, stacking the contribution from each limb. We observe in (5) that Assumption 11 allows us to impose a sort of ''decoupling'' of the reaction forces λ from the dynamical effects (terms dependent onq,q). VOLUME 10, 2022

B. CoM AND PITCH DYNAMICS
Based on Assumption 12, A θ (3) is full rank, and so we can solve 1 for λ from the top row of (5). From (3), we can find a left pseudo-inverse of A T θ , where Dg † j is a standard pseudo-inverse; applying this to (5), In case Dg j is square (the limb kinematics are not redundant), (9) simply contain the diagonal blocks of A †T θ horizontally stacked. The last row of (9) picks up the last row of A T x from (3), which can be simplified further by cancellations of R and Dg: define the pitch affordance vector We emphasize that when the contacts are active (and the contact locations a j are fixed), c j (the vector connecting the toe location to the CoM) does not vary with φ, but rather only with p, a fact we shall exploit in Prop. 1. Using (3) and (1), we see that the j th block column of the lower row of A T x is Using this in (10) together with (9), we get whereĪ := I · · · I (horizontally stacked for each leg).

III. INPUT-DECOUPLED ANCHORING: CoM TEMPLATES
In currently-practiced template-based control, the target reference dynamics are typically corrupted by high-gain anchoring forces during transient operation down to the attracting submanifold. 2 These perturbations make it particularly difficult to directly deploy template controllers, especially in the context of the energetic non-steady transitional maneuvers targeted in this paper. To address this issue, we propose a new type of anchoring, where we explicitly develop (Prop. 1) a template coordinate change for isolating the template (taskrelated) dynamics (both on T as well as along the anchoring transients down to it) from perturbations due to the (potentially high-gain) control effort required to anchor it:

Definition 1 (Input-Decoupled Anchoring): Controlled anchoring where the anchoring forces [44, Appendix A] do not appear in the reduced dynamics.
In addition to putting forward a procedure for inputdecoupled anchoring, we provide a closed-form expression for the reduced (restriction or zero) dynamics on T that has not been possible before in the literature other than in isolated cases [45]- [47] (in the first two examples of which, attraction down to the template submanifold was also not guaranteed). 3 These advances in concert make it possible to attach some guarantees of success to template-based controllers anchored on floating-torso bodies. To underscore the value of exposing an uncorrupted template model to the higher level task in this manner, we empirically demonstrate the input-decoupled anchoring of non-asymptotically stable templates in transitional leaping tasks (such as in Fig. 1). These targeted Hamiltonian systems are particularly sensitive to perturbations generated by the anchoring process.
We exclusively examine (the ubiquitous set of) tasks prioritizing orientation stabilization: Definition 1 Pitch-steady behavior: In the behavior, the body pitch stably tracks φ to φ * (desired body orientation). Specifically, the closed loop dynamics admit the orientation error function 4 → 0 as a LaSalle function. The anchoring posture that embeds the template is the submanifold of the state space, We first define the ''virtual leg'' coordinate projection from the coordinates of the physical system (p, physical CoM, and φ, orientation) into r ∈ R 2 as where h(p, φ), a correction term, is defined abstractly in (37) and constructed by successive approximants given in (41). This correction reduces to the literal mass-center projection (p, φ) → p on the pitch-steady submanifold, T (Def. 1). In the text below, we use the term ''virtual CoM'' in world-frame to refer to r + 1 k j a j (q) = h(p, φ). This expression exhibits the template coordinates as describing the configuration of the classical notion of a ''virtual leg'' joining the torso's mass center to the centroid of the toe contact locations [29], while corrupted by correction terms that disappear along with the orientation error. Before we construct h (in the proof of Prop. 1 and Appendix VI), we need some additional definitions and computations: Note that the conventional usage of the · † notation would dictate that x † be the pseudoinverse of x, but in this case we define c † φ as stated here to avoid repeating the cumbersome x T † notation.
are decoupled from u φ . The specific form of E T (which affects the virtual CoM afforfance) depends on the system, and is explored in Prop. 2. The remainder term in (13) asymptotically approaches zero with orientation error, and is detailed further in Appendix VI.
Proof: We propose a control strategy that recruits a single dimension of τ as the anchoring (pitch-steadying) torque for orientation control, leaving the remaining inputs In (14), we used the condition e T φ E T = 0, and we remind the readers that ζ in (15) is the orientation error as defined in Definition 1.
Note that as long as c φ (p) = 0, this is well-defined. The (rightmost) cancellation term in (15) is only required for cases where there is joint compliance (G θ = 0) in order to drive the orientation error to 0 (as we will see in the next subsection). When omitted, the Lyapunov argument in (18) shows that the orientation error will be driven down to a ball outside which the quadratic error K dφ 2 dominates the noise due to the G θ term. We leave u T as a free input for now and define it where we use it for template control in Prop. 2.
We utilize a fast (high gain) pitch control strategy of the form (15) for anchoring the pitch-steady behavior. Note that the last row of (11) is where we observe that there are no gravity-like terms in this row due to Assumption 14. Substituting (14) in the above, where we can see that the orientation dynamics have been decoupled from u T . Using (15), the closed loop dynamics take the form VOLUME 10, 2022 Using a quadratic Lyapunov function This shows that the T submanifold of the state space (Def. 1) is not only attracting, but also invariant (since the closed-loop orientation dynamics (16) are decoupled from other dynamics). This satisfies property 2a. Lemma 1 (stated and proved in Appendix VI) reveals that the first few terms of r (12) are where due to our definition of ζ (Def. 1), O(∇ζ 2 ) = O(ζ ), and the remainder terms are observed to disappear with the orientation error ζ (φ), satisfying property 2b. Lemma 1 also shows that we get the template restriction dynamics (13), and that they are appropriately decoupled from u φ , as claimed in property 2c. For purposes of comparison, we also briefly describe a naive non-input-decoupled anchoring procedure, and also its relation to input-decoupled anchoring, in Appendix VI.

Remark 1 (Input-Decoupled-Anchorable System Examples):
To convey an intuitive idea of the conditions for Prop. 1, in Fig. 2, we depict a number of familiar sagittal plane abstractions and existing physical robots to exemplify the variety of systems covered by Prop. 1. For concreteness and without loss of generality (since any kinematic-singularityfree design can be mapped via an appropriate kinematics coordinate change), in this subsection we assume a revoluteprismatic (RP) kinematics for the leg, where the notation θ ji denotes joint angle i of leg j. We solve for The j th block row of the above simplifies to Dg T j J (d j + g j ) using (1). Using (20), letting d j = (d xj , d zj ) and c j2 , s j2 be the cosine and sine of the relative leg angle θ j2 , .
Consider the special case in (21) where d j = 0, so that satisfying the condition of Prop. 1. So for a single leg attached at the CoM (d j = 0), actuation of only the θ j2 joint (hip angle) is required for decoupled orientation control. This design corresponds to a version of Raibert's planar hopper [29] with an unactuated compliant leg shank (middle of Fig. 2). Raibert's simple three-part controller [29] made this choice as well. However, when d j = 0, a decoupled orientation controller needs contributions from the shank extension actuator as well. Our construction (14) can be used to generalize the orientation controller to this case, where Raibert's decoupled control strategy cannot be directly applied [48]. Proposition 2 Template Behavior: For systems satisfying Prop. 1, the template control signal contribution in (13) can be further decomposed using columns of E T , e c ∈ R 2k , and the columns of E f are orthogonal to both e φ , e c . The reduced dynamics (13) admits a re-expression of the leading right hand side term as where u f ∈ R m−2 is a ''free'' virtual input that can be utilized for template control. In (7)  Proof: First, note that from (10), e T φ e c = 0, i.e. e c satisfies the condition to be in the column span of e ⊥ φ , as required by the condition in Prop. 2. Next, the first term on the right hand side of (13) is agreeing with (23). If E f = 0, since we are assured that E f is orthogonal to e c , some component of the template control signal acts along a direction tangential to r, thus affording full 2DoF control of the virtual CoM.
Remark 2 (Definition of UA, CFA, FA Templates): We depict these template models in Fig. 2 along the top row. Each template has two DoFs r ∈ R 2 (translation of the virtual mass center relative to the virtual toe), but they differ in the actuator affordance, and, hence, the span of accelerations that can be imparted to the virtual CoM. The unactuated (UA) model has no available actuators and cannot be controlled, the central-force-actuated (CFA) model can be accelerated along the direction of the (virtual) leg like the spring loaded inverted pendulum (SLIP) model [49], and the fully-actuated (FA) model can be completely controlled in its (physical as well as virtual) sagittal plane. we present a nearly ubiquitious example where due to joint compliance G θ , the reduced dynamics are closely approximate to those of an inverted pendulum (IP) subject to gravitational and compliance forces ( Fig. 2A). When one actuator is added and we can use Prop. 22, we have shown that the remaining input acts as a central force. Long years of experience with approximations (e.g. [50], [51], etc.) suggest that even when cancellation of the gravity-like terms is not possible or ill-advised (due to model or sensor noise, etc.) the central force model can be leveraged to gain productive insight into the dynamical behavior, as leveraged in the template controllers devised in Sec. IV-A, IV-B, and V-A.
When additional actuation is available and we can use Prop. 23, we have shown that the virtual CoM can be controlled as a fully-actuated point mass, agreeing with the feedback linearizability of these models.

Remark 4 (Anchoring Preserves Natural Dynamics: Raibert Hopper Example):
The remaining terms on the right side of the CoM template dynamics (23) preserve any natural compliance from the full dynamics (5) on the invariant pitchsteady manifold. We demonstrate this next using the Raibert hopper with parallel shank compliance (center column of Fig. 2), which satisfies Prop. 21. Since we are restricting our attention to ζ = 0, with the toe location a(q) = 0 (without loss of generality), in (19) we have r = p. We assume the leg has the RP kinematics of (20), and gravitational and compliance terms in the potential energy of the form where ρ 0 is the nominal leg extension. For the radial joint compliance G θ , we use the constraint equation (1) to get Using (8) and substituting in (24), we can note that θ j1 = p and simplify the expression to Since there is only one leg, theĪ in (23) is just the identity matrix. Additionally, using (24) in (5), we get Putting these together in (23), we get the reduced dynamics Even if the monopedal model of Remark III-A has an actuated shank, it does not satisfy Prop. 23. A suitable example satisfying this last condition is the sagittal plane biped of Fig. 2 with m = 4 inputs, resulting in u f ∈ R 2 . The full input decomposition is a set of orthogonal columns [e φ , e c , E f ], the first two of which (e φ (10) and e c (22)) have been defined.

B. TRACTION-AWARE VERSION
The anchoring in the prior subsection does not account for crucial traction constraints conventionally incorporated in WBC. In this section we focus on our usage in a sagittal-plane biped, such as Minitaur (Fig. 5), with fully-actuated rigid legs, i.e. B = I , G θ = 0. We implement a QP that utilizes the results of Prop. 1 with arbitrary desired template dynamics m br + G p =: v des , where we use the · des superscript notation to denote the desired control signal. Using the controller (14) with decision variable (to be minimized by the optimization) u = (u φ ,ṽ), whereṽ := E T u T in (13), we can formulate a pointwise-in-time optimization problem where f j ∈ R 2 are stance toe forces for each stance leg j. The objective (27) includes the anchoring and template control terms respectively, where for the latter, (13) allows us to relate −Īṽ to the desired right-hand-side v des .
However, next we additionally incorporate a traction constraint (29). Note that c φ (p) I u = c φ (p)u φ +ṽ (14) = A −T θ τ is a stacked vector of applied toe forces f j . For each of these f j , we use a friction cone approximation, |f jx | ≤ µf jz , and require (29). Thus the constraints are linear in u, and the objective is quadratic in it. Torque constraints are straightforward to add to this optimization problem (since the leg Jacobian linearly relates the toe force f i to the joint torques). However, the direct drive (hence offering greater power density at the expense of reduced force density [15]) nature of Minitaur made these constraints overly conservative in our application, and so we left them off in our empirical trials.
We observe that a zero toe force f j = 0 is always feasible for (28)- (29), and so in configurations where the pitch-steadying control cannot be supported by the available traction, the requested toe forces diminish to zero, resulting in the robot collapsing to the ground.

IV. NUMERICAL RESULTS
We now apply the analytical results above to design controllers for hopping and leaping tasks, using the templates of Fig. 2.

A. PITCH-STEADY RAIBERT HOPPER
First, we present a numerical simulation of a planar hopper with a passively-compliant leg attached to the CoM (Fig. 3), which satisfies Prop. 21. As we have discussed in Remark III-A, the application of the controller (14) to this model produces dynamical behavior of the virtual CoM (19) closely reflecting SLIP (26). We simulate this model with our reduction controller tasked with steadying the pitch to φ * = −1 rad, and compare the resulting behavior of the physical CoM (in red) as well as the virtual CoM (13) (in blue) to the ideal SLIP behavior (in magenta, dashed). The SLIP model is initialized with the same initial conditions as the virtual CoM.
We make the following observations from Fig. 3: (a) the orientation is stabilized and displays the exponential attraction of the decoupled stable anchoring dynamics prescribed in (15); (b) the virtual CoM behavior closely resembles that of SLIP (especially in the angular momentum about the virtual toe); (c) the radial dynamics of the virtual CoM, a UA template (Fig. 2), closely resemble the passively-compliant SLIP leg.
In the following section, we examine a model with an added actuator in the leg that enables actuation of the radial component of the virtual CoM for template-based control of a leaping task.

B. PITCH-STEADY LEAPING WITH OFFSET-HIP MONOPED
We apply our results to a leaping task on a sagittal-plane monoped with offset hip, as shown in Fig. 4. The task is to correct the body orientation to a desired φ * while attaining a desiredṙ * LO liftoff CoM velocity. For this task, we first develop a template controller using the configuration-reactive template selection of Prop. 2. To anchor these reference dynamics, we compare two methods: the hierarchical controller we have presented here (15) and a model-predictive WBC, as we now detail.

1) TEMPLATE CONTROLLER
Using the results in this paper, we devise a controller for a CFA template for an underactuated monopedal leaping task (Fig. 2) to (a) anchor the template model to the posture φ = φ * in the physical robot using the control component u φ (15), and (b) control the template dynamics to accomplish the leaping task using the component u T (22).
Since we find it convenient to work in virtual-leg polar coordinates, we define ρ = r , ψ = r as functions of the virtual leg position r (12). We use a template control input, u c , (acting as a central force (23)) which performs proportional velocity control of the radial component ρ around an  (15), showing the virtual leg r (12) superimposed in blue on the physical model. While the frames are sequenced from left to right, the direction of travel of the CoM is right-to-left, and the snapshots are evenly spaced in time between t = 0.1s (touchdown) and the liftoff time. Along the bottom rows, we plot various template states using our proposed controller (blue) as well as when the reference dynamics are tracked by an MPC-based WBC, applying the same template controller (30) to the full body dynamics (magenta). Note that the behavior with the two implementations is very similar.

event-sequenced pair of setpoints as follows
where w(ρ des ) := k v (ρ des −ρ), whereρ p , ψ p are constant controller parameters whose values are obtained from the task and initial condition as described below, andρ des is a placeholder for the argument of the function w. Here χ p is an analytic switching function that transitions from 0 to 1 on the event that ψ crosses ψ p .
The key insights underlying this controller exploit the dynamical properties of the CFA template, in particular (a) the angular momentum about the virtual toe [50], [51], i.e., is only perturbed by gravity in stance α(q,q) ≡ α, and (b) the radial DoF can be velocity-controlled so as to impart the kinetic energy needed for the leap without disturbing the angular DoF. Next, we give a computational prescription for the task parameters,ρ p , ψ p , of (30) and an intuitive account of their meaning. Under the assumed template behavior described above, note thatψ = α/ρ 2 is sign-definite through the behavior, and so ψ can be used as a monotonic ''phase-like'' variable. Thus, the controller (30) drives the system sequentially through the ''wait'' (χ p = 0) and ''push'' (χ p = 1) phases, and takeoff is controlled (by releasing the leg) when r = ρ LO . In the following, we use the notationr = r/ r . Define v * := ṙ * LO . Conservation of (31) in the push phase implies Here we know the left hand side, and J Tṙ * LO , and can use the equation above to calculate r LO =: ψ LO .
Next, in the push phase, we can integrate both sides oḟ ψ = α/ρ 2 (31): Under our assumed template behavior, the radial velocity is controlled to (constant)ρ p in the push phase, so we pull it out of the integral to get VOLUME 10, 2022 Lastly, to find the value ofρ p necessary to ensure the takeoff velocity of the assumed velocity-controlled CFA template matches the desired valueṙ * LO , note that the polar transformation follows r = ρr ⇒ṙ =ρr + ρJrψ.
Applying to the desired liftoff state, where ψ LO , ρ LO , anḋ r LO =ṙ * LO are known, we can calculatė Now (33), (34) together completely define the necessary terms in the controller (30).

2) NUMERICAL SIMULATION
In the task shown in Fig. 4, the simulated robot is launched just before touching down (this event is marked by the light gray vertical line in the plots) initial pitch φ approximately horizontal, and initial horizontal velocityẋ = −1.5 m/s. The target orientation is φ * = −1 rad (w.r.t. vertical), and the target takeoff velocity isṙ * LO = (−2, 2) m/s. The plots along the bottom row show various template states, and in the second-from-right plot, we also display the soft switch signal χ p (22) with dashed lines.

a: INPUT-DECOUPLED ANCHORING
From the closeness of the traces in the top two plot panes of Fig. 4 to their commanded counterparts in dashed grey, we can conclude that the task is completed successfully by the naive template controller above. We consider its successful anchoring (which relies very heavily on the conservation of (31), a dynamical property of the mode-reactive template selection) evidence to the utility of input-decoupled anchoring. Since the template controller relies on an assumption of constant angular momentum (31), we use a low-passed filtered version of the measured angular momentum. As visible in the bottom left plot, α remains roughly constant and almost unaffected by the switch to the push phase in the case of our proposed controller (blue), and ultimately this results in the template CoM velocity closely matching the commanded values. In addition, we can observe the following from Fig. 4: (a) the orientation can be effectively controlled while controlling the template (φ after controller engagement, bottom right); (b) the template CoM velocity can be altered drastically in the push phase as evidenced byż (second from left) with no coupling to the φ dynamics in the case of our proposed control (blue).

b: COMPARISON TO AN MPC-BASED WBC
For comparison, we present results from an implementation of an MPC-based whole-body controller for tracking the desired accelerations output by the template controller. Note that by using the mode-reactive template (contribution of this paper) to generate the reference dynamics, we avoid underactuation issues that would plague a conventional MPC implementation with naive reference dynamics. The MPC uses as its dynamics model a discretized linearization of the floating torso dynamics (11) along with a first-order integration scheme. We have described the details of the formulation of the MPC in Appendix VI. With access to a complete and accurate model of the system dynamics (note the presence of M x and G(x) in the equations of Appendix VI, and their absence in (15), (23)) and a dynamically feasible reference, the MPC successfully stabilizes the body pitch and can drive the CoM velocity to the desired quantity.
We believe that the most important contribution here is the algorithmic mode-reactive template selection, and have demonstrated its effectiveness with a new anchoring controller (Sec. VI) as well as an MPC-based WBC here. Based on the comparison in Fig. 4, we can conclude that our simple template controller based around momentum can be anchored by both methods with similar performance. However, the traction-aware version of the proposed Sec. III controller is less reliant on an accurate model, and is significantly more computationally-efficient than the MPC, facilitating implementation onboard Minitaur (Fig. 1).
The modular architecture of the controller allows not just the same template controller (22) to be used with different anchoring strategies (as discussed above), but also for different template control strategies to be utilized with the same anchoring. For example, the hierarchical pitch-steady anchoring procedure (Sec. III-B) could be utilized with the output of a trajectory optimization solution to the leaping task above via its output v des , as an alternative to (22).
As a very approximate estimate of the relative complexity of the traction-aware QP (Sec. III-B) and the MPC here, the total number of nonzeros in the objective and constraint matrices in the QP of Sec. III-B is 11, whereas the number of nonzeros in H (44) in the MPC is approximately 70000. The former is also solved onboard Minitaur's embedded 180MHz Cortex-M4 microcontroller in a few hundred microseconds, while the latter takes about 120ms on a desktop processor. The former is implemented in C and solved using OSQP [52], while the latter is solved via MATLAB's solvers. Note that a well-optimized MPC implementation for dynamically simpler tasks can run onboard in millisecond timescales on a laptop-class processor [20], but would likely require longer horizons for a task like the one simulated here.

A. UNDERACTUATED MONOPEDAL MONKEY BARS TASK
Many useful leaping tasks such as achieving a foothold on a high ledge [53] or door opening [54] require the ability to reach objects or handholds outside of a platform's quasi-static workspace. To showcase the effectiveness of this controller in such problem settings, we devise a representative task (Fig. 1) which requires the robot to swing from a handhold,  Fig. 1, showing the convergence of the body to the desired pitch and pitch velocity, (top two plots) as well as the relative consistency of angular momentum throughout stance (bottom plot, corresponding with only the green wait and blue push modes). Note that the traces are plotted such that the end of the ''wait'' phase is at t = 0.25 s in all trials, and the time at which the shortest wait mode begins is denoted by ''Latest TD time. '' We cut the horizontal axis off at t = 0.4 s since the latest takeoff time is earlier than that. Each trial is displayed with a different color in the bottom plot, and they are only drawn when the leg is in contact, as calculated from (31). B Position and velocity of CoM in the trials of Sec. V-A show resemblance to a SLIP (26) simulation (dashed magenta).
release it, and then upon landing perform a single stance leap that enables a reach toward and grasp of a second handhold. The handholds are placed sufficiently high and far apart that they will be reachable only if (a) the body pitch is maintained near vertical, and (b) the trajectory of the CoM maintains its forward velocity, and achieves a sufficient vertical velocity to allow the upper legs to reach the target handhold.
This task is similar to the one in Sec. IV-B, but while in Sec. IV-B the task requires control of the takeoff velocity vector precisely, this one requires less precise control of the velocity. The properties of the CFA template are used predominantly for stance control in Sec. IV-B, and for selecting an appropriate landing leg angle here. The rapid stabilization of body orientation (Def. 1) is critical in both tasks, thus motivating a pitch-steady anchoring in either case.
We construct a sequential composition of three template controllers defining a hybrid system as in [55] whose mode transitions are triggered by guards sensitive only to the template states. The first (a brachiating template [46] which executes swing-up to excite the leap-down from the bar) and the third (a point particle manipulator to position the aerial mode toes to grasp the bar [55]) lie outside the scope of this paper, hence we will only provide the briefest description that permits interpreting the data presented in Fig. 5. During the middle stance mode, we implement input-decoupled anchoring (Prop. 1) to ensure that the body pitch is stabilized to a desired φ * and that the reduced virtual CoM behaves in a SLIP-like fashion (Prop. 2). For our experiments we used both the pinned-toe version (14) as well as the traction-aware version of Sec. III-B.
The construction of the template controller, takes the form of a conventional sequential composition [2] of the ''wait'' and ''push'' controllers, and G (wait) denotes the goal set of the wait controller. This closely resembles the template controller of Sec. IV-B1, with the following differences: (a) we replace the piecewise-constant velocity command (30) in the wait mode with a SLIP-like virtual spring control in the wait mode (to minimize the required peak torque at the switching instants); (b) we replace the analytic switch χ p with a discrete switch from wait to push mode when the orientation error state enters an -ball around the origin and (c) in the push phase, we replace the velocity controller (30) in the push mode a constant command, u r,max , saturating the actuators' torque outputs in the fashion of [53], launching the aerial ascent to the targeted next monkey bar. Leveraging the input-decoupled template behavior (23) and prior work on monopedal hopping, we select the initial leg angle before landing using Raibert's neutral point approximation whereẋ is the horizontal velocity of the CoM, T s is the stance duration, ρ is the nominal leg extension,ẋ d is the desired horizontal CoM velocity, and kẋ is a control gain. The unreliable nature of velocity estimates makes it prudent to pre-select an estimate ofẋ during the descent. Since the VOLUME 10, 2022 virtual leg extension ρ is held constant in flight, the expression becomes the constantψ = 0.21 radians. We first directly implement the pinned-toe version of the analytical controller (14), and test across 20 trials. Of these trials, the task is successfully completed 9 times, with the vast majority of failures caused by traction loss events. We observe successful task completion in 9 out of 10 trials using the traction-aware version of Sec. III-B, and the single failure in this implementation was one related to traction: the required motor torques needed to stabilize the body could not satisfy the friction constraints (1), and so the objective (27) could not be sufficiently minimized, resulting in the body collapsing and falling over. Fig. 1 superimposes snapshots of task execution, accompanied in Fig. 5A by state trajectory traces of all 10 of the traction-aware trials as well as one each of a success and failure using the pinned-toe version. The plots reveal that φ is effectively stabilized to the target range, and the virtual leg angular momentum remains roughly constant through the energetic behavior. This figure also presents an overview of the ability of the controller presented here to decouple the complex leaping problem into two lower-dimensional control problems: the behavior designer works within the CoM behavior depicted in Fig. 5B, which reveals a phase-portrait and CoM trajectory resembling the SLIP template (26), while the controller isolates the behavior of the orientation DoF.

B. BIPEDAL LEAP: IMPOSING HORIZONTAL CONTROL
Our past experience [55] with leaping on to objects-as part of a suite of pedipulation behaviors-with Minitaur provides a backdrop against which to compare advances with the proposed controller. In such problem settings, due to the number of possible unwanted collisions between the body or legs and object to be manipulated, it is crucial that the body be able to stabilize to a desired orientation while the CoM is energized for the leap.
In the double stance case, using the FA template (per Prop. 2), we can utilize a control input u T for direct control over the vertical and horizontal components of the CoM in the sagittal plane, while stabilizing the body pitch. We execute the leap with the FA template by simply servoing to a desired takeoff velocity, as in [55]. Fig. 6 (left) presents data from two sets of 5 trials to demonstrate the ability of the controller to quickly correct the pitch to a desired value (level in this case), as seen in the left and center of the figure, while accelerating the CoM to a desired velocity trajectory, as seen in the bottom two subplots. Fig. 6 (right) presents data from two more sets of 5 trials each, this time with the pitch initially level, but with the front and back legs at different extensions to maintain this attitude. We see that the controller successfully maintains the orientation while imparting significant acceleration to the CoM.

VI. CONCLUSION
In this paper, we presented an algorithmic and formal mode-reactive template selection procedure that facilitates the online construction of feasible reference dynamics. We anticipate that using these templates in a mode-reactive template planner preceding the anchoring step (as we have demonstrated here) can enable better behaviors than a conventional approach of assuming a FA template model (suffer from underactuation) or prior trajectory optimization (computationally too demanding to run reactively), followed by WBC. We also presented an accompanying hierarchical ''pitch-steady'' prioritized anchoring strategy that is computationally very efficient, and utilized it to demonstrate dynamically challenging leaping behaviors with (CFA and FA) template-based controllers on a quadrupedal robot. These two components are connected in a modular fashion, where either the template controller could be replaced by trajectory optimization, or the anchoring controller could be replaced by a different whole-body controller (Sec. IV-B2.b). Both applications illustrated the value of input-decoupled anchoring, whereby the strong control authority required to stabilize pitch within the short available stance mode preceding the leaps could barely be detected in the state trajectories when projected onto the virtual template coordinates. We remind the reader that the guaranteed dynamically-feasible reference dynamics from mode-reactive templates can enhance the performance of any WBC approach, including (but not limited to) the two we compared in Sec. IV-B2.b. Additionally, the dynamical simplicity of the template models enables reactive template (re)planning, which is crucial if the behavior includes combinations of single-and double-contact intervals and potentially unintended contact [55].
There are a number of avenues of future extension. First, in terms of the model, though our limited focus on a massive torso and massless legs results in a simple expression for the angular kinetic energy, this idea could be generalized to settings with distributed mass-such as flexible spines [56], inertial appendages such as tails [57] and flails-by controlling centroidal angular momentum [10], or the net angular momentum about the CoM. Second, here we restricted our attention to planar models, but an extension to the spatial case does not present any conceptual obstacles. Lastly, work currently underway by the second author is investigating the application of this strategy to the control of steady-state gaits, as well as tasks that include non-point-attractor orientation dynamics such as bounding [37].

APPENDIX A BACKGROUND: TEMPLATES AND ANCHORS
Hierarchical control structures and reduced-order models have been studied in the literature with the language of ''templates'' (reduced dynamics residing on an invariant ''template submanifold'' T ) and anchoring dynamics that render T attracting [28], [44], or ''zero dynamics'' (restriction dynamics on T ) and ''virtual constraints'' that render T attracting and invariant [42]. The benefits of hierarchy include modularity in the control design [29], [58], [59] allowing control designers to pull back [44] template controllers on to the anchoring body (''template-based control''), and its empirical usage in robotics has a long tradition stretching back to Raibert's hoppers [29]. There is a long and continuing tradition of using such reduced locomotion models, in turn, as control targets to be exposed to higher level task controllers [30], [45], [60], [61], as well as in optimization-based WBC [35], [36].

APPENDIX B INPUT-DECOUPLED ANCHORING VIRTUAL LEG PROJECTION
First, we define the notation D p h =: Lemma 1:r does not depend on u φ if we can find h (12) such that Proof: Taking derivatives of (12), Now all the u φ terms must appear inp,φ; they cannot in the last parenthesized terms (until further derivatives are taken). From (6), (11), the first two terms from above that are affected by the input torque τ are where O(τ 0 ) refers to terms without any τ -dependence. Thus, from (12), we need that 1) The matrix H p I · · · I + m b i b h φ c T φ has a non-trivial nullspace.
2) The anchoring control u φ points in that nullspace direction. Next, we show that (37) is a sufficient condition for the former: Using (37) to rewrite h φ in the parenthesized matrix in (39), which clearly has c φ itself as a nullspace vector.
Using (14) in (11) and (39)- (40), but now looking only at the first two rows and denoting G p as the projection of G x on to the first two rows, we see that (up to O(φ,φ)), . So we see that u φ does not appear in this expression. Additionally, from (19), H p = I + O(φ), thus giving us the expression of (13).
Lemma 1 shows that the template coordinates r are unaffected by the anchoring force u φ , satisfying our Def. 1. The main restriction to applying it is that h(p, φ) satisfying (37) exists. The following lemma shows that we can approximate this function.
Lemma 2: With r = h 0 + h 1 + . . ., the approximation error for (37), can be controlled to orders of the orientation error ζ , δ k = ∇ζ k k!δ k (p), by setting whereδ is defined recursively asδ k (p) := −D pδk−1 (p) · c φ (p). Proof: We use a proof by induction: the base case h 0 = p reveals that δ 0 =c φ (p) =δ 0 (p). The induction step is The induction step relies at least upon the ∂h k−1 ∂pc φ (p) term cancelling with the ∂h k ∂φ term. This requires a factorial in the denominator so that ∂h k ∂φ will be multiplied by 1 (k−1)! and that the lingering factor of ∂h k−1 ∂pc φ (p) will also have that denominator and cancel. So, this series (with k terms) can approximate the error (37) to O(∇ζ k ).

APPENDIX C NON-INPUT-DECOUPLED PITCH-STEADY ANCHORING
In this section we present a feedback-linearization approach to anchoring pitch-steady dynamics and demonstrate its pitfalls compared to the approach of Prop. 1.

Proposition 3: If B T A †
θ c φ = 0, where c φ is in (10), A † θ is in (8), and B is in (5), then we can preferentially prescribe the template manifold as attracting and invariant to satisfy Def. 1.
Proof: We set a pseudo-inverse for the coefficient of τ in the last row of (11). In closed loop, where the feedback law assigned u φ will be specified in (15), the last row of (11) is = −k p ∇ζ − k dφ , which can render the ζ = 0 template manifold attracting and invariant.
We can evaluate the Lie derivative of the first two rows of (11) along the flow, where we can find u φ as a (noise-contributing) defect in thë p dynamics. In other words, the closed-loop dynamics of the template coordinates cannot be decoupled from the anchoring dynamics, thus failing Def. 1. We shall rectify this deficiency with an additional assumption and a new anchoring controller in Prop. 1.
Remark 6 (Relationship to Input-Decoupled Anchoring): First, note that the condition required in Prop. 1 implies satisfaction of the condition for conventional anchoring in Prop. 3: if A T θ c φ ∈ B, for given υ = 0, we can find τ s.t. Bτ = A T θ c φ υ. Then, c T φ A †T θ Bτ = c T φ c φ υ = 0, 5 and so it must be true that c T φ A †T θ B = 0 which implies anchorability in the sense of Prop. 3.
In exchange for the slightly stricter condition, we get the benefit of property Prop. 12c: (13) has no dependence on the (possibly large) control force u φ (15) (as posited by Def. 1), and only on the O(φ,φ) ''state error'' terms, which vanish on the template manifold. In practice, as we show in Sec. IV, the orientation-error-dependent terms cause negligible disturbance also off this manifold, while the ''classical'' alternative of relying on template-like behavior of the actual CoM suffers from large disturbances introduced by the anchoring process.

APPENDIX D MPC-BASED WBC
Solving for acceleration in (9) for a single-toed contact mode simplifies to ∈ R 2 are the forces at the toes and u := ϒ 1 is enforced using an equality constraint. The dynamics of the system for state y := ẋ x ∈ R 6 can then be described aṡ x G(x) u Linearizing around an operating point (y 0 , u 0 ), the system can be approximated aṡ  The objective of the quadratic program is specified as a quadratic error of the states from the reference states, The reference state y * is obtained from the outputs of our template controller (22) acting on the mode-reactive CFA template, and the reference input isū * = 0 (i.e. ϒ 0 = 0 0 T ). In the objective function for the MPC, there is a quadratic error from each y k in the horizon to y * . The state penalty matrices (Q and P) and input penalty matrix R are also different in the two modes. In particular in Fig. 4, The guard φ p between the two modes is calculated as in Sec. IV-B1 with the SLIP-like angle ψ := (p − a) and there is no use of the virtual COM by the controller. The QP can be written to include or not the traction constraints (A µ , b µ above) that we apply in Sec. III-B. The horizon length for the example was N = 100, and MPC timestep of dt = 1ms, so that the length in time of the horizon is T = Ndt = 100ms, and the MPC was recalculated every 10ms.