Consider the RP manipulator shown in Fig. 1. Its links have masses $m_1, m_2$, local coordinates of their centers of mass $c_1^L, c_2^L$ and local inertias $H_1, H_2$. Since this is a planar problem, inertias are simply scalars. The two coordinates are $q=(\theta,r)$ where $\theta$ is the CCW rotation of the first link relative to the $x$ axis, and $r$ gives the extent of the $P$ manipulator along its local $x$ axis. Let us assume for simplicity that $c_2^L = 0$.

Via forward kinematics, the world-space coordinates of each center of mass are given by: $$c_1(q) = R(\theta) c_1^L$$ $$c_2(q) = R(\theta) \begin{bmatrix}r \\ 0 \end{bmatrix} = \begin{bmatrix}{r \cos \theta}\\{r \sin \theta}\end{bmatrix}.$$ By differentiation, the velocities of each center of mass are $$v_1(q) = \dot{\theta} \begin{bmatrix}0&-1\\1&0\end{bmatrix} c_1(q)$$ and $$v_2(q) = \dot{\theta} \begin{bmatrix}0&-1\\1&0\end{bmatrix} c_2(q) = \begin{bmatrix}{-r \dot{\theta} \sin \theta + \dot{r}\sin \theta} \\ {r \dot{\theta} \cos \theta + \dot{r}\cos \theta} \end{bmatrix}.$$ The angular velocity of each link are $\omega_1 = \omega_2 = \dot{\theta}$.

Overall, the kinetic energy of link 1 is the sum of linear and angular kinetic energy terms: $$\begin{aligned} K_1(q,\dot{q}) &= \frac{1}{2}m_1 \|v_1(q)\|^2 + \frac{1}{2}H_1 \omega_1^2 \\ &= \frac{1}{2}m_1\dot{\theta}^2 \left\| \begin{bmatrix}0&-1\\1&0\end{bmatrix} c_1(q) \right\|^2 + \frac{1}{2} H_1 \dot{\theta}^2 = \frac{1}{2} \dot{\theta}^2 (H_1 + m_1 \| c_1^L \|^2) \end{aligned}$$ where we observe that $\theta$ does not affect the norm of the c.o.m.'s velocity. (This can also be derived from the orthogonality of $R(\theta)$ and skew symmetry of $\begin{bmatrix}0&-1\\1&0\end{bmatrix}$.)

For link 2, we obtain: $$\begin{aligned} K_2(q,\dot{q}) &= \frac{1}{2} m_2 \| v_1(q) \|^2 + \frac{1}{2} \omega_2^2 H_2 \\ &= \frac{1}{2} m_2 ((-r \dot{\theta} \sin \theta + \dot{r}\sin \theta)^2 + (r \dot{\theta} \cos \theta + \dot{r}\cos \theta)^2) + \frac{1}{2} \dot{\theta}^2 H_2. \end{aligned}$$ This can be expanded and simplified using the fact that $\cos^2 x + \sin^2 x = 1$ to obtain $$K_2(q,\dot{q}) = \frac{1}{2} \dot{\theta}^2 (H_2 + m_2 r^2) + \frac{1}{2} m_2 \dot{r}^2.$$

With $K(q,\dot{q}) = K_1(q,\dot{q}) + K_2(q,\dot{q})$ we first calculate the term $\frac{\partial K}{\partial \dot{q}}$: $$\frac{\partial K}{\partial \dot{q}} = \begin{bmatrix}{\frac{\partial K}{\partial \dot{\theta}}}\\{\frac{\partial K}{\partial \dot{r}}}\end{bmatrix} = \begin{bmatrix}{\dot{\theta}(H_1 + m_1\|c_1^L\|^2 + H_2 + m_2 r^2)}\\{m_2 \dot{r}}\end{bmatrix}.$$ Now, let us define the constant $\bar{H} = H_1 + m_1\|c_1^L\|^2 + H_2$, and take the derivative with respect to time to obtain the term $\frac{d}{dt}(\frac{\partial K}{\partial \dot{q}})$ in the Lagrangian: $$\frac{d}{dt}(\frac{\partial K}{\partial \dot{q}}) = \begin{bmatrix}{\ddot{\theta}(\bar{H} + m_2 r^2) + 2 \dot{r} \dot{\theta} m_2 r}\\{m_2 \ddot{r}}\end{bmatrix}$$ where we must apply the product rule to the term proportional to $\dot{\theta} r^2$.

Proceeding to the $\frac{\partial K}{\partial q}$ term, we see no dependence of $K_1$ on $q$, but a dependence of $K_2$ on $r$ to obtain $$\frac{\partial K}{\partial q} = \begin{bmatrix}{0}\\{m_2 \dot{\theta}^2 r}\end{bmatrix}.$$

The last term in the Lagrangian dynamics equation is the derivative of potential energy $P(q) = mgh = mgr\sin \theta$: $$\frac{\partial P}{\partial q} = mg \begin{bmatrix}{r \cos \theta}\\{\sin \theta}\end{bmatrix}.$$ Overall, we have the dynamic equations: $$\begin{aligned} (\bar{h}+m_2 r^2)\ddot{\theta} + 2 \dot{r}\dot{\theta} m_2 r + m g r \cos \theta &= u_1 \\ m_2 \ddot{r} - m_2 \dot{\theta}^2 r + mg \sin \theta &= u_2 \end{aligned}$$ where $u_1$ is the torque on the first link, and $u_2$ is the force on the second.

Let us now determine the standard form of the dynamics. First, we can determine the $B$ matrix by matching terms that depend on $\ddot{q}$: $$B(q)\ddot{q} = \begin{bmatrix}{\ddot{\theta}(\bar{H}+m_2r^2)}\\{m_2 \ddot{r}}\end{bmatrix} = \begin{bmatrix}{\bar{H}+m_2r^2}&{0}\\{0}&{m_2} \end{bmatrix}\ddot{q}.$$ The velocity-dependent terms go into $C$: $$C(q,\dot{q}) = \begin{bmatrix}{2 m_2 r \dot{r} \dot{\theta}}\\{-m_2 r \dot{\theta}^2}\end{bmatrix}$$ Finally, the generalized gravity vector $G$ contains all other terms: $$G(q) = mg \begin{bmatrix}{r \cos \theta}\\{\sin \theta}\end{bmatrix}.$$

In many applications we may wish to convert between joint space dynamics and workspace dynamics. This concept of effective inertial lets us answer questions like or "How much force would the robot apply to an object at point $p$?" or "How much external force is needed to accelerate a point $p$ on the robot at a given rate?".

Let us assume that without any external force, at the robot's current state $(q,\dot{q})$ and external torques $u_0$ generates accelerations $\ddot{q}_{0}$: $$\ddot{q}_0 = B^{-1}(q)( u_{0} - C(q,\dot{q}) - G(q))$$ We now wish to calculate what accelerations $\ddot{q}$ would occur a force $f$ were applied at this point in time at point $p$. Supposing $J(q)$ is the Jacobian of $p$, the external torques become $u = u_0 + J^T(q)f$. Hence, we see that: $$\ddot{q} = B^{-1}(q)( u - C(q,\dot{q}) - G(q)) = B^{-1}(q)( u_0 + J^T(q)f - C(q,\dot{q}) - G(q)) = B^{-1}(q)J^T(q)f + \ddot{q}_0$$ Hence, the change in joint accelerations $\ddot{q}-\ddot{q}_0$ caused by the force is $B^{-1}(q)J^T(q)f$.

Now, we may wish to ask what the change in the *point's* acceleration
is. Via the Jacobian, we have $$\dot{p} = J(q)\dot{q}$$ giving
$$\ddot{p} = \dot{J}(q)\dot{q} + J(q)\ddot{q}.$$ Compared to the
un-forced acceleration of $p$
$$\ddot{p}_0 = \dot{J}(q)\dot{q} + J(q)\ddot{q}_0$$ we have
$$\ddot{p} - \ddot{p}_0 = J(q)\ddot{q} - J(q)\ddot{q}_0 = J(q)B^{-1}(q)J^T(q)f.$$
Another way of saying this, which matches the form of Newton's second law
$f=ma$, is $$f = (J(q)B^{-1}(q)J^T(q))^{-1}(\ddot{p} - \ddot{p}_0).$$
Hence, the matrix $(J(q)B^{-1}(q)J^T(q))^{-1}$ is known as the
*effective inertia matrix* at point $p$.

Unlike mass matrices, the effective inertia can actually be infinite when the robot is at a singularity. Although $B^{-1}$ is always full rank, singularities in $J$ may cause $J(q)B^{-1}(q)J^T(q)$ to drop rank, making it non-invertible. The ultimate effect is that in some directions, there may be no force that can accelerate the robot. Similarly, if the manipulability of the Jacobian is small, a huge force may be needed to cause a small acceleration.

In some cases this can be dangerous, such as when robots handle delicate objects or are supposed to be human-safe. Like a nutcracker, large forces can be exerted at pinch points near the robot's joints. This can be observed in the effective inertia matrix, since its entries grow larger as $p$ approaches a joint fixed to the world. In other cases, large effective inertia can be exploited, such as when a robot must handle a heavy load. Indeed, humans exploit large effective inertia while walking, keeping knees straight to avoid additional exertion. Similarly, when hanging by the hands or lifting large objects, it is more energetically efficient to keep the elbows straight.

AKA Recursive Newton-Euler

Compare complexity

Klampt, DART implementations

In [ ]:

```
```