# Multibody dynamics of simple biomechanical models¶

Marcos Duarte

The human body is composed of multiple interconnected segments (which can be modeled as rigid or flexible) and each segment may have translational and rotational movement. The part of mechanics for the study of movement and forces of interconnected bodies is called multibody system or multibody dynamics.

There are different approaches to deduce the kinematics and dynamics of such bodies, the most common are the Newton-Euler and the Langrangian formalisms. The Newton-Euler formalism is based on the well known Newton-Euler equations. The Langrangian formalism uses the principle of least action and describes the movement based on generalized coordinates, a set of parameters (typically, a convenient minimal set) to describe the configuration of the system taking into account its constraints. For a system with multiple bodies and several constraints, e.g., the human body, it is easier to describe the dynamics of such system using the Langrangian formalism.

Next, we will study two simple problems of multibody systems in the context of biomechanics which we can handle well using the Newton-Euler approach. First a planar one-link system (which is not a multibody!), which can represent the movement of one limb of the body or the entire body as a single inverted pendulum. Second, a planar two-link system, which can represent the movement of two segments of the body, e.g., upper arm and forearm. Zajac and Gordon (1989) and Zajac (1993) offer excellent discussions about applying multibody system concepts to understanding human body movement.

## Newton-Euler equations¶

For a two-dimensional movement in the $XY$ plane, the Newton-Euler equations are:

$$\left\{ \begin{array}{l l} \sum F_X = m \ddot{x}_{cm} \\ \\ \sum F_Y = m \ddot{y}_{cm} \\ \\ \sum M_Z = I_{cm} \ddot{\alpha}_Z \end{array} \right.$$

Where the movement is described around the body center of mass ($cm$). $(F_X,\,F_Y)$ and $M_Z$ are, respectively, the forces and moment of forces (torques) acting on the body, $(\ddot{x}_{cm},\,\ddot{y}_{cm})$ and $\ddot{\alpha}_Z$ are, respectively, the linear and angular accelerations, and $I_{cm}$ is the body moment of inertia around the $Z$ axis passing through the body center of mass.

Let's use Sympy to derive some of the characteristics of the systems.

In [1]:
from sympy import Symbol, symbols, cos, sin, Matrix, simplify
from sympy.physics.mechanics import dynamicsymbols, mlatex, init_vprinting
init_vprinting()
from IPython.display import display, Math


Let's study the dynamics of a planar inverted pendulum as a model for the movement of a human body segment with an external force acting on the segment (see Figure 1).

In [2]:
t = Symbol('t')
d, L = symbols('d L', positive=True)
a = dynamicsymbols('alpha')

In [3]:
x, y = d*cos(a), d*sin(a)
xd, yd = x.diff(t), y.diff(t)
xdd, ydd = xd.diff(t), yd.diff(t)

display(Math(r'x=' + mlatex(x)))
display(Math(r'\dot{x}=' + mlatex(xd)))
display(Math(r'\ddot{x}=' + mlatex(xdd)))
display(Math(r'y=' + mlatex(y)))
display(Math(r'\dot{y}=' + mlatex(yd)))
display(Math(r'\ddot{y}=' + mlatex(ydd)))

$$x=d \operatorname{cos}\left(\alpha\right)$$
$$\dot{x}=- d \operatorname{sin}\left(\alpha\right) \dot{\alpha}$$
$$\ddot{x}=- d \operatorname{sin}\left(\alpha\right) \ddot{\alpha} - d \operatorname{cos}\left(\alpha\right) \left(\dot{\alpha}\right)^{2}$$
$$y=d \operatorname{sin}\left(\alpha\right)$$
$$\dot{y}=d \operatorname{cos}\left(\alpha\right) \dot{\alpha}$$
$$\ddot{y}=- d \operatorname{sin}\left(\alpha\right) \left(\dot{\alpha}\right)^{2} + d \operatorname{cos}\left(\alpha\right) \ddot{\alpha}$$

The terms in $\ddot{x}$ and $\ddot{y}$ proportional to $\dot{\alpha}^2$ are components of the centripetal acceleration on the body. As the name suggests, the centripetal acceleration is always directed to the center (torwards the joint) when the segment is rotating. See the notebook Kinematic chain for more on that.

As an exercise, let's go back to the Newton-Euler equation for the sum of torques around the center of mass where the torques due to the joint reaction forces are explicit. From the equation for the the sum of forces, hence we have expressions for the linear accelerations, we can isolate the reaction forces and substitute them on the equation for the torques. With a little help from Sympy:

In [4]:
m, I, g = symbols('m I g', positive=True)
Fex, Fey = symbols('F_ex F_ey')

In [5]:
Frx = m*xdd - Fex
Fry = m*ydd + m*g - Fey
display(Math(r'F_{rx}=' + mlatex(Frx)))
display(Math(r'F_{ry}=' + mlatex(Fry)))

$$F_{rx}=- F_{ex} + m \left(- d \operatorname{sin}\left(\alpha\right) \ddot{\alpha} - d \operatorname{cos}\left(\alpha\right) \left(\dot{\alpha}\right)^{2}\right)$$
$$F_{ry}=- F_{ey} + g m + m \left(- d \operatorname{sin}\left(\alpha\right) \left(\dot{\alpha}\right)^{2} + d \operatorname{cos}\left(\alpha\right) \ddot{\alpha}\right)$$
In [6]:
T = I*add - d*sin(a)*Frx + d*cos(a)*Fry + (L-d)*sin(a)*Fex - (L-d)*cos(a)*Fey

$$T\quad=\quad F_{ex} \left(L - d\right) \operatorname{sin}\left(\alpha\right) - F_{ey} \left(L - d\right) \operatorname{cos}\left(\alpha\right) + I \ddot{\alpha} - d \left(- F_{ex} + m \left(- d \operatorname{sin}\left(\alpha\right) \ddot{\alpha} - d \operatorname{cos}\left(\alpha\right) \left(\dot{\alpha}\right)^{2}\right)\right) \operatorname{sin}\left(\alpha\right) + d \left(- F_{ey} + g m + m \left(- d \operatorname{sin}\left(\alpha\right) \left(\dot{\alpha}\right)^{2} + d \operatorname{cos}\left(\alpha\right) \ddot{\alpha}\right)\right) \operatorname{cos}\left(\alpha\right)$$

This equation for the torques around the center of mass of only one rotating segment seems too complicated. The equation we derived before for the torques around the joint was much simpler. However, if we look at the terms on this last equation, we can simplify most of them. Let's use Sympy to simplify this equation:

In [7]:
T = simplify(T)
display(Math(r'T=' + mlatex(T)))

$$T=F_{ex} L \operatorname{sin}\left(\alpha\right) - F_{ey} L \operatorname{cos}\left(\alpha\right) + I \ddot{\alpha} + d^{2} m \ddot{\alpha} + d g m \operatorname{cos}\left(\alpha\right)$$

And we are back to the more simple equation we've seen before. The first two terms on the right side are the torque due to the external force, the third and fourth are the moment of inertia around the joint (use the theorem of parallel axis) times the acceleration, and the last term is the gravitational torque.

But what happenned with all the other terms in the equation?

First, the terms proportional to the angular acceleration were just components from each direction of the 'inertial' torque that when summed resulted in $md^2\ddot{\alpha}$. Second, the terms proportional to $\dot{\alpha}^2$ are components of the torque due to the centripetal force (acceleration). But the centripetal force passes through the joint as well as through the center of mass, i.e., it has zero lever arm and this torque should be zero. Indeed, when summed these terms are cancelled out.

Now let's study a two-link system which can rotate independently around each joint. We will see that now the torque due to the centripetal force most of the times will not cancel out and a new torque component will appear.

### The Jacobian matrix¶

Another way to deduce the velocity and acceleration of a point at the rotating link is to use the Jacobian matrix (see Kinematic chain). Remember that in the context of kinematic chains, the Jacobian is a matrix of all first-order partial derivatives of the linear position vector of the endpoint with respect to the angular position vector. For the planar one-link case, this means that the Jacobian matrix is:

$$\mathbf{J}= \begin{bmatrix} \frac{\partial x}{\partial \alpha} \\ \frac{\partial y}{\partial \alpha} \\ \end{bmatrix}$$

In [24]:
r = Matrix((x, y))
J = r.diff(a)
display(Math(r'\mathbf{J}=' + mlatex(J)))

$$\mathbf{J}=\left[\begin{matrix}- d \operatorname{sin}\left(\alpha\right)\\d \operatorname{cos}\left(\alpha\right)\end{matrix}\right]$$

And Sympy has a function to calculate the Jacobian:

In [25]:
J = r.jacobian([a])
display(Math(r'\mathbf{J}=' + mlatex(J)))

$$\mathbf{J}=\left[\begin{matrix}- d \operatorname{sin}\left(\alpha\right)\\d \operatorname{cos}\left(\alpha\right)\end{matrix}\right]$$

The linear velocity of a point in the link will be given by the product between the Jacobian of the kinematic link and its angular velocity:

$$\mathbf{v} = \mathbf{J} \dot{\alpha}$$

Using Sympy:

In [30]:
vel = J*a.diff(t)
display(Math(r'\begin{bmatrix} \dot{x} \\ \dot{y} \end{bmatrix}=' + mlatex(vel)))

$$\begin{bmatrix} \dot{x} \\ \dot{y} \end{bmatrix}=\left[\begin{matrix}- d \operatorname{sin}\left(\alpha\right) \dot{\alpha}\\d \operatorname{cos}\left(\alpha\right) \dot{\alpha}\end{matrix}\right]$$

And the linear acceleration will be given by the derivative of this last expression:

$$\mathbf{a} = \dot{\mathbf{J}} \dot{\alpha} + \mathbf{J} \ddot{\alpha}$$

And using Sympy again:

In [32]:
acc = (J*a.diff(t)).diff(t)
display(Math(r'\begin{bmatrix} \ddot{x} \\ \ddot{y} \end{bmatrix}=' + mlatex(acc)))

$$\begin{bmatrix} \ddot{x} \\ \ddot{y} \end{bmatrix}=\left[\begin{matrix}- d \operatorname{sin}\left(\alpha\right) \ddot{\alpha} - d \operatorname{cos}\left(\alpha\right) \left(\dot{\alpha}\right)^{2}\\- d \operatorname{sin}\left(\alpha\right) \left(\dot{\alpha}\right)^{2} + d \operatorname{cos}\left(\alpha\right) \ddot{\alpha}\end{matrix}\right]$$

Same expressions as before.

We can also use the Jacobian matrix to calculate the torque due to a force on the link:

$$T = \mathbf{J}^T \begin{bmatrix} F_{ex} \\ F_{ey} \end{bmatrix}$$

In [44]:
Te = J.T*Matrix((Fex, Fey))
display(Math(r'T_e=' + mlatex(Te[0])))

$$T_e=- F_{ex} d \operatorname{sin}\left(\alpha\right) + F_{ey} d \operatorname{cos}\left(\alpha\right)$$

Let's study the dynamics of a planar double inverted pendulum (see Figure 2) as a model of two interconnected segments in the human body with an external force acting on the distal segment. Once again, we will consider that there are muscles around each joint and they generate torques.

In [8]:
t = Symbol('t')
d1, d2, L1, L2 = symbols('d1, d2, L_1 L_2', positive=True)
a1, a2 = dynamicsymbols('alpha1 alpha2')
a1d, a2d = a1.diff(t), a2.diff(t)
a1dd, a2dd = a1.diff(t, 2), a2.diff(t, 2)

In [9]:
x1, y1 = d1*cos(a1), d1*sin(a1)
x1d, y1d = x1.diff(t), y1.diff(t)
x1dd, y1dd = x1d.diff(t), y1d.diff(t)

display(Math(r'x_1=' + mlatex(x1)))
display(Math(r'\dot{x}_1=' + mlatex(x1d)))
display(Math(r'\ddot{x}_1=' + mlatex(x1dd)))
display(Math(r'y_1=' + mlatex(y1)))
display(Math(r'\dot{y}_1=' + mlatex(y1d)))
display(Math(r'\ddot{y}_1=' + mlatex(y1dd)))

$$x_1=d_{1} \operatorname{cos}\left(\alpha_{1}\right)$$
$$\dot{x}_1=- d_{1} \operatorname{sin}\left(\alpha_{1}\right) \dot{\alpha}_{1}$$
$$\ddot{x}_1=- d_{1} \operatorname{sin}\left(\alpha_{1}\right) \ddot{\alpha}_{1} - d_{1} \operatorname{cos}\left(\alpha_{1}\right) \left(\dot{\alpha}_{1}\right)^{2}$$
$$y_1=d_{1} \operatorname{sin}\left(\alpha_{1}\right)$$
$$\dot{y}_1=d_{1} \operatorname{cos}\left(\alpha_{1}\right) \dot{\alpha}_{1}$$
$$\ddot{y}_1=- d_{1} \operatorname{sin}\left(\alpha_{1}\right) \left(\dot{\alpha}_{1}\right)^{2} + d_{1} \operatorname{cos}\left(\alpha_{1}\right) \ddot{\alpha}_{1}$$
In [10]:
x2, y2 = L1*cos(a1) + d2*cos(a1+a2), L1*sin(a1) + d2*sin(a1+a2)
x2d, y2d = x2.diff(t), y2.diff(t)
x2dd, y2dd = x2d.diff(t), y2d.diff(t)

display(Math(r'x_2=' + mlatex(x2)))
display(Math(r'\dot{x}_2=' + mlatex(x2d)))
display(Math(r'\ddot{x}_2=' + mlatex(x2dd)))
display(Math(r'y_2=' + mlatex(y2)))
display(Math(r'\dot{y}_2=' + mlatex(y2d)))
display(Math(r'\ddot{y}_2=' + mlatex(y2dd)))

$$x_2=L_{1} \operatorname{cos}\left(\alpha_{1}\right) + d_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$
$$\dot{x}_2=- L_{1} \operatorname{sin}\left(\alpha_{1}\right) \dot{\alpha}_{1} - d_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right) \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)$$
$$\ddot{x}_2=- L_{1} \operatorname{sin}\left(\alpha_{1}\right) \ddot{\alpha}_{1} - L_{1} \operatorname{cos}\left(\alpha_{1}\right) \left(\dot{\alpha}_{1}\right)^{2} - d_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right)^{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) - d_{2} \left(\ddot{\alpha}_{1} + \ddot{\alpha}_{2}\right) \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)$$
$$y_2=L_{1} \operatorname{sin}\left(\alpha_{1}\right) + d_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)$$
$$\dot{y}_2=L_{1} \operatorname{cos}\left(\alpha_{1}\right) \dot{\alpha}_{1} + d_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right) \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$
$$\ddot{y}_2=- L_{1} \operatorname{sin}\left(\alpha_{1}\right) \left(\dot{\alpha}_{1}\right)^{2} + L_{1} \operatorname{cos}\left(\alpha_{1}\right) \ddot{\alpha}_{1} - d_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right)^{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) + d_{2} \left(\ddot{\alpha}_{1} + \ddot{\alpha}_{2}\right) \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$

Inspecting the equations above, we see a new kind of acceleration, proportional to $\dot{\alpha_1}\dot{\alpha_2}$. This acceleration is due to the Coriolis effect and is present only when there are movement in the two joints.

### Kinetics¶

From the free body diagrams, the Newton-Euler equations for the planar double inverted pendulum are:

$$\begin{array}{l l} F_{r2x} + F_{e,x} = m_2\ddot{x}_{2} \\ \\ F_{r2y} - m_2g + F_{e,y} = m_2\ddot{y}_{2} \\ \\ T_2 + d_2F_{r2x}\sin(\alpha_1+\alpha_2) - d_2F_{r2y}\cos(\alpha_1+\alpha_2) - (L_2-d_2)F_{e,x}\sin(\alpha_1+\alpha_2) - (L_2-d_2)F_{e,y}\cos(\alpha_1+\alpha_2) = I_{2}(\ddot{\alpha}_1+\ddot{\alpha}_2) \end{array}$$

$$\begin{array}{l l} F_{r1x} - F_{r2x} = m_1\ddot{x}_{1} \\ \\ F_{r1y} - F_{r2y} - m_1g = m_1\ddot{y}_{1} \\ \\ T_1 - T_2 + d_1F_{r1x}\sin\alpha_1 - d_1F_{r1y}\cos\alpha_1 + (L_1-d_1)F_{r2x}\sin\alpha_1 - (L_1-d_1)F_{r2y}\cos\alpha_1 = I_{1}\ddot{\alpha}_1 \end{array}$$

If we want to determine the joint torques and we know the kinematics of the links, the inverse dynamics approach, we isolate the joint torques in the equations above, start solving for link 2 and then link 1. To determine the kinematics knowing the joint torques, the direct dynamics approach, we isolate the joint angular accelerations in the equations above and solve the ordinary differential equations.

Let's express the equations for the torques substituing the terms we know:

In [11]:
m1, m2, I1, I2, g = symbols('m_1, m_2, I_1 I_2 g', positive=True)

In [12]:
# link 2
Fr2x = m2*x2dd - Fex
Fr2y = m2*y2dd + m2*g - Fey
T2 = I2*(a1dd+a2dd) - d2*Fr2x*sin(a1+a2) + d2*Fr2y*cos(a1+a2) + (L2-d2)*Fex*sin(a1+a2) - (L2-d2)*Fey*cos(a1+a2)
T2 = simplify(T2)
Fr1x = m1*x1dd + Fr2x
Fr1y = m1*y1dd + Fr2y + m1*g
T1 = I1*a1dd + T2 - d1*Fr1x*sin(a1) + d1*Fr1y*cos(a1) - (L1-d1)*Fr2x*sin(a1) + (L1-d1)*Fr2y*cos(a1)
T1 = simplify(T1)


The expressions for the joint moments of force are:

In [13]:
display(Math(r'T_1\quad = \quad ' + mlatex(T1)))

$$T_1\quad = \quad F_{ex} L_{1} \operatorname{sin}\left(\alpha_{1}\right) + F_{ex} L_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) - F_{ey} L_{1} \operatorname{cos}\left(\alpha_{1}\right) - F_{ey} L_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) + I_{1} \ddot{\alpha}_{1} + I_{2} \ddot{\alpha}_{1} + I_{2} \ddot{\alpha}_{2} + L_{1}^{2} m_{2} \ddot{\alpha}_{1} - 2 L_{1} d_{2} m_{2} \operatorname{sin}\left(\alpha_{2}\right) \dot{\alpha}_{1} \dot{\alpha}_{2} - L_{1} d_{2} m_{2} \operatorname{sin}\left(\alpha_{2}\right) \left(\dot{\alpha}_{2}\right)^{2} + 2 L_{1} d_{2} m_{2} \operatorname{cos}\left(\alpha_{2}\right) \ddot{\alpha}_{1} + L_{1} d_{2} m_{2} \operatorname{cos}\left(\alpha_{2}\right) \ddot{\alpha}_{2} + L_{1} g m_{2} \operatorname{cos}\left(\alpha_{1}\right) + d_{1}^{2} m_{1} \ddot{\alpha}_{1} + d_{1} g m_{1} \operatorname{cos}\left(\alpha_{1}\right) + d_{2}^{2} m_{2} \ddot{\alpha}_{1} + d_{2}^{2} m_{2} \ddot{\alpha}_{2} + d_{2} g m_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$
$$T_2\quad = \quad F_{ex} L_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) - F_{ey} L_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) + I_{2} \ddot{\alpha}_{1} + I_{2} \ddot{\alpha}_{2} + L_{1} d_{2} m_{2} \operatorname{sin}\left(\alpha_{2}\right) \left(\dot{\alpha}_{1}\right)^{2} + L_{1} d_{2} m_{2} \operatorname{cos}\left(\alpha_{2}\right) \ddot{\alpha}_{1} + d_{2}^{2} m_{2} \ddot{\alpha}_{1} + d_{2}^{2} m_{2} \ddot{\alpha}_{2} + d_{2} g m_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$

There is an elegant form to display the equations for the torques using generalized coordinates, $q=[\alpha_1, \alpha_2]^T$ and grouping the terms proportional to common quantities in matrices, see for example, Craig (2005, page 180), Pandy (2001), and Zatsiorsky (2002, page 383):

$$\tau = M(q)\ddot{q} + C(q,\dot{q}) + G(q) + E(q,\dot{q})$$

Where $\tau$ is a matrix (2x1) of joint torques; $M$ is the mass or inertia matrix (2x2); $\ddot{q}$ is a matrix (2x1) of angular accelerations; $C$ is a matrix (2x1) of centipetal and Coriolis torques; $G$ is a matrix (2x1) of gravitational torques; and $E$ is a matrix (2x1) of external torques.
Let's use Sympy to display the equations in this new form:

In [14]:
T1, T2 = T1.expand(), T2.expand()
q1, q2 = dynamicsymbols('q_1 q_2')
q1d, q2d = q1.diff(t), q2.diff(t)
q1dd, q2dd = q1.diff(t, 2), q2.diff(t, 2)
T1 = T1.subs({a1:q1, a2:q2, a1d:q1d, a2d:q2d, a1dd:q1dd, a2dd:q2dd})
T2 = T2.subs({a1:q1, a2:q2, a1d:q1d, a2d:q2d, a1dd:q1dd, a2dd:q2dd})

In [15]:
M = Matrix(((simplify(T1.coeff(q1dd)), simplify(T1.coeff(q2dd))),
(simplify(T2.coeff(q1dd)), simplify(T2.coeff(q2dd)))))
C = Matrix((simplify(T1.coeff(q1d**2)*q1d**2 + T1.coeff(q2d**2)*q2d**2 + T1.coeff(q1d*q2d)*q1d*q2d),
simplify(T2.coeff(q1d**2)*q1d**2 + T2.coeff(q2d**2)*q2d**2 + T2.coeff(q1d*q2d)*q1d*q2d)))
G = Matrix((simplify(T1.coeff(g)*g),
simplify(T2.coeff(g)*g)))
E = Matrix((simplify(T1.coeff(Fex)*Fex + T1.coeff(Fey)*Fey),
simplify(T2.coeff(Fex)*Fex + T2.coeff(Fey)*Fey)))

display(Math(r'\begin{eqnarray}\tau&=&\begin{bmatrix}\tau_1\\ \tau_2\\ \end{bmatrix} \\' +
r'M(q)&=&' + mlatex(M) + r'\\' +
r'\ddot{q}&=&' + mlatex(Matrix((q1dd, q2dd))) + r'\\' +
r'C(q,\dot{q})&=&' + mlatex(C) + r'\\' +
r'G(q)&=&' + mlatex(G) + r'\\' +
r'E(q,\dot{q})&=&' + mlatex(E) + r'\end{eqnarray}'))

$$\begin{eqnarray}\tau&=&\begin{bmatrix}\tau_1\\ \tau_2\\ \end{bmatrix} \\M(q)&=&\left[\begin{matrix}I_{1} + I_{2} + L_{1}^{2} m_{2} + 2 L_{1} d_{2} m_{2} \operatorname{cos}\left(q_{2}\right) + d_{1}^{2} m_{1} + d_{2}^{2} m_{2} & I_{2} + L_{1} d_{2} m_{2} \operatorname{cos}\left(q_{2}\right) + d_{2}^{2} m_{2}\\I_{2} + L_{1} d_{2} m_{2} \operatorname{cos}\left(q_{2}\right) + d_{2}^{2} m_{2} & I_{2} + d_{2}^{2} m_{2}\end{matrix}\right]\\\ddot{q}&=&\left[\begin{matrix}\ddot{q}_{1}\\\ddot{q}_{2}\end{matrix}\right]\\C(q,\dot{q})&=&\left[\begin{matrix}- L_{1} d_{2} m_{2} \left(2 \dot{q}_{1} + \dot{q}_{2}\right) \operatorname{sin}\left(q_{2}\right) \dot{q}_{2}\\L_{1} d_{2} m_{2} \operatorname{sin}\left(q_{2}\right) \left(\dot{q}_{1}\right)^{2}\end{matrix}\right]\\G(q)&=&\left[\begin{matrix}g \left(L_{1} m_{2} \operatorname{cos}\left(q_{1}\right) + d_{1} m_{1} \operatorname{cos}\left(q_{1}\right) + d_{2} m_{2} \operatorname{cos}\left(q_{1} + q_{2}\right)\right)\\d_{2} g m_{2} \operatorname{cos}\left(q_{1} + q_{2}\right)\end{matrix}\right]\\E(q,\dot{q})&=&\left[\begin{matrix}F_{ex} \left(L_{1} \operatorname{sin}\left(q_{1}\right) + L_{2} \operatorname{sin}\left(q_{1} + q_{2}\right)\right) - F_{ey} \left(L_{1} \operatorname{cos}\left(q_{1}\right) + L_{2} \operatorname{cos}\left(q_{1} + q_{2}\right)\right)\\L_{2} \left(F_{ex} \operatorname{sin}\left(q_{1} + q_{2}\right) - F_{ey} \operatorname{cos}\left(q_{1} + q_{2}\right)\right)\end{matrix}\right]\end{eqnarray}$$

With this convention, to perform inverse dynamics we would calculate:

$$\tau = M(q)\ddot{q} + C(q,\dot{q}) + G(q) + E(q,\dot{q})$$

And for direct dynamics we would solve the differential equation:

$$\ddot{q} = M(q)^{-1} \left[\tau - C(q,\dot{q}) - G(q) - E(q,\dot{q}) \right]$$

The advantage of calculating analytically the derivatives of the position vector as function of the joint angles and using the notation above is that each term that contributes to each joint torque or acceleration can be easily identified.

#### Coupling (or interaction) effects¶

The terms off the main diagonal in the inertia matrix (which are the same) and the centripetal and Coriolis terms represent the effects of the movement (nonzero velocity) of one joint over the other. These torques are referred as coupling or interaction effects (see for example Hollerbach and Flash (1982) for an application of this concept in the study of the motor control of the upper limb movement).

#### Planar double pendulum¶

Using the same equations above, one can represent a planar double pendulum (hanging from the top, not inverted) considering the angles $\alpha_1$ and $\alpha_2$ negative, e.g., at $\alpha_1=-90^o$ and $\alpha_2=0$ the pendulum is hanging vertical.

WARNING: $F_r$ is not the actual joint reaction force!

For these two examples, in the Newton-Euler equations based on the free body diagrams we represented the consequences of all possible muscle forces on a joint as a net muscle torque and all forces acting on a joint as a resultant joint reaction force. That is, all forces between segments were represented as a resultant force that doesn't generate torque and a force couple (or free moment) that only generates torque. This is an important principle in mechanics of rigid bodies, see for example this text. However, this principle creates the unrealistic notion that the sum of forces is applied directly on the joint (which has no further implication for a rigid body), but it is inaccurate for the understanding of the local effects on the joint. So, if we are trying to understand the stress on the joint or mechanisms of joint injury, the forces acting on the joint and on the rest of the segment must be considered individually.

#### Determination of muscle force¶

The torque $T$ exerted by a muscle is given by the product between the muscletendon moment arm $r$ and its force $F$. For the human body, there is more than one muscle crossing a joint and several joints. In such case, the torques due to the muscles are expressed in the following matrix form considering $n$ joints and $m$ muscles:

$$\begin{eqnarray} \begin{bmatrix} T_1 \\ \vdots \\ T_n \end{bmatrix} = \begin{bmatrix} r_{11} & \cdots & r_{1m} \\ \vdots & \ddots & \vdots \\ r_{n1} & \cdots & r_{nm} \end{bmatrix} \begin{bmatrix} F_1 \\ \vdots \\ F_m \end{bmatrix} \end{eqnarray}$$

Where $r_{nm}$ is the moment arm about joint $n$ of the muscle $m$.
In the example of the two-link system, we sketched two uniarticular muscles for each of the two joints, consequently:

$$\begin{eqnarray} \begin{bmatrix} T_1 \\ T_2 \end{bmatrix} = \begin{bmatrix} r_{1,ext} & r_{1,flex} & 0 & 0 \\ 0 & 0 & r_{1,ext} & r_{1,flex} \end{bmatrix} \begin{bmatrix} F_{1,ext} \\ -F_{1,flex} \\ F_{2,ext} \\ -F_{2,flex} \end{bmatrix} \end{eqnarray}$$

The moment arm of a muscle varies with the motion of the joints it crosses. In this case, using the virtual work principle the moment arm can be given by (Sherman et al., 2013; Nigg and Herzog, 2006, page 634):

$$r(q) = \frac{\partial L_{MT}(q)}{\partial q}$$

Where $L_{MT}(q)$ is the length of the muscletendon unit expressed as a function of angle $q$.

For the simulation of human movement, muscles can be modeled as Hill-type muscles, the torques they generate are given by the matrix above, and this matrix is entered in the ODE for a multibody system dynamics we deduced before:

$$\ddot{q} = M(q)^{-1} \left[R_{MT}(q)F_{MT}(a,L_{MT},\dot{L}_{MT}) - C(q,\dot{q}) - G(q) - E(q,\dot{q}) \right]$$

Where $R_{MT}$ and $F_{MT}$ are matrices for the moment arms and muscletendon forces, respectively. This ODE is then solved numerically given initial values; but this problem is far from trivial for a simulation with several segments and muscles.

## Numerical simulation¶

Let's simulate a voluntary movement of the upper limb using the planar two-link system as a model in order to visulize the contribution of each torque term. We will ignore the muscle dynamics and we will calculate the joint torques necessary to move the upper limb from one point to another under the assumption that the movement is performed with the smoothest trajectory possible. I.e., the movement is performed with a minimum-jerk trajectory, a hypothesis about control of voluntary movements proposed by Flash and Hogan (1985).

Once we determine the desired trajectory, we can calculate the velocity and acceleration of the segments and combine with anthropometric measures to calculate the joint torques necessary to move the segments. This means we will perform inverse dynamics.

Let's simulate a slow (4 s) and a fast (0.5 s) movement of the upper limb starting at the anatomical neutral position (upper limb at the side of the trunk) and ending with the upper arm forward at horizontal and elbow flexed at 90 degrees.

First, let's import the necessary Python libraries and customize the environment:

In [16]:
import numpy as np
%matplotlib inline
import matplotlib
import matplotlib.pyplot as plt
matplotlib.rcParams['lines.linewidth'] = 3
matplotlib.rcParams['font.size'] = 13
matplotlib.rcParams['lines.markersize'] = 5
matplotlib.rc('axes', grid=False, labelsize=14, titlesize=16, ymargin=0.05)
matplotlib.rc('legend', numpoints=1, fontsize=11)
import sys
sys.path.insert(1, r'./../functions')  # add to pythonpath


Let's take the anthropometric data from Dempster's model (see Body segment parameters):

In [17]:
height, mass = 1.70,               70  # m, kg
L1n, L2n     = 0.188*height,       0.253*height
d1n, d2n     = 0.436*L1n,          0.682*L2n
m1n, m2n     = 0.0280*mass,        0.0220*mass
rg1n, rg2n   = 0.322,              0.468
I1n, I2n     = m1n*(rg1n*L1n)**2,  m2n*(rg2n*L2n)**2


Considering these lengths, the initial and final positions of the endpoint (finger tip) for the simulated movement will be:

In [18]:
xi, yi = 0, -L1n-L2n
xf, yf = L1n, L2n
gn = 9.81  # gravity acceleration m/s2


### Slow movement¶

In [19]:
duration = 4  # seconds


The endpoint minimum jerk trajectory will be (see Kinematic chain in a plane (2D)):

In [20]:
from minjerk import minjerk

In [21]:
time, rlin, vlin, alin, jlin = minjerk([xi, yi], [xf, yf], duration=duration)


Let's find the joint angles to produce this minimum-jerk trajectory (inverse kinematics):

In [22]:
from invkin2_2d import invkin

In [23]:
rang = invkin(time, rlin, L1=L1n, L2=L2n)

Endpoint value outside working area. Value will be coerced.


For the joint torques, we need to calculate the angular velocity and acceleration. Let's do that using numerical differentiation:

In [24]:
def diff_c(ang, duration):
"""Numerical differentiations using the central difference for the angular data.
"""
# central difference (f(x+h)-f(x-h))/(2*h)
dt = duration/(ang.shape[0]-1)
vang = np.empty_like(rang)
aang = np.empty_like(rang)
vang[:, 0] = np.gradient(rang[:, 0], dt)
vang[:, 1] = np.gradient(rang[:, 1], dt)
aang[:, 0] = np.gradient(vang[:, 0], dt)
aang[:, 1] = np.gradient(vang[:, 1], dt)

_, ax = plt.subplots(1, 3, sharex=True, figsize=(10, 3))
ax[0].plot(time, rang*180/np.pi)
ax[0].legend(['Ang 1', 'Ang 2'], framealpha=.5, loc='best')
ax[1].plot(time, vang*180/np.pi)
ax[2].plot(time, aang*180/np.pi)
ylabel = [r'Displacement [$\mathrm{^o}$]', r'Velocity [$\mathrm{^o/s}$]', r'Acceleration [$\mathrm{^o/s^2}$]']
for i, axi in enumerate(ax):
axi.set_xlabel('Time [$s$]')
axi.set_ylabel(ylabel[i])
axi.xaxis.set_major_locator(plt.MaxNLocator(4))
axi.yaxis.set_major_locator(plt.MaxNLocator(4))
plt.tight_layout()
plt.show()

return vang, aang

vang, aang = diff_c(rang, duration)

In [25]:
def dyna(time, L1n, L2n, d1n, d2n, m1n, m2n, gn, I1n, I2n, q1, q2, rang, vang, aang, Fexn, Feyn, M, C, G, E):
"""Numerical calculation and plot for the torques of a planar two-link system.
"""
from sympy import lambdify, symbols
Mfun  = lambdify((I1, I2, L1, L2, d1, d2, m1, m2, q1, q2), M, 'numpy')
Mn    = Mfun(I1n, I2n, L1n, L2n, d1n, d2n, m1n, m2n, rang[:, 0], rang[:, 1])
M00   = Mn[0, 0][:, 0]*aang[:, 0]
M01   = Mn[0, 1][:, 0]*aang[:, 1]
M10   = Mn[1, 0][:, 0]*aang[:, 0]
M11   = Mn[1, 1]*aang[:, 1]
Q1d, Q2d = symbols('Q1d Q2d')
dicti = {q1.diff(t, 1):Q1d, q2.diff(t, 1):Q2d}
C0fun = lambdify((L1, d2, m2, q2, Q1d, Q2d), C[0].subs(dicti), 'numpy')
C0    = C0fun(L1n, d2n, m2n, rang[:, 1], vang[:, 0], vang[:, 1])
C1fun = lambdify((L1, d2, m2, q2, Q1d, Q2d), C[1].subs(dicti), 'numpy')
C1    = C1fun(L1n, d2n, m2n, rang[:, 1], vang[:, 0], vang[:, 1])
G0fun = lambdify((L1, d1, d2, m1, m2, g, q1, q2), G[0], 'numpy')
G0    = G0fun(L1n, d1n, d2n, m1n, m2n, gn, rang[:, 0], rang[:, 1])
G1fun = lambdify((L1, d1, d2, m1, m2, g, q1, q2), G[1], 'numpy')
G1    = G1fun(L1n, d1n, d2n, m1n, m2n, gn, rang[:, 0], rang[:, 1])
E0fun = lambdify((L1, L2, q1, q2, Fex, Fey), E[0], 'numpy')
E0    = E0fun(L1n, L2n, rang[:, 0], rang[:, 1], 0, 0)
E1fun = lambdify((L1, L2, q1, q2, Fex, Fey), E[1], 'numpy')
E1    = E1fun(L1n, L2n, rang[:, 0], rang[:, 1], Fexn, Feyn)

_, ax = plt.subplots(1, 2, sharex=True, squeeze=True, figsize=(10, 4))
ax[0].plot(time, M00+M01)
ax[0].plot(time, C0)
ax[0].plot(time, G0)
ax[0].plot(time, E0)
ax[0].plot(time, M00+M01+C0+G0, 'k--', linewidth=4)
ax[0].set_ylabel(r'Torque [Nm]')
ax[0].set_title('Joint 1')
ax[1].plot(time, M10+M11, label='Mass/Inertia')
ax[1].plot(time, C1, label='Centripetal/Coriolis')
ax[1].plot(time, G1, label='Gravitational')
ax[1].plot(time, E1, label='External')
ax[1].plot(time, M10+M11+C1+G1, 'k--', linewidth=4, label='Muscular (sum)')
ax[1].set_title('Joint 2')
ax[1].legend(framealpha=.5, loc='upper right', bbox_to_anchor=(1.6, 1))
for i, axi in enumerate(ax):
axi.set_xlabel('Time [$s$]')
axi.xaxis.set_major_locator(plt.MaxNLocator(4))
axi.yaxis.set_major_locator(plt.MaxNLocator(4))
plt.tight_layout()
plt.show()

return M00, M01, M10, M11, C0, C1, G0, G1, E0, E1

In [26]:
Fexn, Feyn = 0, 0
M00, M01, M10, M11, C0, C1, G0, G1, E0, E1 = dyna(time, L1n, L2n, d1n, d2n, m1n, m2n, gn, I1n, I2n,
q1, q2, rang, vang, aang, Fexn, Feyn, M, C, G, E)


The joint torques essentially compensate the gravitational torque.

### Fast movement¶

Let's see what is changed for a fast movement:

In [27]:
duration = 0.5  # seconds
time, rlin, vlin, alin, jlin = minjerk([xi, yi], [xf, yf], duration=duration)
rang = invkin(time, rlin, L1=L1n, L2=L2n)
vang, aang = diff_c(rang, duration)
M00, M01, M10, M11, C0, C1, G0, G1, E0, E1 = dyna(time, L1n, L2n, d1n, d2n, m1n, m2n, gn, I1n, I2n,
q1, q2, rang, vang, aang, Fexn, Feyn, M, C, G, E)

Endpoint value outside working area. Value will be coerced.


The interaction torques are larger than the gravitational torques for most part of the movement.

### Fast movement in the horizontal plane¶

Let's simulate a fast movement in the horizontal plane:

In [28]:
gn = 0  # gravity acceleration m/s2
M00, M01, M10, M11, C0, C1, G0, G1, E0, E1 = dyna(time, L1n, L2n, d1n, d2n, m1n, m2n, gn, I1n, I2n,
q1, q2, rang, vang, aang, Fexn, Feyn, M, C, G, E)


## Exercises¶

1. Derive the equations of motion for a single pendulum (not inverted).
2. Derive the equations of motion for a double pendulum (not inverted).
3. Consider the double pendulum moving in the horizontal plane and with no external force. Find out the type of movement and which torque terms are changed when:
a) $\dot{\alpha}_1=0^o$
b) $\alpha_2=0^o$
c) $\dot{\alpha}_2=0^o$
d) $2\alpha_1+\alpha_2=180^o$ (hint: a two-link system with this configuration is called polar manipulator)
4. Derive the equations of motion and the torque terms using angles in the segmental space $(\theta_1,\,\theta_2)$.
5. Run the numerical simulations for the torques with different parameters.