In the previous post, we show how the equations of motion of an \(n\)-DOF open-chain torque-actuated robot manipulator can be derived: \[ \mathbf{M(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)}=\boldsymbol{\tau}_\text{in} \tag{1} \] The notations of the equation are self-explanatory, and we assume there are no external forces applied to the robot. Given the equations of motion, there are some favorable properties for the matrices of the equations of motion
While \(\mathbf{C(q,\dot{q})\dot{q}}\) is unique, \(\mathbf{C(q,\dot{q})}\) itself is not unique. Recall that the Coriolis/centrifugal matrix is derived from the Euler-Lagrange equation. For simplicity, assume that the gravitational force can be neglected. The Lagrangian \(L(\mathbf{q},\mathbf{\dot{q}})\) is simply the kinetic energy of the system: \[ L(\mathbf{q},\mathbf{\dot{q}}) = \frac{1}{2} \sum_{j=1}^{n}\sum_{k=1}^{n} M_{jk}(\mathbf{q}) \dot{q}_j\dot{q}_k \] Using the Euler-Lagrange equation and \(M_{jk}(\mathbf{q})=M_{kj}(\mathbf{q})\), for \(i=1,2,\cdots, n\): \[ \begin{align} \frac{\partial L(\mathbf{q},\mathbf{\dot{q}})}{\partial \dot{q}_i } &= \frac{1}{2} \sum_{k=1}^{n}M_{ik}(\mathbf{q})\dot{q}_k + \frac{1}{2}\sum_{j=1}^{n}M_{ji}(\mathbf{q})\dot{q}_j = \sum_{j=1}^{n}M_{ij}(\mathbf{q})\dot{q}_j\\ \frac{d}{dt} \bigg( \frac{\partial L(\mathbf{q},\mathbf{\dot{q}})}{\partial \dot{q}_i }\bigg) &= \sum_{j=1}^{n}M_{ij}(\mathbf{q})\ddot{q}_j + \sum_{j=1}^{n}\sum_{k=1}^{n}\frac{\partial M_{ij}(\mathbf{q})}{\partial q_k}\dot{q}_j\dot{q}_k \\ \frac{\partial L(\mathbf{q},\mathbf{\dot{q}})}{\partial q_i } &= \frac{1}{2} \sum_{j=1}^{n}\sum_{k=1}^{n} \frac{\partial M_{jk}(\mathbf{q})}{\partial q_i} \dot{q}_j\dot{q}_k \\ \frac{d}{dt} \bigg( \frac{\partial L(\mathbf{q},\mathbf{\dot{q}})}{\partial \dot{q}_i }\bigg) - \frac{\partial L(\mathbf{q},\mathbf{\dot{q}})}{\partial q_i } &= \sum_{j=1}^{n}M_{ij}(\mathbf{q})\ddot{q}_j + \sum_{j=1}^{n}\sum_{k=1}^{n}\frac{\partial M_{ij}(\mathbf{q})}{\partial q_k}\dot{q}_j\dot{q}_k - \frac{1}{2} \sum_{j=1}^{n}\sum_{k=1}^{n} \frac{\partial M_{jk}(\mathbf{q})}{\partial q_i} \dot{q}_j\dot{q}_k \end{align} \]
The \(i\)-th term of \(\mathbf{C(q,\dot{q})\dot{q}}\) is: \[ \begin{align} \sum_{j=1}^{n} C_{ij}\mathbf{(q,\dot{q})} \dot{q}_j &= \sum_{j=1}^{n}\sum_{k=1}^{n}\frac{\partial M_{ij}(\mathbf{q})}{\partial q_k}\dot{q}_j\dot{q}_k - \frac{1}{2} \sum_{j=1}^{n}\sum_{k=1}^{n} \frac{\partial M_{jk}(\mathbf{q})}{\partial q_i} \dot{q}_j\dot{q}_k \equiv \sum_{j=1}^{n}\sum_{k=1}^{n} \Gamma_{ijk}(\mathbf{q})\dot{q}_j\dot{q}_k \end{align} \] Where: \[ \Gamma_{ijk}(\mathbf{q}) =\frac{\partial M_{ij}(\mathbf{q})}{\partial q_k} - \frac{1}{2} \frac{\partial M_{jk}(\mathbf{q})}{\partial q_i} \] It is now clear to show that while \(\mathbf{C(q,\dot{q})\dot{q}}\) is unique, the choice of elements of \(\mathbf{C(q,\dot{q})}\) is not unique. For simplicity, consider the case of \(n=2\). Then the 1st component of \(\mathbf{C(q,\dot{q})\dot{q}}\) is: \[ \begin{align} \sum_{j=1}^{2} C_{1j}\mathbf{(q,\dot{q})} \dot{q}_j &= \Gamma_{111}(\mathbf{q}) \dot{q}_1^2 + \Gamma_{112}(\mathbf{q}) \dot{q}_1\dot{q}_2 + \Gamma_{121}(\mathbf{q}) \dot{q}_1\dot{q}_2 + \Gamma_{122}(\mathbf{q}) \dot{q}_2^2 \\ &= \begin{bmatrix} \Gamma_{111}(\mathbf{q})\dot{q}_1+ \Gamma_{112}(\mathbf{q})\dot{q}_2 & \Gamma_{121}(\mathbf{q})\dot{q}_1+ \Gamma_{122}(\mathbf{q})\dot{q}_2 \\ \end{bmatrix} \begin{bmatrix} \dot{q}_1 \\ \dot{q}_2 \end{bmatrix} \\ &= \begin{bmatrix} \Gamma_{111}(\mathbf{q})\dot{q}_1+ \Gamma_{112}(\mathbf{q})\dot{q}_2 + \Gamma_{121}(\mathbf{q})\dot{q}_2 & \Gamma_{122}(\mathbf{q})\dot{q}_2 \\ \end{bmatrix} \begin{bmatrix} \dot{q}_1 \\ \dot{q}_2 \end{bmatrix} \end{align} \] Thus, it is already immediate that the term associated with \(\Gamma_{121}(\mathbf{q})\) can be moved around and \(C_{ij}(\mathbf{q},\mathbf{\dot{q}})\) is not unique.
Let \(\mathbf{\dot{q}}\) be the
joint velocity of the robot. Then: \[
\forall \mathbf{q},\mathbf{\dot{q}}: \mathbf{\dot{q}^T \Big\{
\dot{M}(q)-2C(q,\dot{q}) \Big\} \dot{q}} =0
\] One should be aware that this is not the
skew-symmetric property, since the array multipled in the front and back
should be \(\mathbf{\dot{q}}\), which
is the argument of the Coriolis/centrifugal matrix \(\mathbf{C(q,\dot{q})}\). This equation is
satisfied for any choices of \(\mathbf{C(q,\dot{q})}\). The proof is
straightforward [1], [2]. The power input of the robot should be the
time derivative of kinetic energy: \[
\mathbf{\dot{q}^T} \{ \boldsymbol{\tau}_{in} -\mathbf{g(q)} \} =
\frac{d}{dt}\bigg( \frac{1}{2}\mathbf{\dot{q}^TM(q)\dot{q}} \bigg)
\] The further derivations are immediate: \[
\require{cancel}
\begin{align}
\mathbf{\dot{q}^T} \{ \boldsymbol{\tau}_{in} -\mathbf{g(q)} \} &=
\mathbf{\dot{q}^T} \{
\mathbf{M(q)\ddot{q}+C(q,\dot{q})\dot{q}+\cancel{g(q)}}
-\cancel{\mathbf{g(q)}} \} = \mathbf{\dot{q}^T} \{
\mathbf{M(q)\ddot{q}+C(q,\dot{q})\dot{q} }\} \\
\frac{d}{dt}\bigg( \frac{1}{2}\mathbf{\dot{q}^TM(q)\dot{q}} \bigg)
&= \mathbf{\dot{q}^TM(q)\ddot{q}} +
\frac{1}{2}\mathbf{\dot{q}^T\dot{M}(q)\dot{q}}
\end{align}
\] \[
\frac{1}{2}\mathbf{\dot{q}^T \{ \dot{M}(q) - 2C(q,\dot{q}) \}
\dot{q}} = 0
\]
While there are multiple choices of \(\mathbf{C(q,\dot{q})}\), for a specific choice of \(\mathbf{C(q,\dot{q})}\), we show that: \[ \forall \mathbf{v} \in\mathbb{R}^{n}: \mathbf{v^T \Big\{ \dot{M}(q)-2C(q,\dot{q}) \Big\} v} =0 \]
In other words, \(\mathbf{\dot{M}(q)-2C(q,\dot{q})}\) is a
skew-symmetric matrix. The proof is also standard [3] [4]. Recall
that: \[
\begin{align}
\sum_{j=1}^{n}C_{ij}(\mathbf{q,\dot{q}})\dot{q}_j =
\sum_{j=1}^{n}\sum_{k=1}^{n}\Gamma_{ijk}(\mathbf{q})\dot{q}_j\dot{q}_k =
\sum_{j=1}^{n}\sum_{k=1}^{n}\Big( \frac{\partial
M_{ij}(\mathbf{q})}{\partial q_k} - \frac{1}{2} \frac{\partial
M_{jk}(\mathbf{q})}{\partial q_i} \Big)\dot{q}_j\dot{q}_k
\end{align}
\] Due to the summation, \(j\)
and \(k\) can be interchanged.
Therefore: \[
\begin{align}
&\sum_{j=1}^{n}\sum_{k=1}^{n}\Big( \frac{\partial
M_{ij}(\mathbf{q})}{\partial q_k} - \frac{1}{2} \frac{\partial
M_{jk}(\mathbf{q})}{\partial q_i} \Big)\dot{q}_j\dot{q}_k =
\sum_{j=1}^{n}\sum_{k=1}^{n}\Big( \frac{1}{2}\frac{\partial
M_{ij}(\mathbf{q})}{\partial q_k} + \frac{1}{2}\frac{\partial
M_{ij}(\mathbf{q})}{\partial q_k} - \frac{1}{2} \frac{\partial
M_{jk}(\mathbf{q})}{\partial q_i} \Big)\dot{q}_j\dot{q}_k \\
=& \sum_{j=1}^{n}\sum_{k=1}^{n}\Big( \frac{1}{2}\frac{\partial
M_{ij}(\mathbf{q})}{\partial q_k} + \frac{1}{2}\frac{\partial
M_{ik}(\mathbf{q})}{\partial q_j} - \frac{1}{2} \frac{\partial
M_{jk}(\mathbf{q})}{\partial q_i} \Big)\dot{q}_j\dot{q}_k
\end{align}
\] Hence, we define \(\Gamma_{ijk}(\mathbf{q})\) as: \[
\Gamma_{ijk}(\mathbf{q}) = \frac{1}{2}\Big( \frac{\partial
M_{ij}(\mathbf{q})}{\partial q_k} +\frac{\partial
M_{ik}(\mathbf{q})}{\partial q_j} - \frac{\partial
M_{jk}(\mathbf{q})}{\partial q_i}\Big)
\]
This is called the Christoffel
symbols of the first type. Note that \(\Gamma_{ijk}(\mathbf{q})=\Gamma_{ikj}(\mathbf{q})\).
With this specific choice of \(\Gamma_{ijk}(\mathbf{q})\), it is clear
that \(\mathbf{\dot{M}(q)-2C(q,\dot{q})}\) is a
skew-symmetric matrix. By definition, the \(ij\)-th matrix component is: \[
\begin{align}
\sum_{k=1}^{n}\frac{\partial M_{ij}(\mathbf{q})}{\partial q_k}
\dot{q}_k - 2 \sum_{k=1}^{n} \Gamma_{ijk}(\mathbf{q}) \dot{q}_k &=
\sum_{k=1}^{n} \Big( \frac{\partial M_{ij}(\mathbf{q})}{\partial
q_k}-\frac{\partial M_{ij}(\mathbf{q})}{\partial q_k} -\frac{\partial
M_{ik}(\mathbf{q})}{\partial q_j} + \frac{\partial
M_{jk}(\mathbf{q})}{\partial q_i} \Big) \dot{q}_k \\
&= \sum_{k=1}^{n} \Big( -\frac{\partial
M_{ik}(\mathbf{q})}{\partial q_j} + \frac{\partial
M_{jk}(\mathbf{q})}{\partial q_i} \Big) \dot{q}_k
\end{align}
\] And it is immediate that the sign of the term changes if we
change \(i\) and \(j\).