Efficient recursive dynamics algorithms for operational-space control with application to legged locomotion

Abstract

This paper presents new recursive dynamics algorithms that enable operational-space control of floating-base systems to be performed at faster rates. This type of control approach requires the computation of operational-space quantities and suffers from high computational order when these quantities are directly computed through the use of the mass matrix and Jacobian from the joint-space formulation. While many efforts have focused on efficient computation of the operational-space inertia matrix ${\varvec{{\Lambda }}}$, this paper provides a recursive algorithm to compute all quantities required for floating-base control of a tree-structure mechanism. This includes the first recursive algorithm to compute the dynamically consistent pseudoinverse of the Jacobian ${\,\, \bar{{\varvec{\!\!J}}}}$ for a tree-structure system. This algorithm is extended to handle arbitrary contact constraints with the ground, which are often found in legged systems, and uses effective ground contact dynamics approximations to retain computational efficiency. The usefulness of the algorithm is demonstrated through application to control of a high-speed quadruped trot in simulation. Our contact-consistent algorithm demonstrates pitch and roll stabilization for a large dog-sized quadruped running at 3.6 m/s without any contact force sensing, and is shown to outperform a simpler Raibert-style posture controller. In addition, the operational-space control approach allows the dynamic effects of the swing legs to be effectively accounted for at this high speed.

Appendix: Validity of the big-ground-inertia approximation

This section provides a proof that the closed-chain algorithm with the big-ground-inertia approximation computes the correct terms $\tilde{{\varvec{{\Lambda }}}}_t$, $\tilde{{\varvec{b}}}_t$, and ${\,\,\tilde{\bar{{\varvec{\!\!J}}}}\, \!{}^T_{\!t}}$ in the limit when the contact inertias ${\varvec{I}}_{c_j}$ become infinite. Throughout this section it is assumed that the floating base for the contact-constrained system has a full 6 degrees of motion freedom. To assist in the proof, $\tilde{{\varvec{{\Lambda }}}}_{M}$ is defined according to its matrix definition in Eq. 20 and $\tilde{{\varvec{{\Lambda }}}}_t(\epsilon )$ is defined as the output of the closed-chain algorithm with ${{\varvec{I}}_{c_j} = \epsilon ^{-1} \mathbf{1}}$ for each contact $j$. It will be shown that

$$\begin{aligned} \lim _{\epsilon \rightarrow 0} \tilde{{\varvec{{\Lambda }}}}_t(\epsilon ) = \tilde{{\varvec{{\Lambda }}}}_{M} . \end{aligned}$$

(60)

Proof is provided here for $\tilde{{\varvec{{\Lambda }}}}_t$ since its recursive computation is the simplest algebraically, and proof that the algorithm achieves the correct limits for $\tilde{{\varvec{b}}}_t$ and ${\,\,\tilde{\bar{{\varvec{\!\!J}}}}\, \!{}^T_{\!t}}$ follows through identical arguments.

While the algorithms thus far have been derived from unconstrained dynamic equations of motion, recursive constrained dynamics can be derived exactly (not approximately as done here for the modified algorithm) using alternative constraint-propagation methods in (Featherstone 2008). These alternative algorithms are composed in terms of inverse inertias, denoted here as $\,\bar{\!{\varvec{I}}}_i = {\varvec{I}}_i^{-1}$, and articulated inverse inertias $\,\bar{\!{\varvec{I}}}{}_i^A$.

Although the standard articulated-body equations of motion are only valid when an articulated subsystem has a full 6 degrees of motion freedom, the more general equations

$$\begin{aligned} \mathbf{a}_i = \,\bar{\!{\varvec{I}}}^A_i\, \mathbf{f}_i + {\varvec{b}}^A_i + \sum _{k \in c^*(i)} {\varvec{A}}_{ik} {\varvec{\tau }}_k \end{aligned}$$

(61)

apply when constraints limit motion freedom. In the case that the subsystem rooted at $i$ has 6 degrees of motion freedom $\,\bar{\!{\varvec{I}}}_i^A = \left( \,\tilde{\!{\varvec{I}}}{}_i^A\right) ^{-1}$ and Eq. 61 can be rearranged to the articulated inertia relationship used in the main text

$$\begin{aligned} \mathbf{f}_i = \,\tilde{\!{\varvec{I}}}{}_i^A \mathbf{a}_i + \,\tilde{\!{\varvec{\beta }}}{}_i^A + \sum _{k \in c^*(i)} \,\tilde{\!{\varvec{B}}}_{i k} {\varvec{\tau }}_k . \end{aligned}$$

At each contact $c_i$, the constraint that $\mathbf{a}_{c_i} = -{}^{c_i} \mathbf{a}_{g}$, imposed approximately by Eq. 43, can be imposed exactly selecting $\,\bar{\!{\varvec{I}}}^A_{c_i} = {\varvec{0}}$ and seeding an inverse inertia recursion with the equation

$$\begin{aligned} \mathbf{a}_{c_i} = \,\bar{\!{\varvec{I}}}^A_{c_i} \,\mathbf{f}_{c_i}- {}^{c_i} \mathbf{a}_g. \end{aligned}$$

(62)

The articulated-body inertia recursions used in the main text of this paper could then be replaced with analogous recursions on the inverse inertia (Featherstone 2008) that follow

$$\begin{aligned} \,\bar{\!{\varvec{I}}}_{p(i)}^A&=\,\bar{\!{\varvec{I}}}_{p(i)}^A - \,\bar{\!{\varvec{I}}}_{p(i)}^A {\varvec{\varphi }}_i \left( {\varvec{\varphi }}_i^T ( \,\bar{\!{\varvec{I}}}_{p(i)}^A + \,\bar{\!{\varvec{I}}}_{i}^A ) {\varvec{\varphi }}_i \right) ^{-1} {\varvec{\varphi }}_i^T \,\bar{\!{\varvec{I}}}_{p(i)}^A, \end{aligned}$$

(63)

where ${\varvec{\varphi }}_i = {\varvec{{\Phi }}}_i^c$ (the matrix describing the constrained modes of motion for joint $i$). While coordinate transformations are omitted in this equation for clarity, they would be required for implementation. These recursions, while more general than the usual articulated-body recursions, are more computationally costly since the quantity

$$\begin{aligned} \left( {\varvec{\varphi }}_i^T ( \,\bar{\!{\varvec{I}}}_{p(i)}^A + \,\bar{\!{\varvec{I}}}_{i}^A ) {\varvec{\varphi }}_i \right) ^{-1} \end{aligned}$$

requires a $5\times 5$ matrix inverse for common revolute joints. The motivation to approximate these recursions through a constraint approximation, is that it allows the use of standard articulated inertia recursions which are more computationally amenable. Recursions for ${\varvec{b}}_i^A$ and ${\varvec{A}}_{ik}$ can be derived analogous to those for $\,\tilde{\!{\varvec{\beta }}}{}_i^A$ and $\,\tilde{\!{\varvec{B}}}_{i k}$.

Proof of Eq. 60:

Let $\,\bar{\!{\varvec{I}}}_{c_j} = \epsilon \mathbf{1}$ for each contact and denote $\,\bar{\!{\varvec{I}}}_i^A(\epsilon )$ as the final value of $\,\bar{\!{\varvec{I}}}_i^A$ obtained from the recursions of Eq. 63. In the case when $\epsilon =0$ the following exact relationship holds

$$\begin{aligned} \tilde{{\varvec{{\Lambda }}}}_{M} = \left( \,\bar{\!{\varvec{I}}}_1^A(0)\right) ^{-1} \end{aligned}$$

(64)

since Eq. 62 imposes an exact constraint at the contact in this case. Additionally when $\epsilon \ne 0$ the inverse inertia algorithm and the regular inertia algorithm provide equivalent output. That is

$$\begin{aligned} \tilde{{\varvec{{\Lambda }}}}_t(\epsilon ) = \left( \,\bar{\!{\varvec{I}}}_1^A(\epsilon ) \right) ^{-1} , \quad \forall \, \epsilon >0\,. \end{aligned}$$

(65)

Yet, the recursion equation Eq. 63 is comprised of matrix inverses, additions, and multiplies, which are all continuous with respect to their arguments. Thus, all quantities $\,\bar{\!{\varvec{I}}}_i^A(\epsilon )$ obey

$$\begin{aligned} \lim _{\epsilon \rightarrow 0} \,\bar{\!{\varvec{I}}}_i^A(\epsilon ) = \,\bar{\!{\varvec{I}}}_i^A(0)\,. \end{aligned}$$

(66)

Combination of these relationships provides

$$\begin{aligned} \lim _{\epsilon \rightarrow 0} \tilde{{\varvec{{\Lambda }}}}_t(\epsilon )&= \lim _{\epsilon \rightarrow 0} \left( \,\bar{\!{\varvec{I}}}_1^A(\epsilon ) \right) ^{-1} \nonumber \\&= \left( \lim _{\epsilon \rightarrow 0} \,\bar{\!{\varvec{I}}}_1^A(\epsilon ) \right) ^{-1} \nonumber \\&= \left( \,\bar{\!{\varvec{I}}}_1^A(0) \right) ^{-1} = \tilde{{\varvec{{\Lambda }}}}_M \end{aligned}$$

(67)

where once again the continuity of the matrix inverse of $\,\bar{\!{\varvec{I}}}_1^A(\epsilon )$ at $\epsilon =0$ was used in going from line 1 to line 2.