Dynamics (Lagrangian formulation)

Dynamics (Lagrangian formulation)#

Compared to the previous kinematics concerning how a robot moves, robot dynamics concerns about why a robot moves. Dynamics equation establishes a relationship between the forces/torques and their acceleration/velocities/positions.

Lagrange method provides a systematic way to derive dynamics equation of a mechanical system. Suppose that the configuration space of a \(n\)-DOF mechanical system is described by a set of independent variables \(q_{i}, i=1, \ldots, n\), named generalized coordinates, the Lagrangian of the system is defined as

\[\mathcal{L}=\mathcal{T}-\mathcal{U}\]

where \(\mathcal{T}\) and \(\mathcal{U}\) are the kinetic energy and potential energy of the system, respectively. The dynamics from the Lagrangian is

(27)#\[\frac{d}{d t} \frac{\partial \mathcal{L}}{\partial \dot{q}_{i}}-\frac{\partial \mathcal{L}}{\partial q_{i}}=\xi_{i} \quad i=1, \ldots, n\]

where \(\xi_{i}\) is the generalized force associated with \(q_{i}\) (the generalized force \(\xi_i\) is dual to \(q_i\) in the sense that \(\xi_i*q_i\) generate power). In compact form, the above equation can be written as

(28)#\[\frac{d}{d t}\left(\frac{\partial \mathcal{L}}{\partial \dot{\boldsymbol{q}}}\right)^{T}-\left(\frac{\partial \mathcal{L}}{\partial \boldsymbol{q}}\right)^{T}=\boldsymbol{\xi}\]

For an open-chain robot arm, the generalized coordinates are joint variables \(\boldsymbol{q}\). The generalized forces are the net torque/force at each joint.

Kinetic Energy of a Robot Arm#

Consider a \(n\)-DoF robot arm. The kinetic energy is due to the motion of each link. The total kinetic energy is given by

\[\mathcal{T}=\sum_{i=1}^{n}\mathcal{T}_{\ell_{i}}\]

where \(\mathcal{T}_{\ell_{i}}\) is the kinetic energy of Link \(i\).

../_images/linki_kinematics.jpg

Fig. 67 Motion of Link \(i\)#

As shown in Fig. 67, the kinetic energy contribution of Link \(i\) is given by

(29)#\[ \mathcal{T}_{\ell_{i}}=\frac{1}{2} \int_{V_{\ell_{i}}} \dot{\boldsymbol{p}}_{i}^{* T} \dot{\boldsymbol{p}}_{i}^{*} \rho d V\]

where \(\rho\) is the density of the elementary particle of volume \(d V\); \(\dot{\boldsymbol{p}}_{i}^{*}\) denotes the linear velocity vector of the elementary particle; and \(V_{\ell_{i}}\) is the volume of Link \(i\). Here, the position \(\boldsymbol{p}_{i}^{*}\) of the elementary particle is expressed in the base frame.

The position \(\boldsymbol{p}_{l_{i}}\) of the link center of mass (COM) defined as

\[\boldsymbol{p}_{\ell_{i}}=\frac{1}{m_{\ell_{i}}} \int_{V_{\ell_{i}}} \boldsymbol{p}_{i}^{*} \rho d V\]

where \(m_{\ell_{i}}\) is the mass of link \(i\). We also define

\[\boldsymbol{r}_{i}=\left[\begin{array}{lll} r_{i x} & r_{i y} & r_{i z} \end{array}\right]^{T}=\boldsymbol{p}_{i}^{*}-\boldsymbol{p}_{\ell_{i}}\]

As a result, the velocity of an elementary particle of link \(i\) can be expressed as

(30)#\[\label{equ.2} \begin{aligned} \dot{\boldsymbol{p}}_{i}^{*} & =\dot{\boldsymbol{p}}_{\ell_{i}}+\boldsymbol{\omega}_{i} \times \boldsymbol{r}_{i} =\dot{\boldsymbol{p}}_{\ell_{i}}+\boldsymbol{S}\left(\boldsymbol{\omega}_{i}\right) \boldsymbol{r}_{i} \end{aligned}\]

where \(\dot{\boldsymbol{p}}_{\ell_{i}}\) is the linear velocity of COM and \(\boldsymbol{\omega}_{i}\) is the angular velocity of the link.

By substituting (30) into (29), it leads to that the total kinetic energy of link \(i\), \( \mathcal{T}_{\ell_{i}}\), is the sumation of the following three terms

Translational term

(31)#\[\frac{1}{2} \int_{V_{\ell_{i}}} \dot{\boldsymbol{p}}_{\ell_{i}}^{T} \dot{\boldsymbol{p}}_{\ell_{i}} \rho d V=\frac{1}{2} m_{\ell_{i}} \dot{\boldsymbol{p}}_{\ell_{i}}^{T} \dot{\boldsymbol{p}}_{\ell_{i}}\]

Cross term

(32)#\[2\left(\frac{1}{2} \int_{V_{\ell_{i}}} \dot{\boldsymbol{p}}_{\ell_{i}}^{T} \boldsymbol{S}\left(\boldsymbol{\omega}_{i}\right) \boldsymbol{r}_{i} \rho d V\right)=2\left(\frac{1}{2} \dot{\boldsymbol{p}}_{\ell_{i}}^{T} \boldsymbol{S}\left(\boldsymbol{\omega}_{i}\right) \int_{V_{\ell_{i}}}\left(\boldsymbol{p}_{i}^{*}-\boldsymbol{p}_{\ell_{i}}\right) \rho d V\right)=0\]

Rotational term

(33)#\[\frac{1}{2} \int_{V_{\ell_{i}}} \boldsymbol{r}_{i}^{T} \boldsymbol{S}^{T}\left(\boldsymbol{\omega}_{i}\right) \boldsymbol{S}\left(\boldsymbol{\omega}_{i}\right) \boldsymbol{r}_{i} \rho d V=\frac{1}{2} \boldsymbol{\omega}_{i}^{T}\left(\int_{V_{\ell_{i}}} \boldsymbol{S}^{T}\left(\boldsymbol{r}_{i}\right) \boldsymbol{S}\left(\boldsymbol{r}_{i}\right) \rho d V\right) \boldsymbol{\omega}_{i}=\frac{1}{2} \boldsymbol{\omega}_{i}^{T} \boldsymbol{I}_{\ell_{i}} \boldsymbol{\omega}_{i}\]

Recall that

\[\begin{split}\boldsymbol{S}\left(\boldsymbol{r}_{i}\right)=\left[\begin{array}{ccc} 0 & -r_{i z} & r_{i y} \\ r_{i z} & 0 & -r_{i x} \\ -r_{i y} & r_{i x} & 0 \end{array}\right]\end{split}\]

Then,

\[\begin{split}\begin{aligned} \boldsymbol{I}_{\ell_{i}} & =\left[\begin{array}{ccc} \int\left(r_{i y}^{2}+r_{i z}^{2}\right) \rho d V & -\int r_{i x} r_{i y} \rho d V & -\int r_{i x} r_{i z} \rho d V \\ * & \int\left(r_{i x}^{2}+r_{i z}^{2}\right) \rho d V & -\int r_{i y} r_{i z} \rho d V \\ * & * & \int\left(r_{i x}^{2}+r_{i y}^{2}\right) \rho d V \end{array}\right] \end{aligned}\end{split}\]

is the inertia tensor relative to the COM of Link \(i\). Since \(\boldsymbol{r}_i\) is expressed in the base frame, this inertia tensor is thus also expressed in the base frame, thus is configuration-dependent.

Let’s consider \(\boldsymbol{r}_i^i\) be expressed in the frame of link \(i\)

\[\boldsymbol{r}_{i}=\boldsymbol{R}_{i} \boldsymbol{r}_{i}^{i}\]

where \(\boldsymbol{R}_{i}\) is the rotation matrix from Link \(i\) frame to the base frame. Then, from (33), we have

\[\begin{split} \begin{aligned} &\frac{1}{2} \boldsymbol{\omega}_{i}^{T}\left(\int_{V_{\ell_{i}}} \boldsymbol{S}^{T}\left(\boldsymbol{R}_{i} \boldsymbol{r}_{i}^{i}\right) \boldsymbol{S}\left(\boldsymbol{R}_{i} \boldsymbol{r}_{i}^{i}\right) \rho d V\right) \boldsymbol{\omega}_{i}\\ =& \frac{1}{2} \boldsymbol{\omega}_{i}^{T}\left(\int_{V_{\ell_{i}}} \boldsymbol{R}_{i} \boldsymbol{S}^{T}\left(\boldsymbol{r}_{i}^{i}\right) \boldsymbol{R}_{i} ^T \boldsymbol{R}_{i} \boldsymbol{S}\left( \boldsymbol{r}_{i}^{i}\right) \boldsymbol{R}_{i}^T \rho d V\right) \boldsymbol{\omega}_{i}\\ =& \frac{1}{2} \boldsymbol{\omega}_{i}^{T} \boldsymbol{R}_{i} \left(\int_{V_{\ell_{i}}} \boldsymbol{S}^{T}\left(\boldsymbol{r}_{i}^{i}\right) \boldsymbol{S}\left( \boldsymbol{r}_{i}^{i}\right) \rho d V\right) \boldsymbol{R}_{i}^T\boldsymbol{\omega}_{i}\\ =& \frac{1}{2} \boldsymbol{\omega}_{i}^{T} \boldsymbol{R}_{i} \boldsymbol{I}_{\ell_{i}}^{i} \boldsymbol{R}_{i}^{T}\boldsymbol{\omega}_{i} \end{aligned} \end{split}\]

Here,

\[ \boldsymbol{I}_{\ell_{i}}^{i} =\left(\int_{V_{\ell_{i}}} \boldsymbol{S}^{T}\left(\boldsymbol{r}_{i}^{i}\right) \boldsymbol{S}\left( \boldsymbol{r}_{i}^{i}\right) \rho d V\right) \]

is expressed in the body frame (link \(i\) frame), and thus is configuration-independent.

By summing the translational and rotational terms, the total kinetic energy in (29) of link \(i\) is

(34)#\[\mathcal{T}_{\ell_{i}}=\frac{1}{2} m_{\ell_{i}} \dot{\boldsymbol{p}}_{\ell_{i}}^{T} \dot{\boldsymbol{p}}_{\ell_{i}}+\frac{1}{2} \boldsymbol{\omega}_{i}^{T} \boldsymbol{R}_{i} \boldsymbol{I}_{\ell_{i}}^{i} \boldsymbol{R}_{i}^{T} \boldsymbol{\omega}_{i}\]

Now, let’s find out the linear and angular velocities of Link \(i\) using Jacobian! Using our previous method to find Jacobian up to link \(i\), we have

\[\begin{split}\begin{aligned} \dot{\boldsymbol{p}}_{\ell_{i}} & =\boldsymbol{\jmath}_{P 1}^{\left(\ell_{i}\right)} \dot{q}_{1}+\ldots+\boldsymbol{J}_{P i}^{\left(\ell_{i}\right)} \dot{q}_{i}=\boldsymbol{J}_{P}^{\left(\ell_{i}\right)} \dot{\boldsymbol{q}} \\ \boldsymbol{\omega}_{i} & =\boldsymbol{\jmath}_{O 1}^{\left(\ell_{i}\right)} \dot{q}_{1}+\ldots+\boldsymbol{J}_{O i}^{\left(\ell_{i}\right)} \dot{q}_{i}=\boldsymbol{J}_{O}^{\left(\ell_{i}\right)} \dot{\boldsymbol{q}}, \end{aligned}\end{split}\]

where (think about why the columns after \(i\) is zeros?)

(35)#\[\begin{aligned} \boldsymbol{J}_{P}^{\left(\ell_{i}\right)} =\left[\begin{array}{llllll} \boldsymbol{J}_{P 1}^{\left(\ell_{i}\right)} & \ldots & \boldsymbol{J}_{P i}^{\left(\ell_{i}\right)} & \mathbf{0} & \ldots & \mathbf{0} \end{array}\right] \qquad \boldsymbol{J}_{O}^{\left(\ell_{i}\right)} =\left[\begin{array}{llllll} \boldsymbol{J}_{O 1}^{\left(\ell_{i}\right)} & \ldots & \boldsymbol{J}_{O i}^{\left(\ell_{i}\right)} & \mathbf{0} & \ldots & \mathbf{0} \end{array}\right] ; \end{aligned}\]

where

\[\begin{split}\begin{gathered} \boldsymbol{J}_{P j}^{\left(\ell_{i}\right)}= \begin{cases}\boldsymbol{z}_{j-1} & \text { for a prismatic joint } \\ \boldsymbol{z}_{j-1} \times\left(\boldsymbol{p}_{\ell_{i}}-\boldsymbol{p}_{j-1}\right) & \text { for a revolute joint }\end{cases} \qquad \boldsymbol{\jmath}_{O j}^{\left(\ell_{i}\right)}= \begin{cases}\mathbf{0} & \text { for a prismatic joint } \\ \boldsymbol{z}_{j-1} & \text { for a revolute joint. }\end{cases} \end{gathered}\end{split}\]

where \(\boldsymbol{p}_{j-1}\) is the position of the origin of Frame \(j-1\) and \(\boldsymbol{z}_{j-1}\) is the unit vector of axis \(z\) of Frame \(j-1\).

It follows that the kinetic energy of Link \(i\) in (34) can be written as

\[\mathcal{T}_{\ell_{i}}=\frac{1}{2} m_{\ell_{i}} \dot{\boldsymbol{q}}^{T} \boldsymbol{J}_{P}^{\left(\ell_{i}\right) T} \boldsymbol{J}_{P}^{\left(\ell_{i}\right)} \dot{\boldsymbol{q}}+\frac{1}{2} \dot{\boldsymbol{q}}^{T} \boldsymbol{J}_{O}^{\left(\ell_{i}\right) T} \boldsymbol{R}_{i} \boldsymbol{I}_{\ell_{i}}^{i} \boldsymbol{R}_{i}^{T} \boldsymbol{J}_{O}^{\left(\ell_{i}\right)} \dot{\boldsymbol{q}}\]

Important

Finally, by summing the kinetic energies of all Links, the total kinetic energy of a robot arm is

(36)#\[\mathcal{T}=\frac{1}{2} \dot{\boldsymbol{q}}^{T} \boldsymbol{B}(\boldsymbol{q}) \dot{\boldsymbol{q}}=\frac{1}{2} \sum_{i=1}^{n} \sum_{j=1}^{n} b_{i j}(\boldsymbol{q}) \dot{q}_{i} \dot{q}_{j}\]

where

(37)#\[\begin{aligned} \boldsymbol{B}(\boldsymbol{q})=\sum_{i=1}^{n}\left(m_{\ell_{i}}\right. & \boldsymbol{J}_{P}^{\left(\ell_{i}\right) T} \boldsymbol{J}_{P}^{\left(\ell_{i}\right)}+\boldsymbol{J}_{O}^{\left(\ell_{i}\right) T} \boldsymbol{R}_{i} \boldsymbol{I}_{\ell_{i}}^{i} \boldsymbol{R}_{i}^{T} \boldsymbol{J}_{O}^{\left(\ell_{i}\right)}\left.\right) \end{aligned}\]

is the \((n \times n)\) inertia matrix which is symmetric, positive definite, and configuration - dependent.




Potential Energy of a Robot Arm#

The potential energy of a robot arm is given by the sum of the contributions of each link:

\[\mathcal{U}=\sum_{i=1}^{n}\mathcal{U}_{\ell_{i}}\]

The potential energy of Link \(i\) is

\[\mathcal{U}_{\ell_{i}}=-\int_{V_{\ell_{i}}} \boldsymbol{g}^{T} \boldsymbol{p}_{i}^{*} \rho d V=-m_{\ell_{i}} \boldsymbol{g}^{T} \boldsymbol{p}_{\ell_{i}}\]

where \(\boldsymbol{g}\) is the gravity acceleration vector in the base frame (e.g., \(\boldsymbol{g}=\) \(\left[\begin{array}{lll}0 & 0 & -g\end{array}\right]^{T}\) if \(z\) is the vertical axis), and \(\boldsymbol{p}_{\ell_{i}}\) the center of mass of Link \(i\).

Important

The total potential energy of the robot arm is

(38)#\[\mathcal{U}=-\sum_{i=1}^{n}m_{\ell_{i}} \boldsymbol{g}^{T} \boldsymbol{p}_{\ell_{i}}\]

which shows that potential energy is only a function of the joint variable \({\boldsymbol{q}}\) because \(\boldsymbol{p}_{\ell_{i}}\) is a function of \(\boldsymbol{q}\),




Dynamics Equation (Different Forms)#

Vector Form#

Having computed the total kinetic energy \(\mathcal{T}(\boldsymbol{q}, \dot{\boldsymbol{q}})\) in (36) and potential energy \(\mathcal{U}(\boldsymbol{q})\) in (38) of the robot arm, the Lagrangian of the system is

\[\mathcal{L}(\boldsymbol{q}, \dot{\boldsymbol{q}})=\mathcal{T}(\boldsymbol{q}, \dot{\boldsymbol{q}})-\mathcal{U}(\boldsymbol{q})\]

Important

By applying the vector form of Lagrange formulation (28), we have the vector form of the dynamics equation:

(39)#\[\boldsymbol{B}(\boldsymbol{q}) \ddot{\boldsymbol{q}}+\boldsymbol{n}(\boldsymbol{q}, \dot{\boldsymbol{q}})=\boldsymbol{\xi}\]

where

\[\boldsymbol{n}(\boldsymbol{q}, \dot{\boldsymbol{q}})=\dot{\boldsymbol{B}}(\boldsymbol{q}) \dot{\boldsymbol{q}}-\frac{1}{2}\left(\frac{\partial}{\partial \boldsymbol{q}}\left(\dot{\boldsymbol{q}}^{T} \boldsymbol{B}(\boldsymbol{q}) \dot{\boldsymbol{q}}\right)\right)^{T}+\left(\frac{\partial \mathcal{U}(\boldsymbol{q})}{\partial \boldsymbol{q}}\right)^{T}\]

Detailed Form#

Below, let’s also apply the detailed form of Lagrange formulation (27). Notice that \(\mathcal{U}\) does not depend on \(\dot{\boldsymbol{q}}\) and

\[\begin{aligned} \frac{d}{d t}\left(\frac{\partial \mathcal{L}}{\partial \dot{q}_{i}}\right)=\frac{d}{d t}\left(\frac{\partial \mathcal{T}}{\partial \dot{q}_{i}}\right) & =\sum_{j=1}^{n} b_{i j}(\boldsymbol{q}) \ddot{q}_{j}+\sum_{j=1}^{n} \frac{d b_{i j}(\boldsymbol{q})}{d t} \dot{q}_{j} =\sum_{j=1}^{n} b_{i j}(\boldsymbol{q}) \ddot{q}_{j}+\sum_{j=1}^{n} \sum_{k=1}^{n} \frac{\partial b_{i j}(\boldsymbol{q})}{\partial q_{k}} \dot{q}_{k} \dot{q}_{j} \end{aligned}\]

and

\[\frac{\partial \mathcal{T}}{\partial q_{i}}=\frac{1}{2} \sum_{j=1}^{n} \sum_{k=1}^{n} \frac{\partial b_{j k}(\boldsymbol{q})}{\partial q_{i}} \dot{q}_{k} \dot{q}_{j}\]

Further,

\[\begin{aligned} \frac{\partial \mathcal{U}}{\partial q_{i}} & =-\sum_{j=1}^{n}m_{\ell_{j}} \boldsymbol{g}^{T} \frac{\partial \boldsymbol{p}_{\ell_{j}}}{\partial q_{i}} =-\sum_{j=1}^{n}m_{\ell_{j}} \boldsymbol{g}^{T} \boldsymbol{J}_{P i}^{\left(\ell_{j}\right)}(\boldsymbol{q})=g_{i}(\boldsymbol{q}) \end{aligned}\]

Important

By applying the detailed form of Lagrange formulation (27), the detailed form of the dynamics equation is

(40)#\[\sum_{j=1}^{n} b_{i j}(\boldsymbol{q}) \ddot{q}_{j}+\sum_{j=1}^{n} \sum_{k=1}^{n} h_{i j k}(\boldsymbol{q}) \dot{q}_{k} \dot{q}_{j}+g_{i}(\boldsymbol{q})=\xi_{i} \quad i=1, \ldots, n .\]

where

(41)#\[h_{i j k}=\frac{\partial b_{i j}}{\partial q_{k}}-\frac{1}{2} \frac{\partial b_{j k}}{\partial q_{i}}\]

and

(42)#\[ g_{i}(\boldsymbol{q}) =-\sum_{j=1}^{n}m_{\ell_{j}} \boldsymbol{g}^{T} \boldsymbol{J}_{P i}^{\left(\ell_{j}\right)}(\boldsymbol{q}) \]

A physical interpretation to each term in (40)

  • Acceleration term \(\sum_{j=1}^{n} b_{i j}(\boldsymbol{q}) \ddot{q}_{j}\): the diagonal coefficient \(b_{i i}\) represents the moment of inertia at Joint \(i\) axis, and off-diagonal coefficient \(b_{i j}\) accounts for the coupling effect of acceleration of Joint \(i\) on Joint \(j\).

  • For the quadratic velocity term \(\sum_{j=1}^{n} \sum_{k=1}^{n} h_{i j k}(\boldsymbol{q}) \dot{q}_{k} \dot{q}_{j}\): the term \(h_{i j j} \dot{q}_{j}^{2}\) is the centrifugal effect to Joint \(i\) by Joint \(j\). The term \(h_{i j k} \dot{q}_{j} \dot{q}_{k}\) represents the Coriolis effect on Joint \(i\) by Joints \(j\) and \(k\).

  • For the configuration-dependent term \(g_{i}(\boldsymbol{q})\): the term \(g_{i}\) represents the moment generated at Joint \(i\) axis by the gravity.

Christoffel-Symbols Form#

In may textbooks/papers, there is another commonly-used form of dynamics equation derived from (40). We introduce below. The quadratic velocity term in (40) has

\[\begin{split}\begin{aligned} \sum_{j=1}^{n} \sum_{k=1}^{n}h_{ijk}\dot{q}_{k} \dot{q}_{j}&= \sum_{j=1}^{n} \sum_{k=1}^{n}\Big(\frac{\partial b_{i j}}{\partial q_{k}}-\frac{1}{2} \frac{\partial b_{j k}}{\partial q_{i}}\Big)\dot{q}_{k} \dot{q}_{j}\\ &=\sum_{j=1}^{n} \sum_{k=1}^{n}\Big(\frac{1}{2}\frac{\partial b_{i j}}{\partial q_{k}}+\frac{1}{2}\frac{\partial b_{i j}}{\partial q_{k}}-\frac{1}{2} \frac{\partial b_{j k}}{\partial q_{i}}\Big)\dot{q}_{k} \dot{q}_{j}\\ &=\sum_{j=1}^{n} \sum_{k=1}^{n}\underbrace{\Big(\frac{1}{2}\frac{\partial b_{i j}}{\partial q_{k}}+\frac{1}{2}\frac{\partial b_{i k}}{\partial q_{j}}-\frac{1}{2} \frac{\partial b_{j k}}{\partial q_{i}}\Big)}_{c_{ijk}}\dot{q}_{k} \dot{q}_{j} \end{aligned}\end{split}\]

Thus, we define

\[c_{ijk}=\frac{1}{2}\Big(\frac{\partial b_{i j}}{\partial q_{k}}+\frac{\partial b_{i k}}{\partial q_{j}}- \frac{\partial b_{j k}}{\partial q_{i}}\Big)\]

which is called Christoffel-symbols. Then, the dynamics equation in (40) becomes

(43)#\[\begin{split} \begin{aligned} \sum_{j=1}^{n} b_{i j}(\boldsymbol{q}) \ddot{q}_{j}+\sum_{j=1}^{n} \sum_{k=1}^{n} h_{i j k}(\boldsymbol{q}) \dot{q}_{k} \dot{q}_{j}+g_{i}(\boldsymbol{q})&= \sum_{j=1}^{n} b_{i j}(\boldsymbol{q}) \ddot{q}_{j}+\sum_{j=1}^{n} \sum_{k=1}^{n} c_{i j k}(\boldsymbol{q}) \dot{q}_{k} \dot{q}_{j}+g_{i}(\boldsymbol{q})\\ &=\sum_{j=1}^{n} b_{i j}(\boldsymbol{q}) \ddot{q}_{j}+\sum_{j=1}^{n} \Big(\sum_{k=1}^{n}c_{ijk}\dot{q}_{k}\Big) \dot{q}_{j}+g_{i}(\boldsymbol{q})\\ &=\sum_{j=1}^{n} b_{i j}(\boldsymbol{q}) \ddot{q}_{j}+\sum_{j=1}^{n} C_{ij} \dot{q}_{j}+g_{i}(\boldsymbol{q})\\ &=\xi_{i} \quad i=1, \ldots, n . \end{aligned} \end{split}\]

with

\[C_{ij}=\sum_{k=1}^{n}c_{ijk}\dot{q}_{k}\]

Important

By writing (43) into a compact vector form, we have a new form of dynamics equation

(44)#\[\boldsymbol{B}(\boldsymbol{q}) \ddot{\boldsymbol{q}}+\boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}}) \dot{\boldsymbol{q}}+\boldsymbol{g}(\boldsymbol{q})={\boldsymbol{\xi}}\]

where \(\boldsymbol{C}\) is a \((n \times n)\) matrix such that its elements \(C_{i j}\) is calculated by

(45)#\[C_{ij}=\sum_{k=1}^{n}c_{ijk}\dot{q}_{k}\]

with

(46)#\[c_{ijk}=\frac{1}{2}\Big(\frac{\partial b_{i j}}{\partial q_{k}}+\frac{\partial b_{i k}}{\partial q_{j}}- \frac{\partial b_{j k}}{\partial q_{i}}\Big)\]

Generalized forces/torques#

The generalized force \(\boldsymbol{\xi}\) applied on the right side of (39) or (40) includes

\[\boldsymbol{\xi}=\underbrace{\boldsymbol{\tau}}_{\text{motor torque}}-\underbrace{\boldsymbol{F}_{v} \dot{\boldsymbol{q}}}_{\text{viscous friction torques}}-\underbrace{\boldsymbol{F}_{s} \operatorname{sgn}(\dot{\boldsymbol{q}})}_{\text{Coulomb friction torques}}-\underbrace{\boldsymbol{J}^{T}(\boldsymbol{q}) \boldsymbol{h}_{e}}_{\text{torques by contact forces.}}\]

where \(\boldsymbol{F}_{v}\) denotes the \((n \times n)\) diagonal matrix of viscous friction coefficients; \(\boldsymbol{F}_{s}\) is an \((n \times n)\) diagonal matrix and \(\operatorname{sgn}(\dot{\boldsymbol{q}})\) denotes the \((n \times 1)\) vector whose components are given by the sign functions of the single joint velocities; and \(\boldsymbol{h}_{e}\) denotes the vector of force and moment exerted by the end-effector on the environment.



Skew-symmetry of \(\dot{\boldsymbol{B}}-2\boldsymbol{C}\)#

The idea: the total time derivative of kinetic energy of a robot arm equals the power generated by all the forces/torques including the gravity.

The time derivative of the kinetic energy:

(47)#\[\label{equ.diff_kinenergy} \frac{d \mathcal{T}}{dt }=\frac{1}{2}\frac{d}{dt}\left( \dot{\boldsymbol{q}}^{T} \boldsymbol{B}(\boldsymbol{q})\dot{\boldsymbol{q}} \right)=\dot{\boldsymbol{q}}^{T}\boldsymbol{B}(\boldsymbol{q})\ddot{\boldsymbol{q}}+\frac{1}{2}\left( \dot{\boldsymbol{q}}^{T} \dot{\boldsymbol{B}}(\boldsymbol{q})\dot{\boldsymbol{q}} \right)\]

The power generated by all (generalized) external forces including gravity:

(48)#\[\label{equ.power_forces} \dot{\boldsymbol{q}}^{T}(-\boldsymbol{F}_v{\dot{\boldsymbol{q}}}-\boldsymbol{F}_s\text{sgn}({\dot{\boldsymbol{q}}})-\boldsymbol{g}(\boldsymbol{q})+\boldsymbol{\tau}-\boldsymbol{J}^T(\boldsymbol{q})\boldsymbol{h}_{e})\]

Equalling (48) to (47), we have

(49)#\[ \dot{\boldsymbol{q}}^{T}\boldsymbol{B}(\boldsymbol{q})\ddot{\boldsymbol{q}}+\frac{1}{2}\left( \dot{\boldsymbol{q}}^{T} \dot{\boldsymbol{B}}(\boldsymbol{q})\dot{\boldsymbol{q}} \right)=\dot{\boldsymbol{q}}^{T}(-\boldsymbol{F}_v{\dot{\boldsymbol{q}}}-\boldsymbol{F}_s\text{sgn}({\dot{\boldsymbol{q}}})-\boldsymbol{g}(\boldsymbol{q})+\boldsymbol{\tau}-\boldsymbol{J}^T(\boldsymbol{q})\boldsymbol{h}_{e})\]

Recall the dynamics equation:

(50)#\[\boldsymbol{B}(\boldsymbol{q}) \ddot{\boldsymbol{q}}+\boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}}) \dot{\boldsymbol{q}}+\boldsymbol{g}(\boldsymbol{q})=\boldsymbol{\tau}-\boldsymbol{J}^{T}(\boldsymbol{q}) \boldsymbol{h}_{e}-\boldsymbol{F}_{v} \dot{\boldsymbol{q}}-\boldsymbol{F}_{s} \operatorname{sgn}(\dot{\boldsymbol{q}}) \]

We multiply \(\dot{\boldsymbol{q}}^T\) on both sides of (50) and subtract (50) from (49) on both sides. This yields

\[\frac{1}{2} \dot{\boldsymbol{q}}^{T} \boldsymbol{B}(\boldsymbol{q})\dot{\boldsymbol{q}}-\dot{\boldsymbol{q}}^T\boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}}) \dot{\boldsymbol{q}}=\frac{1}{2} \dot{\boldsymbol{q}}^{T} \big( \boldsymbol{B}(\boldsymbol{q})\dot{\boldsymbol{q}}-2\boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}})\big) \dot{\boldsymbol{q}}=\boldsymbol{0}\]

which holds for any \(\dot{\boldsymbol{q}}\). It means that \(\boldsymbol{B}(\boldsymbol{q})\dot{\boldsymbol{q}}-2\boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}})\) is a skew-symmetric matrix.