8 Forward dynamics (FD) for a planar robot manipulator

The dynamic equation of a planar robot with an arbitrary number of links can be derived using the Lagrangian formulation. The first step to use this method is to represent the kinetic and potential energies of the robot as functions of generalized coordinates, which are joint angles in this case. We assume that all masses are located at the end of each link, and we neglect the terms that contain rotational energies (i.e, zero moments of inertia). To do this, we exploit the formulation derived in Section 3 without the orientation part, so the position of the j- th mass can be written as

\bm{f}_{j}=\begin{bmatrix}f_{j,1}\\ f_{j,2}\end{bmatrix}=\begin{bmatrix}\sum_{k=1}^{j}l_{k}c_{k}\\ \sum_{k=1}^{j}l_{k}s_{k}\end{bmatrix},\quad\text{where}\quad c_{k}=\cos(\sum_{i=1}^{k}x_{i}),\ \ \ s_{k}=\sin(\sum_{i=1}^{k}x_{i}),
(105)

whose corresponding velocity can be calculated as

\bm{\dot{f}}_{j}=\begin{bmatrix}-\sum_{k=1}^{j}l_{k}(\sum_{i=1}^{k}\dot{x}_{i})s_{k}\\ \sum_{k=1}^{j}l_{k}(\sum_{i=1}^{k}\dot{x}_{i})c_{k}\end{bmatrix}.

By knowing the velocity, the j- th point kinetic energy can be expressed as

T_{j}=\frac{1}{2}m_{j}\big(\dot{f}_{j,1}^{2}+\dot{f}_{j,2}^{2}\big)=\frac{1}{2}m_{j}\Bigg(\!\!\Big(\!-\sum_{k=1}^{j}l_{k}\Big(\sum_{i=1}^{k}\dot{x}_{i}\Big)s_{k}\!\Big)^{\!2}\!+\!\Big(\sum_{k=1}^{j}l_{k}\Big(\sum_{i=1}^{k}\dot{x}_{i}\Big)c_{k}\!\Big)^{\!2}\!\Bigg),

and its potential energy as

U_{j}=m_{j}\,g\,f_{j,2}=m_{j}g\Big(\sum_{k=1}^{j}l_{k}s_{k}\Big).

The kinetic and potential energies are more dependent on absolute angles \sum_{i=1}^{k}x_{i} rather than relative ones x_{i} . This is because the energies of the system are coordinate invariant and are dependent on absolute values. For simplicity, we define the absolute angles as

q_{k}=\sum_{i=1}^{k}x_{i},

so the kinetic energy can be redefined as

T_{j}=\frac{1}{2}m_{j}\Big(\Big(-\sum_{k=1}^{j}l_{k}\dot{q}_{k}S_{k}\Big)^{2}+\Big(\sum_{k=1}^{j}l_{k}\dot{q}_{k}C_{k}\Big)^{2}\Big).

The robot’s total kinetic and potential energies are the sum of their corresponding values in each point mass. So, the Lagrangian of the whole system can be written as

E_{\text{lag}}=\sum_{j=1}^{N}T_{j}-U_{j},

where N is the number of links. Using the Euler-Lagrange equation, we can write

u_{z}=\frac{d}{dt}\frac{\partial E_{\text{lag}}}{\partial\dot{q}_{z}}-\frac{\partial E_{\text{lag}}}{\partial q_{z}}\quad=\frac{d}{dt}\frac{\partial(\sum_{j=z}^{N}T_{j})}{\partial\dot{q}_{z}}-\frac{\partial\sum_{j=z}^{N}(T_{j}-U_{j})}{\partial q_{z}}\quad=\sum_{j=z}^{N}\Big(\frac{d}{dt}\frac{\partial T_{j}}{\partial\dot{q}_{z}}-\frac{\partial T_{j}}{\partial q_{z}}+\frac{\partial U_{j}}{\partial q_{z}}\Big),
(106)

where u_{z} is the generalized force related to q_{z} . This force can be found from the corresponding generalized work w_{z} , which can be described by

w_{z}=\begin{cases}(\tau_{z}-\tau_{z+1})\dot{q}_{z}&z<N\\ \tau_{N}\dot{q}_{N}&z=N\end{cases},
(107)

where \tau_{z} is the torque applied at z- th joint. The fact that we need subtraction in the first line of ( 107 ) is that when \tau_{z+1} is applied on link l_{z+1} , its reaction is applied on link l_{z} , except for the last joint where there is no reaction from the proceeding links. Please note that if we had used relative angles in our formulation, we did not need to consider this reaction as it will be cancelled by itself at link z+1 (as \dot{q}_{z+1}=\dot{q}_{z}+\dot{x}_{z+1} ). That said, generalized forces can be defined as

u_{z}=\frac{\partial w_{z}}{\partial\dot{q}_{z}}=\begin{cases}(\tau_{z}-\tau_{z+1})&z<N\\ \tau_{N}&z=N\end{cases}.

By substituting the derived equations into ( 106 ), the dynamic equation of a general planar robot with an arbitrary number of links can be expressed as (for more details, please refer to Appendix B )

\sum_{j=z}^{N}m_{j}\Big(\sum_{k=1}^{j}l_{z}l_{k}c_{z-k}\ddot{q}_{k}\Big)=u_{z}-\sum_{j=z}^{N}m_{j}\Big(\sum_{k=1}^{j}l_{z}l_{k}s_{z-k}\dot{q}_{k}^{2}\Big)-\sum_{j=z}^{N}m_{j}gl_{z}c_{z},\quad\text{where}\quad\left\{{\begin{aligned} c_{h-k}&=c_{h}c_{k}-s_{h}s_{k},\\ s_{h-k}&=s_{h}c_{k}-c_{h}s_{k}.\end{aligned}}\right.
(108)

In ( 108 ), the coefficient of \ddot{q}_{k} can be written as

l_{z}l_{k}c_{z-k}\sum_{j=z}^{N}m_{j}\bm{1}(j-k),

where \bm{1}(\cdot) is a function that returns 0 if it receives a negative input, and 1 otherwise. The coefficient of \dot{q}_{k}^{2} can be found in a similar way as

-l_{z}l_{k}s_{z-k}\sum_{j=z}^{N}m_{j}\bm{1}(j-k).

By concatenation of the results for all z’s, the dynamic equation of the whole system can be expressed as

\tilde{\bm{M}}\ddot{\bm{q}}+\tilde{\bm{g}}+\tilde{\bm{C}}\text{diag}(\bm{\dot{q}})\bm{\dot{q}}=\bm{u},
(109)

where

\bm{q}=\begin{bmatrix}q_{1}\\ q_{2}\\ \vdots\\ q_{N-1}\\ q_{N}\end{bmatrix},\ \ \ \bm{u}=\begin{bmatrix}u_{1}\\ u_{2}\\ \vdots\\ u_{N-1}\\ u_{N}\end{bmatrix},\quad\tilde{\bm{M}}(z,k)=l_{z}l_{k}C_{z-k}\sum_{j=z}^{N}m_{j}\bm{1}(j-k),\quad\tilde{\bm{C}}(z,k)=l_{z}l_{k}S_{z-k}\sum_{j=z}^{N}m_{j}\bm{1}(j-k),
\tilde{\bm{g}}=\begin{bmatrix}\sum_{j=1}^{N}m_{j}l_{1}g\cos(q_{1})\\ \sum_{j=2}^{N}m_{j}l_{2}g\cos(q_{2})\\ \vdots\\ \sum_{j={N-1}}^{N}m_{j}l_{N-1}g\cos(q_{N-1})\\ m_{N}l_{N}g\cos(q_{N})\end{bmatrix}.

To use this equation with relative angles and torque commands, one can replace the absolute angles and general forces by

\bm{q}=\bm{L}\bm{x},\ \ \ \bm{u}=\bm{L}^{-{\scriptscriptstyle\top}}\bm{\tau},\quad\text{where}\quad\bm{x}=\begin{bmatrix}x_{1}\\ x_{2}\\ \vdots\\ x_{N-1}\\ x_{N}\end{bmatrix},\ \ \ \bm{\tau}=\begin{bmatrix}\tau_{1}\\ \tau_{2}\\ \vdots\\ \tau_{N-1}\\ \tau_{N}\end{bmatrix}.

By substituting these variables into ( 109 ), we would have

\bm{M}\ddot{\bm{x}}+\bm{g}+\bm{C}\text{diag}(\bm{L\dot{x}})\bm{L\dot{x}}=\bm{\tau},
(110)

where

\bm{M}=\bm{L}^{\scriptscriptstyle\top}\tilde{\bm{M}}\bm{L},\quad\bm{C}=\bm{L}^{\scriptscriptstyle\top}\tilde{\bm{C}},\quad\bm{g}=\bm{L}^{\scriptscriptstyle\top}\tilde{\bm{g}}

Please note that elements in \tilde{\bm{M}},\tilde{\bm{C}} and \tilde{\bm{g}} are defined with absolute angles, which can be replaced with the relative angles by

q_{i}=\bm{L}_{i}\bm{x},

where \bm{L}_{i} is the i - th row of \bm{L} .

8.1 Robot manipulator with dynamics equation

The nonlinear dynamic equation of a planar robot presented in Section 8 can be expressed as

\begin{bmatrix}\bm{q}_{t+1}\\ \dot{\bm{q}}_{t+1}\end{bmatrix}=\begin{bmatrix}\bm{q}_{t}+\dot{\bm{q}}_{t}dt\\ \dot{\bm{q}}_{t}+\tilde{\bm{M}}^{-1}\Big(\bm{u}-\tilde{\bm{g}}-\tilde{\bm{C}}\text{diag}(\bm{\dot{q}}_{t})\bm{\dot{q}_{t}}\Big)dt\end{bmatrix}.
(111)

For simplicity we start with ( 109 ), and at the end, we will describe how the result can be converted to the other choice of variables. The iLQR method requires ( 111 ) to be linearized around the current states and inputs.The corresponding \bm{A}_{t} and \bm{B}_{t} can be found according to ( 73 ) as

\bm{A}_{t}=\begin{bmatrix}\bm{I}&\bm{I}dt\\ \bm{A}_{21}dt&\bm{I}-2\tilde{\bm{M}}^{-1}\tilde{\bm{C}}\text{diag}(\dot{\bm{q}}_{t})dt\end{bmatrix},\quad\bm{B}_{t}=\begin{bmatrix}\bm{0}\\ \tilde{\bm{M}}^{-1}dt\end{bmatrix}.

\bm{A}_{21}=\frac{\partial\ddot{\bm{q}}_{t}}{\partial\bm{q}_{t}} can be found by taking the derivation of ( 109 ) w.r.t. \bm{q}_{t} . The p - th column of \bm{A}_{21} is the partial derivation of \ddot{\bm{q}}_{t} w.r.t. p - th joint angle at time t q_{t}^{p} which can be formulated as

\frac{\partial\ddot{\bm{q}}_{t}}{\partial q_{t}^{p}}=\frac{\partial\tilde{\bm{M}}^{-1}}{\partial q_{t}^{p}}\Big(\bm{u}-\tilde{\bm{g}}-\tilde{\bm{C}}\text{diag}(\bm{\dot{q}}_{t})\bm{\dot{q}}_{t}\Big)-\tilde{\bm{M}}^{-1}\Big(\frac{\partial\tilde{\bm{g}}}{\partial q_{t}^{p}}+\frac{\partial\tilde{\bm{C}}}{\partial q_{t}^{p}}\text{diag}(\dot{\bm{q}}_{t})\dot{\bm{q}}_{t}\Big).

Having this done for all p ’s at time t , we can write

\frac{\partial\ddot{\bm{q}}_{t}}{\partial\bm{q}_{t}}=\Big(\tilde{\bm{M}}^{-1}\Big)^{\prime}\Big(\bm{u}-\tilde{\bm{g}}-\tilde{\bm{C}}\text{diag}(\bm{\dot{q}}_{t})\bm{\dot{q}}_{t}\Big)-\tilde{\bm{M}}^{-1}\Big(\tilde{\bm{g}}^{\prime}+\tilde{\bm{C}}^{\prime}\text{diag}(\dot{\bm{q}}_{t})\dot{\bm{q}}_{t}\Big),

where

\tilde{\bm{g}}^{\prime}(i,j)=\begin{cases}-\sum_{j=i}^{N}m_{j}l_{i}gs_{i}&\text{if }i=j\\ 0&\text{otherwise}\end{cases},
\tilde{\bm{C}}^{\prime}=\begin{bmatrix}\frac{\partial\tilde{\bm{C}}}{\partial q_{1}^{t}}&\frac{\partial\tilde{\bm{C}}}{\partial q_{2}^{t}}&\cdots&\frac{\partial\tilde{\bm{C}}}{\partial q_{N}^{t}}\end{bmatrix},\quad\tilde{\bm{C}}^{\prime}(h,k,p)=\begin{cases}l_{h}l_{k}c_{h-k}\sum_{j=h}^{N}m_{j}\bm{1}(j-k)&\text{if}\ p=h\neq k\\ -l_{h}l_{k}c_{h-k}\sum_{j=h}^{N}m_{j}\bm{1}(j-k)&\text{if}\ p=k\neq h\\ 0&\text{otherwise}\end{cases},
(\tilde{\bm{M}}^{-1})^{\prime}=\begin{bmatrix}\frac{\partial\tilde{\bm{M}}^{-1}}{\partial q_{t}^{1}}&\frac{\partial\tilde{\bm{M}}^{-1}}{\partial q_{t}^{2}}&\cdots&\frac{\partial\tilde{\bm{M}}^{-1}}{\partial q_{t}^{N}}\end{bmatrix},\quad\Big(\tilde{\bm{M}}^{-1}\Big)^{\prime}=-\tilde{\bm{M}}^{-1}\tilde{\bm{M}}^{\prime}\tilde{\bm{M}}^{-1},
\tilde{\bm{M}}^{\prime}(h,k,p)=\begin{cases}-l_{h}l_{k}s_{h-k}\sum_{j=k}^{N}m_{j}\bm{1}(j-k)&\text{if}\ p=h\neq k\\ l_{h}l_{k}s_{h-k}\sum_{j=h}^{N}m_{j}\bm{1}(j-k)&\text{if}\ p=k\neq h\\ 0&\text{otherwise}\end{cases}.

According to Section 7.1 , we can concatenate the results for all time steps as

\begin{bmatrix}\Delta\bm{q}_{1}\\ \Delta\bm{\dot{q}}_{1}\\ \vdots\\ \Delta\bm{q}_{T}\\ \Delta\bm{\dot{q}}_{T}\end{bmatrix}=\bm{S}_{u}^{q}\begin{bmatrix}\Delta\bm{u}_{1}\\ \Delta\bm{u}_{2}\\ \vdots\\ \Delta\bm{u}_{T-1}\end{bmatrix},
(112)

where T is the total time horizon. These variables can be converted to relative angles and joint torque commands by

\begin{bmatrix}\Delta\bm{q}_{1}\\ \Delta\bm{\dot{q}}_{1}\\ \vdots\\ \Delta\bm{q}_{T}\\ \Delta\bm{\dot{q}}_{T}\end{bmatrix}=\underset{\bm{L}_{x}}{\underbrace{\begin{bmatrix}\bm{L}&\bm{0}&\ldots&\bm{0}\\ \bm{0}&\bm{L}&\ldots&\bm{0}\\ \vdots&\vdots&\ddots&\vdots\\ \bm{0}&\bm{0}&\ldots&\bm{L}\end{bmatrix}}}\begin{bmatrix}\Delta\bm{x}_{1}\\ \Delta\bm{\dot{x}}_{1}\\ \vdots\\ \Delta\bm{x}_{T}\\ \Delta\bm{\dot{x}}_{T}\end{bmatrix},\quad\begin{bmatrix}\Delta\bm{u}_{1}\\ \Delta\bm{u}_{2}\\ \vdots\\ \Delta\bm{u}_{T-1}\end{bmatrix}=\underset{\bm{L}_{u}}{\underbrace{\begin{bmatrix}\bm{L}^{-{\scriptscriptstyle\top}}&\bm{0}&\ldots&\bm{0}\\ \bm{0}&\bm{L}^{-{\scriptscriptstyle\top}}&\ldots&\bm{0}\\ \vdots&\vdots&\ddots&\vdots\\ \bm{0}&\bm{0}&\ldots&\bm{L}^{-{\scriptscriptstyle\top}}\end{bmatrix}}}\begin{bmatrix}\Delta\bm{\tau}_{1}\\ \Delta\bm{\tau}_{2}\\ \vdots\\ \Delta\bm{\tau}_{T-1}\end{bmatrix},

so ( 112 ) can be rewritten as

\begin{bmatrix}\Delta\bm{x}_{1}\\ \Delta\bm{\dot{x}}_{1}\\ \vdots\\ \Delta\bm{x}_{T}\\ \Delta\bm{\dot{x}}_{T}\end{bmatrix}=\bm{S}_{\tau}^{x}\begin{bmatrix}\Delta\bm{\tau}_{1}\\ \Delta\bm{\tau}_{2}\\ \vdots\\ \Delta\bm{\tau}_{T-1}\end{bmatrix}=\bm{L}_{x}^{-1}\bm{S}_{u}^{q}\bm{L}_{u}\begin{bmatrix}\Delta\bm{\tau}_{1}\\ \Delta\bm{\tau}_{2}\\ \vdots\\ \Delta\bm{\tau}_{T-1}\end{bmatrix}.