WBC learning notes (II): practical application of WBC control

This article mainly refers to the following articles , The purpose is to deepen my understanding of WBC Formula and understanding of multi priority task application .
Based on the null space method （NUB） Full body control of （WBC） Simple implementation of

Control system input ： The position of the slider on the track $y_h$, Position of each joint of the manipulator $\mathbf{\theta }=[{\theta }_1{\theta }_2{\theta }_3{]}^T$.
Control system output ： The speed of each joint of the manipulator $\dot{\mathbf{\theta }}$.
Control the mission 1： Keep the end position of the manipulator at the point under the world coordinate system $A=[20{]}^T$.
Control the mission 2： Make the end of the mechanical arm face x Axis .

1. Control scheme without priority

（1） Desired coordinates at the end
$${\mathbf{x}}_d=\left[\begin{array}{c}2\\ -y_h\\ 0\end{array}\right]$$
（2） Forward kinematics solution （ The end is relative to the origin ）
$$\mathbf{x}=\left[\begin{array}{l}x\\ y\\ \theta \end{array}\right]=\left[\begin{array}{c}{l}_1{c}_1+l_2{c}_{12}+l_3{c}_{123}\\ l_1{s}_1+l_2{s}_{12}+l_3{s}_{123}\\ {\theta }_1+{\theta }_2+{\theta }_3\end{array}\right]$$
（3） Seeking inverse
take $l_1=l_2=l_3=1m$ Plug in
$$\begin{gathered} \dot{\mathbf{x}}=J\dot{\mathbf{\theta }} \\ \downarrow \\ J=\left[\begin{array}{ccc}-s_1-s_{12}-s_{123}& -s_{12}-s_{123}& -s_{123}\\ c_1+c_{12}+c_{123}& c_{12}+c_{123}& c_{123}\\ 1& 1& 1\end{array}\right] \\ \downarrow \\ \dot{\theta }=J^{†}\dot{\mathbf{x}} \end{gathered}$$
Properties of pseudo inverse ： if $J$ Inverse , be $J^{†}$ yes $J$ The inverse of ; if $J$ No inverse , be $J^{†}$ Satisfy $\dot{\theta }=J^{†}\dot{\mathbf{x}}$ The least square solution of .

2. Control scheme with priority

For example, the position priority is higher than the direction priority
（1） Task breakdown
$$\begin{gathered} \{\begin{array}{l}{\mathbf{r}}_1={\left[\begin{array}{ll}x& y\end{array}\right]}^T\\ {\mathbf{r}}_2=\theta \end{array} \\ \downarrow \\ \{\begin{array}{l}{\mathbf{r}}_{1d}={\left[\begin{array}{ll}2& -y_h\end{array}\right]}^T\\ {\mathbf{r}}_{2d}=0\end{array} \end{gathered}$$
（2） Jacobian matrix solution
The specific parameter values of the matrix have been obtained above , Just break it down
$$\{\begin{array}{l}{J}_1=\left[\begin{array}{ccc}-s_1-s_{12}-s_{123}& -s_{12}-s_{123}& -s_{123}\\ c_1+c_{12}+c_{123}& c_{12}+c_{123}& c_{123}\end{array}\right]\\ J_2=\left[\begin{array}{lll}1& 1& 1\end{array}\right]\end{array}$$
（3） Plug in WBC The formula
$$\dot{\theta }=J_1^{†}{\dot{\mathbf{r}}}_1+\left(I-J_1^{†}{J}_1\right)J_2^{†}{\dot{\mathbf{r}}}_2$$

In actual control , Input the obtained velocity value into the controller of the joint （ Such as PID controller ） Control the joint motor