WBC learning notes (I): manually push WBC formula

Introduce ：WBC The controller is one of the most commonly used controllers for today's foot robot . Based on different priorities of multiple tasks , It can control the joint motor to meet the needs of each task in the order of priority .MIT Open source cheetah mini It uses MPC+WBIC Algorithm to control the landing point .

The derivation process refers to the following materials
mini cheetah in WBC Formula analysis
Based on the null space method （NUB） Full body control of （WBC） Simple implementation of
Robot Dynamics Lecture Notes
Due to my limited ability , about WBC The understanding of controller is not deep enough , Any mistakes in the derivation process are welcome to be criticized and corrected .
Contact information ：[email protected]

2. Formula description

$$\begin{array}{l}\Delta {\mathbf{q}}_i=\Delta {\mathbf{q}}_{i-1}+{\mathbf{J}}_{i\mid pre}^{†}\left({\mathbf{e}}_i-{\mathbf{J}}_i\Delta {\mathbf{q}}_{i-1}\right)\\ \\ {\dot{\mathbf{q}}}_i^{\mathrm{c}\mathrm{m}\mathrm{d}}={\dot{\mathbf{q}}}_{i-1}^{\text{cmd }}+{\mathbf{J}}_{i\mid pre}^{†}\left({\dot{\mathbf{x}}}_i^{\mathrm{d}\mathrm{e}\mathrm{s}}-{\mathbf{J}}_i{\dot{\mathbf{q}}}_{i-1}^{\mathrm{c}\mathrm{m}\mathrm{d}}\right)\\ \\ {\ddot{\mathbf{q}}}_i^{\mathrm{c}\mathrm{m}\mathrm{d}}={\ddot{\mathbf{q}}}_{i-1}^{\mathrm{c}\mathrm{m}\mathrm{d}}+\overline{J_{i\mid pre}^{\mathrm{d}\mathrm{y}\mathrm{n}}}\left({\ddot{\mathbf{x}}}_i^{\mathrm{c}\mathrm{m}\mathrm{d}}-{\dot{\mathbf{J}}}_i\dot{\mathbf{q}}-{\mathbf{J}}_i{\ddot{\mathbf{q}}}_{i-1}^{\mathrm{c}\mathrm{m}\mathrm{d}}\right)\end{array}$$
where
$$\begin{array}{l}{\mathbf{J}}_{i\mid prc}={\mathbf{J}}_i{\mathbf{N}}_{i-1}\\ \\ {\mathbf{N}}_{i-1}={\mathbf{N}}_0{\mathbf{N}}_{1\mid 0}\cdots {\mathbf{N}}_{i-1\mid i-2}\\ \\ {\mathbf{N}}_0=\mathbf{I}-{\mathbf{J}}_c^{†}{\mathbf{J}}_c\\ \\ {\mathbf{N}}_{i\mid i-1}=\mathbf{I}-{\mathbf{J}}_{i\mid i-1}{}^{†}{\mathbf{J}}_{i\mid i-1}\end{array}$$

• The derivation process involves linear algebra Pseudo inverse matrix and Zero space , You can refer to the description in Wikipedia
  Pseudo inverse matrix
  Zero space
• $\mathbf{q}$： Robot configuration matrix , It reflects the position of each joint . If joint i For rotating joints , be $q_i$ Represents the angle of rotation . If joint i To move a joint , be $q_i$ Represents the displacement .

Next, we mainly derive the following formula ：
$${\dot{\mathbf{q}}}_i^{\mathrm{c}\mathrm{m}\mathrm{d}}={\dot{\mathbf{q}}}_{i-1}^{\mathrm{c}\mathrm{m}\mathrm{d}}+{\mathbf{J}}_{i\mid pre}^{†}\left({\dot{\mathbf{x}}}_i^{\mathrm{d}\mathrm{e}\mathrm{s}}-{\mathbf{J}}_i{\dot{\mathbf{q}}}_{i-1}^{\mathrm{c}\mathrm{m}\mathrm{d}}\right)$$

（1） First, understand the representation of tasks .
Suppose there is n A mission , namely n Target states , use $x_i,i=1,...n$ Express , Based on the knowledge of forward kinematics ,
$$\begin{gathered} \dot{x_i}=J_i\dot{q} \\ ⋮ \\ \dot{x_n}=J_n\dot{q} \end{gathered}$$

（2） Redundant control
$$\dot{q}=J^{†}\dot{\mathbf{x}}+\left(I-J^{†}J\right)y$$
This is also WBC The control of the The core idea , That is to say $y$ Is an arbitrary vector that satisfies the number of rows .
For control tasks $\mathbf{x}$ Come on ,$\left(I-J^{†}J\right)$ Is a zero vector matrix , Equivalent to one “ filter ”, hold And control $\mathbf{x}$ Irrelevant robot degrees of freedom are filtered out .
namely , No matter what $y$ What is it? , Will not control $\mathbf{x}$ Any impact . such , We can adopt $y$ To control lower priority control tasks , Without affecting high priority tasks .
As shown in the figure ： Subspace A Represents the operation space under the first task ,B Represents the operation space of the second task ,C Represents the remaining redundant degrees of freedom .

（3） Derivation process
Task a ：
$$\begin{gathered} \dot{{\mathbf{x}}_0}=J_0\dot{q}-----(1) \\ \downarrow \\ \dot{q}=J_0^{†}\dot{{\mathbf{x}}_0}+\left(I-J_0^{†}{J}_0\right)y-----(2) \end{gathered}$$
Task 2 ：
$$\begin{gathered} \dot{{\mathbf{x}}_1}=J_1\dot{q}-----(3) \\ \downarrow \\ \dot{q}=J_1^{†}\dot{{\mathbf{x}}_1}+\left(I-J_1^{†}{J}_1\right)y-----(4) \end{gathered}$$
take （2） Plug in （3） in
$$\begin{gathered} \dot{{\mathbf{x}}_1}=J_1{J}_0^{†}\dot{{\mathbf{x}}_0}+J_1\left(I-J_0^{†}{J}_0\right)y \\ \downarrow \\ \dot{{\mathbf{x}}_1}-J_1{J}_0^{†}\dot{{\mathbf{x}}_0}=J_1\left(I-J_0^{†}{J}_0\right)y \end{gathered}$$
According to the parameter definition in the above formula ：
$$\begin{gathered} N_0=I-J_0^{†}{J}_0 \\ {J}_{1\mid 0}=J_1{N}_0 \\ \downarrow \\ \dot{{\mathbf{x}}_1}-J_1{J}_0^{†}\dot{{\mathbf{x}}_0}=J_{1\mid 0}y \end{gathered}$$
because $J_{1\mid 0}$ It is also a redundancy matrix , So the treatment method is as follows ：
$$\begin{gathered} y=J_{1\mid 0}^{†}(\dot{{\mathbf{x}}_1}-J_1{J}_0^{†}\dot{{\mathbf{x}}_0})+(I-J_{1\mid 0}^{†}{J}_{1\mid 0})z \\ \downarrow \\ y=J_{1\mid 0}^{†}(\dot{{\mathbf{x}}_1}-J_1{J}_0^{†}\dot{{\mathbf{x}}_0})+N_{1\mid 0}z----(5) \end{gathered}$$
take （5） Daihui （2） in
$$\begin{gathered} \dot{q}=J_0^{†}\dot{{\mathbf{x}}_0}+N_0{J}_{1\mid 0}^{†}(\dot{{\mathbf{x}}_1}-J_1{J}_0^{†}\dot{{\mathbf{x}}_0})+N_0{N}_{1\mid 0}z \\ \downarrow \\ \dot{q}=J_0^{†}\dot{{\mathbf{x}}_0}+N_0{J}_{1\mid 0}^{†}(\dot{{\mathbf{x}}_1}-J_1{J}_0^{†}\dot{{\mathbf{x}}_0})+N_{1}z \end{gathered}$$
According to the properties of zero space projection
$$\begin{gathered} N_0{J}_{1\mid 0}^{†}=J_{1\mid 0}^{†} \\ \downarrow \\ \dot{q}=J_0^{†}\dot{{\mathbf{x}}_0}+J_{1\mid 0}^{†}(\dot{{\mathbf{x}}_1}-J_1{J}_0^{†}\dot{{\mathbf{x}}_0})+N_{1}z \end{gathered}$$
Based on the previous discussion
$${\dot{q}}_0^{cmd}=J_0^{†}\dot{{\mathbf{x}}_0}$$
If there is no free redundancy , Then we can get the final result
$${\dot{q}}_1^{cmd}={\dot{q}}_0^{cmd}+J_{1\mid 0}^{†}({\dot{\mathbf{x}}}_0^{\mathbf{c}\mathbf{m}\mathbf{d}}-J_1{\dot{q}}_0^{cmd})$$
Compare the formulas to be derived , You can actually see that , The form has been consistent . So no further iterations .