Integration of prioritized impedance controller in improved hierarchical operational-space torque control frameworks for legged locomotion robots

Abstract

This paper proposes to integrate our prioritized impedance controller (PIC) into four kinds of improved hierarchical operational-space torque-control frameworks: one pseudo-inverse-based dynamics method and three optimization-based techniques, including one null-space-based weighted optimization solver, one hierarchical optimization solver, and one hybrid hierarchical-weighted solver. The integrated PIC enables various impedance control forces to be consistent and compatible with multi-task hierarchies. The concept PIC can handle more general impedance controllers, including those built directly on specifically designed tasks and those indirectly acting on the whole-body behaviors. In the pseudo-inverse-based dynamics method and the null-space-based weighted scheme, each task acceleration with the prioritized impedance controller is extracted using one proposed modified dynamics model embodying PIC, which especially enables the integration of this new prioritized weighted scheme in the strict hierarchical control framework. For the hierarchical optimization solver, another modified dynamics model is proposed to integrate PIC and embody the multi-level control hierarchy. The second modified dynamics model makes it possible to achieve a more general and complete hierarchical optimization solver for both the task acceleration extraction and the same number of constraints for each hierarchy, expressed depending on the actuated torque. Then we use our second modified model for a more complex scenario, combining the hierarchical and the null-space-based weighted frameworks together. The two dynamics models are only developed for the derivation of the relative torque controllers, and the detailed algorithm is developed which is more general and compact for the three optimization solvers. Any of the approaches can be selected depending on the actual user-defined applications. Our methods are tested and compared in several simulation scenarios using a virtual quadruped-on-wheel robot, TowrISIR with a manipulator on its floating base.

Appendices

Appendix A: Derivation in null-space-based weighted convex optimization

The fundamental theory of the multi-task operational space control is referred in [45], which is used to derive the formulas in Appendix A, B and C. Then, the operational space force of the task $i$ can be expressed by using the modified dynamics model with PIC in (11), as below,

$$\begin{aligned} \boldsymbol{F}_{i|pre(i)} &=\boldsymbol{\varLambda }_{i|pre(i)}\ddot{\textbf{x}}_{i}+ \boldsymbol{h}_{i|pre(i)} - \boldsymbol{T}_{i|pre(i)} \end{aligned}$$

(40a)

$$\begin{aligned} &=\boldsymbol{\varLambda }_{i|pre(i)}\ddot{\textbf{x}}_{i}+\boldsymbol{h}_{i|pre(i)} - \boldsymbol{\varLambda }_{i|pre(i)}\boldsymbol{J}_{i}\boldsymbol{M}^{-1}\sum _{t=1}^{i-1}\boldsymbol{J}^{ \top }_{t|pre(t)}\boldsymbol{F}_{t|pre(t)} \end{aligned}$$

(40b)

$$\begin{aligned} &=\boldsymbol{\varLambda }_{i|pre(i)}\ddot{\textbf{x}}_{i}+\boldsymbol{h}_{i|pre(i)} - \boldsymbol{\varLambda }_{i|pre(i)}\boldsymbol{J}_{i}\boldsymbol{M}^{-1}\sum _{t=1}^{i-1}\boldsymbol{J}^{ \top }_{t|pre(t)}\bar{\boldsymbol{J}}^{\top }_{t|pre(t)}(\boldsymbol{{S}}^{ \top }\boldsymbol{\tau }- \boldsymbol{\varGamma }) \end{aligned}$$

(40c)

$$\begin{aligned} & = \bar{\boldsymbol{J}}^{\top }_{i|pre(i)}\left ( \boldsymbol{{S}}^{\top } \boldsymbol{\tau }- \boldsymbol{\varGamma } \right ) , \end{aligned}$$

(40d)

where $\boldsymbol{T}_{i|pre(i)}$ combines the tasks with priorities higher than $i$. (40a) and (40d) are derived by multiplying $\bar{\boldsymbol{J}}^{\top }_{i|pre(i)}$ on the left and right sides of the modified dynamics model in (11), respectively. Then the quadratic form of the task $i$ acceleration extraction in (15a) and (15b) can be achieved using (40a)-(40d) and the following formula,

$$ \ddot{\textbf{x}}_{i} =\boldsymbol{\varLambda }^{-1}_{i|pre(i)}\left [ \boldsymbol{F}_{i|pre(i)} + \boldsymbol{T}_{i|pre(i)} - \boldsymbol{h}_{i|pre(i)}\right ] = \boldsymbol{A}^{\top }_{i} \boldsymbol{\tau } + \boldsymbol{B}_{i}. $$

(41)

Appendix B: Derivation in null-space-based hierarchical convex optimization

The operational space force of the task $i$ can be expressed by using the second modified dynamics model with PIC in (22), as below,

$$\begin{aligned} \boldsymbol{F}_{i|pre(i)} &=\boldsymbol{\varLambda }_{i|pre(i)}\ddot{\textbf{x}}_{i}+ \boldsymbol{h}_{i|pre(i)} - \boldsymbol{T}_{i|pre(i)} \end{aligned}$$

(42a)

$$\begin{aligned} &=\boldsymbol{\varLambda }_{i|pre(i)}\ddot{\textbf{x}}_{i}+\boldsymbol{h}_{i|pre(i)} - \boldsymbol{\varLambda }_{i|pre(i)}\boldsymbol{J}_{i}\boldsymbol{M}^{-1}\sum _{t=1}^{i-1}\boldsymbol{J}^{ \top }_{t|pre(t)}\boldsymbol{F}_{t|pre(t)} \end{aligned}$$

(42b)

$$\begin{aligned} &= \bar{\boldsymbol{J}}^{\top }_{i|pre(i)}\left ( - \boldsymbol{\varGamma } + \sum _{t=1}^{K} \boldsymbol{N}^{\top }_{pre(t)}\boldsymbol{S}^{\top }\boldsymbol{\tau }_{t} \right ) \end{aligned}$$

(42c)

$$\begin{aligned} &= \bar{\boldsymbol{J}}^{\top }_{i|pre(i)}\left ( \boldsymbol{{S}}^{\top } \boldsymbol{\tau }_{i} - \boldsymbol{\varGamma } + \sum _{t=1}^{i-1}\boldsymbol{S}^{\top }\boldsymbol{\tau }_{t} \right ) , \end{aligned}$$

(42d)

where (42a) and (42c) are derived by multiplying $\bar{\boldsymbol{J}}^{\top }_{i|pre(i)}$ on the left and right sides of the second modified dynamics model in (22), respectively. Then the quadratic form of the task $i$ acceleration extraction in (24a), (24b) can be achieved by combining (41) and (42a)-(42d).

Appendix C: Derivation in the null-space-based hybrid hierarchical-weighted convex optimization

The operational space force of task $i$ can also be expressed by using the second modified dynamics model with PIC in (22), as below,

$$\begin{aligned} \boldsymbol{F}_{i_{j}|pre(i_{j})} =&\boldsymbol{\varLambda }_{i_{j}|pre(i_{j})} \ddot{\textbf{x}}_{i_{j}}+\boldsymbol{h}_{i_{j}|pre(i_{j})} - \boldsymbol{T}_{i_{j}|pre(i_{j})} \end{aligned}$$

(43a)

$$\begin{aligned} =& \bar{\boldsymbol{J}}^{\top }_{i_{j}|pre(i_{j})}\left ( \boldsymbol{{S}}^{ \top }\boldsymbol{\tau }_{i} - \boldsymbol{\varGamma } + \sum _{t=1}^{i-1}\boldsymbol{S}^{\top } \boldsymbol{\tau }_{t}\right ), \end{aligned}$$

(43b)

where (43a) and (43b) are derived by multiplying $\bar{\boldsymbol{J}}^{\top }_{i_{j}|pre(i_{j})}$ on the left and right sides of the second modified dynamics model in (22), respectively. $\boldsymbol{T}_{i_{j}|pre(i_{j})}$ combines the tasks with priorities higher than $i$, and the subtasks from $i_{1}$ to $i_{j-1}$, as follows,

$$ \begin{aligned} \boldsymbol{T}_{i_{j}|pre(i_{j})} = & \boldsymbol{\varLambda }_{i_{j}|pre(i_{j})}\boldsymbol{J}_{i_{j}} \boldsymbol{M}^{-1}\left [ \sum _{t=1}^{i-1}\boldsymbol{J}^{\top }_{t|pre(t)}\boldsymbol{F}_{t|pre(t)} +\sum _{p=1}^{j-1}\boldsymbol{J}^{\top }_{i_{p}|pre(i_{p})}\boldsymbol{F}_{i_{p}|pre(i_{p})} \right ] \\ =&\boldsymbol{\varLambda }_{i_{j}|pre(i_{j})}\boldsymbol{J}_{i_{j}}\boldsymbol{M}^{-1}\left [ \sum _{t=1}^{i-1}\boldsymbol{J}^{\top }_{t|pre(t)}\bar{\boldsymbol{J}}^{\top }_{t|pre(t)}(- \boldsymbol{\varGamma } + \sum _{k=1}^{t}\boldsymbol{S}^{\top }\boldsymbol{\tau }_{k})\right ] + \\ &\boldsymbol{\varLambda }_{i_{j}|pre(i_{j})}\boldsymbol{J}_{i_{j}}\boldsymbol{M}^{-1}\left [ \sum _{p=1}^{j-1}\boldsymbol{J}^{\top }_{i_{p}|pre(i_{p})}\bar{\boldsymbol{J}}^{\top }_{i_{p}|pre(i_{p})}( \boldsymbol{{S}}^{\top }\boldsymbol{\tau }_{i} - \boldsymbol{\varGamma } + \sum _{t=1}^{i-1} \boldsymbol{S}^{\top }\boldsymbol{\tau }_{t})\right ] \\ =& \boldsymbol{U}_{i_{j}}\boldsymbol{\tau }_{i} + \boldsymbol{V}_{i_{j}}, \end{aligned} $$

(44)

where $\boldsymbol{U}_{i_{j}}$ and $\boldsymbol{V}_{i_{j}}$ can be extracted using the above formula, as below,

$$\begin{aligned} &\boldsymbol{U}_{i_{j}} = \boldsymbol{\varLambda }_{i_{j}|pre(i_{j})}\boldsymbol{J}_{i_{j}}\boldsymbol{M}^{-1} \sum _{p=1}^{j-1}\boldsymbol{J}^{\top }_{i_{p}|pre(i_{p})}\bar{\boldsymbol{J}}^{\top }_{i_{p}|pre(i_{p})} \boldsymbol{S}^{\top }, \end{aligned}$$

(45a)

$$\begin{aligned} &\begin{aligned} \boldsymbol{V}_{i_{j}} =&\boldsymbol{\varLambda }_{i_{j}|pre(i_{j})}\boldsymbol{J}_{i_{j}}\boldsymbol{M}^{-1} \left [ \sum _{t=1}^{i-1}\boldsymbol{J}^{\top }_{t|pre(t)}\bar{\boldsymbol{J}}^{\top }_{t|pre(t)}(- \boldsymbol{\varGamma } + \sum _{k=1}^{t}\boldsymbol{S}^{\top }\boldsymbol{\tau }_{k})\right ] + \\ &\boldsymbol{\varLambda }_{i_{j}|pre(i_{j})}\boldsymbol{J}_{i_{j}}\boldsymbol{M}^{-1}\left [ \sum _{p=1}^{j-1}\boldsymbol{J}^{\top }_{i_{p}|pre(i_{p})}\bar{\boldsymbol{J}}^{\top }_{i_{p}|pre(i_{p})}(- \boldsymbol{\varGamma } + \sum _{t=1}^{i-1}\boldsymbol{S}^{\top }\boldsymbol{\tau }_{t})\right ] , \end{aligned} \end{aligned}$$

(45b)

Then the quadratic form of the task $i_{j}$ acceleration extraction in (38a) and (38b) can be achieved by using (43a) and (43b) as follows,

$$ \ddot{\textbf{x}}_{i_{j}} =\boldsymbol{\varLambda }^{-1}_{i_{j}|pre(i_{j})}\left [ \boldsymbol{F}_{i_{j}|pre(i_{j})} + \boldsymbol{T}_{i_{j}|pre(i_{j})} - \boldsymbol{h}_{i_{j}|pre(i_{j})} \right ] = \boldsymbol{A}^{\top }_{i_{j}} \boldsymbol{\tau }_{i} + \boldsymbol{B}_{i_{j}}. $$

(46)

Appendix D: Floating-base motion

The base angular velocity can be expressed in the float-base frame as below,

$$\begin{aligned} \boldsymbol{\omega }_{b}&= \underbrace{\begin{bmatrix} 1 & 0 & -\sin (\beta ) \\ 0 & \cos (\alpha ) & \cos (\beta )\sin (\alpha ) \\ 0 & -\sin (\alpha ) & \cos (\alpha )\cos (\beta ) \end{bmatrix}}_{\boldsymbol{\varTheta }}\dot{\boldsymbol{\theta }}_{b}, \end{aligned}$$

(47a)

$$\begin{aligned} \dot{\boldsymbol{\omega }}_{b} &= \dot{\boldsymbol{\varTheta }} \dot{\boldsymbol{\theta }}_{b} + \boldsymbol{\varTheta }\ddot{\boldsymbol{\theta }}_{b}, \end{aligned}$$

(47b)

then $\dot{\boldsymbol{\theta }}_{b}$ and $\ddot{\boldsymbol{\theta }}_{b}$ can be achieved using the following relationships,

$$\begin{aligned} \dot{\boldsymbol{\theta }}_{b} &= \boldsymbol{\varTheta }^{-1} \boldsymbol{\omega }_{b}, \end{aligned}$$

(48a)

$$\begin{aligned} \ddot{\boldsymbol{\theta }}_{b} &= \boldsymbol{\varTheta }^{-1} \dot{\boldsymbol{\omega }}_{b} - \boldsymbol{\varTheta }^{-1}\dot{\boldsymbol{\varTheta }} \dot{\boldsymbol{\theta }}_{b}. \end{aligned}$$

(48b)

The above relationships can be referred detailed in [46]. The base translational velocity is derived as follows,

$$\begin{aligned} \dot{\boldsymbol{p}}_{b} &= {}^{0}\boldsymbol{R}_{b}\boldsymbol{v}_{b}, \end{aligned}$$

(49a)

$$\begin{aligned} \ddot{\boldsymbol{p}}_{b}&= {}^{0}\boldsymbol{R}_{b}\left ( \dot{\boldsymbol{v}}_{b}+ \boldsymbol{\omega }_{b}\times \boldsymbol{v}_{b}\right ) , \end{aligned}$$

(49b)

where ${}^{0}\boldsymbol{R}_{b}$ is the rotation matrix from the floating-base frame to the inertial frame. $\boldsymbol{v}_{b}$ is the base translational velocity expressed in the floating-base frame. The translational relationship can be referred in [18].