# The N-segment piglet limb.

## Inverse Dynamics, application

Posted by Falk Mielke on 10th Jul 2020

## Elementary Examples in Inverse Dynamics

Falk Mielke, FunMorph, Universiteit Antwerpen

This is one part of a series of blog posts on Inverse Dynamics.

import numpy as NP # numerics
import sympy as SYM # symbolic operations
import sympy.physics.mechanics as MECH # some physics/mechanics tools
import matplotlib.pyplot as MPP # plotting

import WrenchToolbox as WT # contains an improved "Wrench" object

# printing of formulas
SYM.init_printing(use_latex='mathjax', pretty_print = False)
MECH.init_vprinting(use_latex='mathjax', pretty_print = False)

# shorthand to get the basis of a reference frame
GetCoordinates = lambda rf: [rf[idx] for idx in rf.indices]

# transforming a sympy vector to a matrix
MatrixToVector = lambda mat, rf: sum([elm * base for elm, base in zip(mat, GetCoordinates(rf)) ])

# Steiner's Theorem, generalized form
# [modern robotics, page 245]
GeneralizedSteiner = lambda mass, columnvector: \
SYM.simplify( \
mass * ( (columnvector.T*columnvector)[0] * SYM.eye(3) \
- columnvector*columnvector.T \
) \
)

# The N-segment piglet limb

This example of inverse dynamics will get us as close to a limb as is good in an educational notebook. It is an implementation of an "n link" robot arm, or in my case "n segment piglet limb". Code is held general, for example the "n" can be adjusted by a single variable.

Focus on a single segment $$i$$ along the limb. It is connected proximally (proximal direction = limb upwards, if the limb is hanging loose) to segment $$i-1$$, and the distal adjacent (limb downward) segment is $$i+1$$. They are held at a relative angle $$\theta_i(t)$$, which is the time-changing angle between the orientation of the proximal adjacent segment and the focal segment.

Joints are points, labeled $$P_i$$ so that the proximal joint of each segment has the same number $$i$$. Each segment has a center of mass (COM, at point $$C_i$$), and it might be relevant to know the position vector of $$P_i$$ to $$C_i$$. The center of mass concentrates a mass $$m_i$$. Segments have a length $$l_i$$, and for simplicity, the center of mass will always be $$\frac{l}{2}$$ along the segment.

Finally, we attach a reference frame to each segment ($$\{B\}_i$$). The segment orientation is fixed in it - or rather the adjacent reference frames are defined to have a relative angle $$\theta_i$$, and segments are fixed in it.

All these definitions manifest in the following sympy code.

# Generalized Coordinates

This time, I'll use the built-in "dynamic symbols" in sympy.mechanics. They are functions of time, and the global time variable is sympy.mechanics.dynamicsymbols._t. Time is a real number, so we first replace the generic time in the module.

# get the time variable for dynamic symbols
t = SYM.symbols('t', real = True)
MECH.dynamicsymbols._t = t


Our system will consist of n_segments segments.

# number of segments
n_segments = 3

Because the links are stiff, and fixed to each other at the joints, the angles $$\theta$$ are useful generalized coordinates.

# angles ...
angles = MECH.dynamicsymbols(f'θ_{{:{n_segments}}}', real = True)

We will also need their time derivatives.

# ... and their time derivatives
angle_velocities = [θ.diff() for θ in angles]
angle_accelerations = [θ.diff().diff() for θ in angles]

angle_accelerations

$$\displaystyle \left[ \ddot{θ}_{{0}}, \ \ddot{θ}_{{1}}, \ \ddot{θ}_{{2}}\right]$$

For simplification, set the links to rotate at a steady angular velocity around the joints. Though this has nothing to do with actual pig limb motion, it can be used to check whether all the fictitious forces (which become regular) come out as expected.

# here are the periods of rotation for each segment
periods = SYM.symbols(f'T_{{:{n_segments}}}', real = True)

# we fix the angular velocity
# ... and can calculate angle and acceleration from there.
substitutes_θ = {θ: SYM.integrate(2*SYM.pi/T, t) for θ, T in zip(angles, periods)}
substitutes_θdot = {θdot: 2*SYM.pi/T for θdot, T in zip(angle_velocities, periods)}
substitutes_θddot = {θddot: SYM.diff(2*SYM.pi/T, t) for θddot, T in zip(angle_accelerations, periods)}

substitutes = {**substitutes_θ, **substitutes_θdot, **substitutes_θddot}
substitutes

$$\displaystyle \left\{ θ_{{0}} : \frac{2 \pi t}{T_{0}}, \ θ_{{1}} : \frac{2 \pi t}{T_{1}}, \ θ_{{2}} : \frac{2 \pi t}{T_{2}}, \ \dot{θ}_{{0}} : \frac{2 \pi}{T_{0}}, \ \ddot{θ}_{{0}} : 0, \ \dot{θ}_{{1}} : \frac{2 \pi}{T_{1}}, \ \ddot{θ}_{{1}} : 0, \ \dot{θ}_{{2}} : \frac{2 \pi}{T_{2}}, \ \ddot{θ}_{{2}} : 0\right\}$$

These functions could be changed to simulate any kind of movement. As a homework, you may add Euler or Coriolis components here, as I did in the previous notebooks.

# Constants

We don't include gravity here, but that would in fact be easy. The remaining constants are the ones we set: masses and lengths of the segments of our n-link limb.

Each segment has a length and a mass.

# segment length
l = SYM.symbols(f'l_{{:{n_segments}}}', real = True)

# segment mass
m = SYM.symbols(f'm_{{:{n_segments}}}', real = True)

We'll store these constants in a dictionary for subsequent substitution.

constants = {  m[0]: 0.7 # kg \
, l[0]: 0.8 # m \
, periods[0]: 2.*SYM.pi #/3. s \
, m[1]: 0.5 # kg \
, l[1]: 0.6 # m \
, periods[1]: 2.*SYM.pi/2. # s \
, m[2]: 0.3 # kg \
, l[2]: 0.4 # m \
, periods[2]: 2.*SYM.pi/4. # s \
, SYM.pi: NP.pi # = 3.0 \
}


And here is a function to check equations and elements below.

# this function will substitute and simplify
PlugAllIn = lambda expression: SYM.simplify(SYM.nsimplify(SYM.simplify( \
expression.subs(substitutes).doit().subs(substitutes).doit() \
.subs(constants).subs(constants) \
)))

# Reference Frames

The whole system is situated in a static reference frame $$\{W\}$$ ("world"). In addition, each segment will get associated with a reference frame ($$\{A\}-\{V\}$$).

# static = inertial = world = earth-bound reference frame
world = MECH.ReferenceFrame('W')

frames = [ MECH.ReferenceFrame(list(map(chr, range(ord('A'), ord('W')+0)))[seg]) \
for seg in range(n_segments+1) \
]

# the world is part of a list of frames
allframes = [world] + frames

# or: as a dict
frames_dict = {str(frame): frame for frame in allframes}
frames_dict
{'W': W, 'A': A, 'B': B, 'C': C, 'D': D}

Because our system is $$\hat{x}_1$$/$$\hat{x}_2$$-planar, the relation of the frames is defined by the joint angles around the $$\hat{x}_3$$ axis. To generalize, one could use Quaternions instead of angles.

# orient all frames by the respective angles
for previous, rf in enumerate(frames[:-1]):
rf.orient(allframes[previous], 'Axis', (angles[previous], allframes[previous].z))

frames[-1].orient(frames[-2], 'Axis', (0., frames[-2].z))

SYM.pprint(PlugAllIn(frames[-1].ang_vel_in(world).to_matrix(world)).T)
[0  0  7.0]

All the transformations between frames are stored in a nested dictionary.

# "trafo" dict will store transformation matrices "to" any frame "from" another frame.
trafo = {frame: {} for frame in allframes}
for frame_from in allframes:
for frame_to in allframes:
trafo[frame_to][frame_from] = SYM.simplify(frame_to.dcm(frame_from))

# display example
PlugAllIn(trafo[world][frames[-1]])

$$\displaystyle \left[\begin{matrix}\operatorname{cos}\left(7 t\right) & - \operatorname{sin}\left(7 t\right) & 0\\\operatorname{sin}\left(7 t\right) & \operatorname{cos}\left(7 t\right) & 0\\0 & 0 & 1\end{matrix}\right]$$

It is important to note the order of attributes: to transform, select trafo[to frame][from frame] or in short trafo[to][fro]. For example, to go from the $$\{B\}$$ frame to the static world $$\{W\}$$, one would use trafo[world][B].

# Points

Starting from the origin, which coincides with the most proximal joint, we define joints.

### define the origin, i.e. the shoulder
origin = MECH.Point('O')

# the origin does never move
origin.set_vel(world, 0)
origin.set_acc(world, 0)

# n joints plus one end point
joints = [MECH.Point(f'P_{seg}') for seg in range(n_segments+1)]
joints
[P_0, P_1, P_2, P_3]

Despite the dynamicsymbols defined above, it is advisible to give sympy correct spatial coordinates and derivatives.


# loop joints
for seg, joint in enumerate(joints):
## set position of joint
if seg == 0:
# zero'th joint coincides with origin
joint.set_pos(origin, 0 * world.y)
# ... and does not move in the world.
joint.set_vel(world, 0)
joint.set_acc(world, 0)

else:
# all subsequent joints are offset by "length"
#   in the y direction of the upchain reference frame
joint.set_pos(joints[seg-1], l[seg-1] * frames[seg-1].y)

# re-iterate joints to get the angular derivatives right
for seg, joint in enumerate(joints):
## set position derivatives
for frame in allframes:
# check whether the frame is associated with the joint
if frames[seg] == frame:
# (i) no movement in the associated frame
joint.set_vel(frame, 0)
joint.set_acc(frame, 0)
else:
# (ii) derivatives in the other frames
joint.set_vel(frame, joint.pos_from(joints[max([seg-1, 0])])\
.express(frame).diff(t, frame).simplify())
joint.set_acc(frame, joint.vel(frame).express(frame).diff(t, frame).simplify())

# print an example
joints[1].pos_from(origin).express(world).diff(t, world)

$$\displaystyle - l_{0} \operatorname{cos}\left(θ_{{0}}\right) \dot{θ}_{{0}}\mathbf{\hat{w}_x} - l_{0} \operatorname{sin}\left(θ_{{0}}\right) \dot{θ}_{{0}}\mathbf{\hat{w}_y}$$

For simplification, the centers of mass are located halfway along a segment.

In a real setting, one would have to take the real position of the centers of mass relative to joints, and associate another reference frame with the center of mass that is aligned with the principal moments of inertial. Here, centers of mass are located in the reference frames defined for the joints.

# center of mass, relative to proximal joint
coms = [MECH.Point(f'C_{seg}') for seg in range(n_segments)]

# fix positions, velocities, accelerations
for seg, com in enumerate(coms):
# set center of mass position: halfway towards the next joint
com.set_pos(joints[seg], l[seg]*1./2. * frames[seg].y)

for frame in allframes:
#         # also get the motion derivatives
#         com.set_vel(frame, com.pos_from(joints[seg-1]).express(frame).diff(t, frame).simplify())
#         com.set_acc(frame, com.vel(frame).express(frame).diff(t, frame).simplify())

# Here, the "v2pt_theory" automatism should work reliably
com.v2pt_theory(joints[seg], frame, frames[seg])

# print an example
coms[1].pos_from(origin).express(world).diff(t, world).simplify()

$$\displaystyle (- l_{0} \operatorname{cos}\left(θ_{{0}}\right) \dot{θ}_{{0}} - 0.5 l_{1} \left(\dot{θ}_{{0}} + \dot{θ}_{{1}}\right) \operatorname{cos}\left(θ_{{0}} + θ_{{1}}\right))\mathbf{\hat{w}_x} + (- l_{0} \operatorname{sin}\left(θ_{{0}}\right) \dot{θ}_{{0}} - 0.5 l_{1} \left(\dot{θ}_{{0}} + \dot{θ}_{{1}}\right) \operatorname{sin}\left(θ_{{0}} + θ_{{1}}\right))\mathbf{\hat{w}_y}$$

It is important to check whether all positions are correct.

for j in range(n_segments):
print('#'*4, j, '#'*4)
SYM.pprint(joints[j+1].pos_from(joints[j]).express(world).simplify().to_matrix(world).T)
SYM.pprint((2*(coms[j].pos_from(joints[j]))).express(world).simplify().to_matrix(world).T)

#### 0 ####
[-l_{0}⋅sin(θ_{0}(t))  l_{0}⋅cos(θ_{0}(t))  0]
[-1.0⋅l_{0}⋅sin(θ_{0}(t))  1.0⋅l_{0}⋅cos(θ_{0}(t))  0]
#### 1 ####
[-l_{1}⋅sin(θ_{0}(t) + θ_{1}(t))  l_{1}⋅cos(θ_{0}(t) + θ_{1}(t))  0]
[-1.0⋅l_{1}⋅sin(θ_{0}(t) + θ_{1}(t))  1.0⋅l_{1}⋅cos(θ_{0}(t) + θ_{1}(t))  0]
#### 2 ####
[-l_{2}⋅sin(θ_{0}(t) + θ_{1}(t) + θ_{2}(t))  l_{2}⋅cos(θ_{0}(t) + θ_{1}(t) + θ
_{2}(t))  0]
[-1.0⋅l_{2}⋅sin(θ_{0}(t) + θ_{1}(t) + θ_{2}(t))  1.0⋅l_{2}⋅cos(θ_{0}(t) + θ_{1
}(t) + θ_{2}(t))  0]

And also check derivatives:

for j in range(n_segments):
print('#'*4, j, '#'*4)
SYM.pprint(PlugAllIn(((joints[j].acc(allframes[j]))).express(world).simplify().to_matrix(world)).T)
SYM.pprint(PlugAllIn(((coms[j].acc(allframes[j]))).express(world).simplify().to_matrix(world)).T)

#### 0 ####
[0  0  0]
⎡2⋅sin(t)  -2⋅cos(t)    ⎤
⎢────────  ──────────  0⎥
⎣   5          5        ⎦
#### 1 ####
[0  0  0]
⎡6⋅sin(3⋅t)  -6⋅cos(3⋅t)    ⎤
⎢──────────  ────────────  0⎥
⎣    5            5         ⎦
#### 2 ####
[0  0  0]
⎡16⋅sin(7⋅t)  -16⋅cos(7⋅t)    ⎤
⎢───────────  ─────────────  0⎥
⎣     5             5         ⎦

# Inertias

Every segment is associated with point masses at the center of mass. Hence, the Mass Moment of Inertia $$I$$ is trivial (zero) at that point.

However, relative to all other points, we get a Steiner component ("parallel axis theorem"), by using the following formula (Lynch and Park, 2017, p. 245):

$$I_{r} = I_{0} + m \left( r^{T} r \cdot I − r r^{T} \right)$$

Where $$r$$ is the position vector from a reference point $$0$$ (at which $$I_0$$ is known) to the new point. Of course, $$I$$ differs between reference frames, even when seen at the same point.

# zero inertia relative to the com (because we have point masses)
inertia = MECH.inertia(frames[-1], 0, 0, 0)
I = [{} for _ in range(n_segments)]

for seg, com in enumerate(coms):
# the segment has an inertia at the com, in the segment frame.
com_inertia = MECH.inertia(frames[seg], 0, 0, 0)

# loop all reference points
for refpoint in joints[:seg+1]: # coms + joints

# take stored inertia and add Steiner component
I[seg][refpoint] = { frames[seg]: \
com_inertia.to_matrix(frames[seg])\
+ GeneralizedSteiner(  m[seg] \
, refpoint.pos_from(com).express(frames[seg]).to_matrix(frames[seg]) \
) \
}

# all the I's can be transformed to the inertial reference frame
for other_frame in allframes:
I[seg][refpoint][other_frame] = \
SYM.simplify(trafo[other_frame][frames[seg]] * I[seg][refpoint][frames[seg]] )


An example:

# print an example
PlugAllIn(I[1][joints[1]][world])

$$\displaystyle \left[\begin{matrix}\frac{9 \operatorname{cos}\left(3 t\right)}{200} & 0 & 0\\\frac{9 \operatorname{sin}\left(3 t\right)}{200} & 0 & 0\\0 & 0 & \frac{9}{200}\end{matrix}\right]$$

This is the mass moment of inertia of the first (not: zero'th) segment, relative to its proximal joint, in the static frame, or: I[1][joints[1]][world].

Generally, the nested inertia dict can be accessed as follows: I[segment id][reference point][ref frame]. It should contain all segments and points, precalculated, or if that is computationally challenging, only the adjacent ones.

# Kinematics

Once the abstract system is well defined, the aggregation of spatial information should be trivial. We will require linear and angular parameters.

In a real experiment, one would take the abstract definitions above, then assemble the numeric values, and plug them into the formulas. In our case, we just re-organize the known positions, as symbolic functions of time.

# shorthands for the derivative levels
p = 0 # zero'th derivative: the position vector
v = 1 # first derivative: the velocity
a = 2 # second derivative: the acceleration

## Linear Kinematics

One part of kinematics is getting the center of mass positions, relative to all other points. Again, I will construct a nested dictionary.

The sympy functions Point.pos_from(other_point) works as a charm here, even if we would have parametrized this with quaternions.

# prepare an empty dictionary
x = {}

# loop reference points
for seg, point in enumerate(coms):
x[point] = {}

# loop focus points
for refjoint in [origin] + joints:
x[point][refjoint] = {}

# loop reference frames
for frame in allframes:
# get position, velocity and acceleration in that frame
pos = point.pos_from(refjoint).express(frame).simplify().to_matrix(frame)
vel = SYM.simplify(pos.diff(t))
acc = SYM.simplify(vel.diff(t))

# ... and store them in the lowest level of the dict
x[point][refjoint][frame] = { \
p: pos \
, v: vel \
, a: acc \
}


And this can be accessed as follows:

# Recall from previous notebooks: the time derivatives depend on reference frame
SYM.pprint(PlugAllIn(trafo[frames[0]][frames[1]]*x[coms[1]][origin][frames[1]][v]).T)
SYM.pprint(PlugAllIn(x[coms[1]][origin][frames[0]][v]).T)
SYM.pprint(PlugAllIn(trafo[frames[0]][world]*x[coms[1]][origin][world][v]).T)
[1.6  0  0]
⎡-3⋅cos(2⋅t)   -3⋅sin(2⋅t)    ⎤
⎢────────────  ────────────  0⎥
⎣     5             5         ⎦
⎡  9⋅cos(2⋅t)   4  -9⋅sin(2⋅t)    ⎤
⎢- ────────── - ─  ────────────  0⎥
⎣      10       5       10        ⎦

In this dictionary, there are labels to select the [com] of interest, a [reference joint], the [reference frame], and finally [derivative] (which can be 0, 1 or 2). Although all positions could be retrieved, it is sufficient to store positions of centers of mass relative to joints. For those relative positions, I found it useful to pre-calculate reference frame transformations and derivatives, although pre-calculation generates some overhead.

So remember: this reads out as x[of com][relative to joint][in frame x][derivative].

## Angular Kinematics

For angular kinematics, no points are needed. The angular motion, often described by the mysterious greek letter $$\omega$$, is completely described by the time derivatives of the relative angle of the reference frames.

# the same dictionary magic as above
ω = {frame: {} for frame in allframes}

## angular positions, exploiting the definitions above
for frame1 in allframes:
for frame2 in allframes:
ω[frame2][frame1] = {v: frame1.ang_vel_in(frame2).to_matrix(frame2) \
, a: frame1.ang_acc_in(frame2).to_matrix(frame2) \
}


Example:


# quick check:
SYM.pprint((ω[world][frames[1]][v]).T)
SYM.pprint((ω[world][frames[0]][v]).T)
SYM.pprint((ω[frames[0]][frames[1]][v]).T)
SYM.pprint((ω[frames[1]][frames[1]][v]).T)
⎡      d              d           ⎤
⎢0  0  ──(θ_{0}(t)) + ──(θ_{1}(t))⎥
⎣      dt             dt          ⎦
⎡      d           ⎤
⎢0  0  ──(θ_{0}(t))⎥
⎣      dt          ⎦
⎡      d           ⎤
⎢0  0  ──(θ_{1}(t))⎥
⎣      dt          ⎦
[0  0  0]

This reads: rotation[in frame][of frame][derivative].

This weird notation is helpful, because when transforming between reference frames, subscript cancellation can be applied, for example:

ωAB = RAS ωSB

Remember that the rotation of a focal frame in another one, transformed to the focal frame, is not necessarily zero:

$$R_{AS}\omega_{SA} = \omega_{AA} \neq 0$$

So I will rather keep the rotation matrices explicit below (you could write $$\omega_{A_sA}$$).

# Visualization

Plotting the joint trajectories is always a good idea. In this case, I plot the center of mass positions, not the joints.

time = NP.linspace(0., 2*NP.pi, 256, endpoint = False)
joint_positions = [SYM.lambdify(t, PlugAllIn(x[point][origin][world][p]), 'numpy') \
for point in coms]

lin_velocity = [SYM.lambdify(t, PlugAllIn(x[point][origin][world][v]), 'numpy') \
for point in coms]
lin_acceleration = [SYM.lambdify(t, PlugAllIn(x[point][origin][world][a]), 'numpy') \
for point in coms]

y = [pos[0] for pos in joint_positions[-1](time)]

v_values = lin_velocity[-1](time)

# arrange in arrays of equal lengths
for v_nr, val in enumerate(v_values):
if (len(val) == 1) and NP.all(NP.sum(NP.abs(val)) == 0):
v_values[v_nr] = [NP.zeros((len(time),))]

# stack velocities
vel = NP.stack([val[0] for val in v_values], axis = 1)

# get a color from v magnitude
v_mag = NP.sqrt(NP.sum(NP.power(vel, 2), axis = 1))
v_cval = v_mag - NP.min(v_mag)

# MPP.plot(time, v_cval);
# MPP.show()
<lambdifygenerated-3>:2: VisibleDeprecationWarning: Creating an ndarray from ragged nested sequences (which is a list-or-tuple of lists-or-tuples-or ndarrays with different lengths or shapes) is deprecated. If you meant to do this, you must specify 'dtype=object' when creating the ndarray
return (array([[-4/5*sin(t) - 3/5*sin(3*t) - 1/5*sin(7*t)], [(4/5)*cos(t) + (3/5)*cos(3*t) + (1/5)*cos(7*t)], [0]]))
<lambdifygenerated-6>:2: VisibleDeprecationWarning: Creating an ndarray from ragged nested sequences (which is a list-or-tuple of lists-or-tuples-or ndarrays with different lengths or shapes) is deprecated. If you meant to do this, you must specify 'dtype=object' when creating the ndarray
return (array([[-4/5*cos(t) - 9/5*cos(3*t) - 7/5*cos(7*t)], [-4/5*sin(t) - 9/5*sin(3*t) - 7/5*sin(7*t)], [0]]))
ys = [[pos[0] for pos in position(time)] \
for position in joint_positions]
y = ys[-1]

fig = MPP.figure(dpi = 100)

for yj in ys:
ax.scatter(yj[0], yj[1], s = 2 \
, color = '0.' \
, marker = '.' \
, alpha = 0.2 \
)
ax.scatter(y[0], y[1], s = 24 \
, color = [(val, val, val) for val in 0.66*v_cval/NP.max(v_cval)] \
, marker = 'o' \
, alpha = 1. \
)
ax.set_xlim(NP.array([-1.05,1.05])*(sum([constants[seglen] for seglen in l])))
ax.set_ylim(NP.array([-1.05,1.05])*(sum([constants[seglen] for seglen in l])))
ax.set_xlabel('$x_1$')
ax.set_ylabel('$x_2$')

ax.scatter(y[0], NP.ones((len(y[0]),))*y[2], s = 24 \
, color = [(val, val, val) for val in 0.66*v_cval/NP.max(v_cval)] \
, marker = 'o' \
, alpha = 1. \
)
ax.yaxis.set_label_position("right")
ax.yaxis.tick_right()
ax.set_xlim(NP.array([-1.05,1.05])*(sum([constants[seglen] for seglen in l])))
ax.set_ylim(NP.array([-1.05,1.05])*(sum([constants[seglen] for seglen in l])))
ax.set_xlabel('$x_1$')
ax.set_ylabel('$x_3$')

MPP.show();
<lambdifygenerated-1>:2: VisibleDeprecationWarning: Creating an ndarray from ragged nested sequences (which is a list-or-tuple of lists-or-tuples-or ndarrays with different lengths or shapes) is deprecated. If you meant to do this, you must specify 'dtype=object' when creating the ndarray
return (array([[-2/5*sin(t)], [(2/5)*cos(t)], [0]]))
<lambdifygenerated-2>:2: VisibleDeprecationWarning: Creating an ndarray from ragged nested sequences (which is a list-or-tuple of lists-or-tuples-or ndarrays with different lengths or shapes) is deprecated. If you meant to do this, you must specify 'dtype=object' when creating the ndarray
return (array([[-4/5*sin(t) - 3/10*sin(3*t)], [(4/5)*cos(t) + (3/10)*cos(3*t)], [0]]))

Maybe I will animate it at some point.

# Inertial Frame Kinetics

Now for the big business: inverse dynamics, using wrenches. To start off, let's calculate the distal joint wrench in the inertial frame.

It is good to remember how the data structures were defined above:

• Transformation trafo[to][fro]
• Mass Moment of Inertia I[segment id][reference point][ref frame]
• Linear Kinematics x[of com][relative to joint][in frame x][derivative]
• Angular Kinematics rotation[in frame][of frame][derivative]

Mass, length and period would simply be accessed by segment id.

# use shorthands to facilitate accessing the functions.
seg = n_segments-1
com = coms[seg]
joint = joints[seg]
frame = frames[seg]

The Dynamic Wrench of the focal segment is the left handside of the balance equation. The easiest way of construction is to define it at the center of mass, and then translate it to the proximal segment joint.

# dynamic force
dynamic_force_S = m[seg]*x[com][joint][world][a]
SYM.pprint(PlugAllIn(dynamic_force_S).T)

# dynamic moment
dynamic_moment_S = I[seg][joint][world]*ω[world][frame][a] \
+ ω[world][frame][v].cross( I[seg][joint][world]*ω[world][frame][v] )
SYM.pprint(PlugAllIn(dynamic_moment_S).T)

# assembling the dynamic wrench (at the center of mass)
dynamic_wrench_S = WT.Wrench.FromMatrices(world, com, dynamic_force_S, dynamic_moment_S \
)

# because it was assembled at the center of mass, we must translate the wrench to the joint
dynamic_wrench_S = dynamic_wrench_S.Translate(joint)

PlugAllIn(dynamic_wrench_S.Matrix()).T
⎡147⋅sin(7⋅t)  -147⋅cos(7⋅t)    ⎤
⎢────────────  ──────────────  0⎥
⎣     50             50         ⎦
[0  0  0]

$$\displaystyle \left[\begin{matrix}\frac{147 \operatorname{sin}\left(7 t\right)}{50} & - \frac{147 \operatorname{cos}\left(7 t\right)}{50} & 0 & 0 & 0 & 0\end{matrix}\right]$$

Now for the right handside. We want to calculate the Joint Wrench. We generate it with placeholders and solve for them below.

force_components_S = SYM.symbols('f_{JS1:4}', real = True)
moment_components_S = SYM.symbols('m_{JS1:4}', real = True)

joint_wrench_S = WT.Wrench.FromComponents(world, joint, force_components_S, moment_components_S)

SYM.pprint(joint_wrench_S.Matrix().T)

[f_{JS1}  f_{JS2}  f_{JS3}  m_{JS1}  m_{JS2}  m_{JS3}]

These are the only wrenches present in our problem. In a real situation, consider adding a tip wrench (at the distal joint) or further other wrenches such as gravity or air drag.

These are all the ingredients for the Balance of Wrenches:

# get the equations
equations_S = dynamic_wrench_S.Equate(joint_wrench_S) # simple.

# desired outcome variables
components_S =  [*force_components_S, *moment_components_S]

# extract the solution
solutions = {}
for param, sol in SYM.solve(equations_S \
, components_S \
).items():
solutions[param] = sol

# transform the result into a wrench
W_JS = WT.Wrench.FromMatrix(world, joint, SYM.Matrix([[solutions[cmp]] for cmp in components_S]) )
SYM.pprint(PlugAllIn(W_JS.express(frame).Matrix()).T)
[0  -2.94  0  0  0  0]

This is always good to have. And notably, the balance in the inertial frame is independent of fictitious forces; so if all wrenches were defined correctly, the calculation will be our reference. However, it might be desirable to also calculate the joint forces in the segment frames.

# Kinetics in Any Frame

By "calculate the joint forces in the segment frames", I really mean "calculate in", as opposed to "transform to" a rotating segment reference frame. Transformation is trivially done, by left-multiplying the transformation matrix. The bonus by really calculating in a reference frame is that one can calculate the fictitious forces.

When searching for the correct equations, I found that many authors don't explicitly state in which reference frame the symbols are calculated, and how they are transformed if necessary. I have ranted about that in previous notebooks. If you experienced the same issues, the following general equations might help.

## Equations

The equations can be used to calculate the joint wrench at the proximal joint of a segment $$i$$ (associated with $$\{A\}$$) when viewed from the reference frame $$\{A\}$$. Therein, $$\{A\}$$ can be any frame, for example the static frame $$\{S\}$$ or the body frame $$\{B\}$$ itself.

### Position Vector and Angular Vectors

The position vector $$r_{PC}$$ is the column vector from proximal joint $$P$$ to center of mass $$C$$. It can be expressed in different reference frames, for example in $$\{A\}$$, then it is $$r_{PC\{A\}}$$

The angular position $$\omega$$ of a reference frame (e.g. $$\{B\}$$) in another one (e.g. $$\{A\}$$) is $$\omega_{AB}$$, and that is implicitly expressed in the frame denoted by the first index, here $$\{A\}$$. Note that usually $$\omega_{BA} = -\omega_{AB}$$.

Time derivatives are indicated by dots, and you will find first derivatives (e.g. $$\dot{r}_{PC\{A\}}$$ is linear speed) and second derivatives (e.g. $$\ddot{\omega}_{AB}$$ is angular acceleration).

### Inertia

The mass moment of inertia $$I$$ of the $$i$$'th segment, relative to joint $$P$$, expressed in $$\{A\}$$, is calculated from the inertia at the segment's center of mass $$C$$ by

$$I_{iP\{A\}} = R_{AB}\cdot\left( I_{iC\{B\}} + m_i \cdot\left( r^{T}_{PC\{B\}} r_{PC\{B\}} \cdot I_{iC\{B\}} - r_{PC\{B\}} r^{T}_{PC\{B\}} \right) \right)$$

The second summand in brackets is the Steiner component.

### Left Hand Side: The Dynamic Wrench

The change of momentum and angular momentum of the segment $$i$$ is summarized by the dynamic wrench, which is best defined at the center of mass $$C$$. Note that the angular kinematics used here are those of the segment ($$\{B\}$$) in the static frame ($$\{S\}$$), but transformed to the calculation frame ($$\{A\}$$).

$$W_{dyn\{A\}@C} = \left[ m_i \ddot{r}_{PC\{A\}}\ ;\quad I_{iP\{A\}} \cdot\left( R_{AS}\ddot{\omega}_{SB} \right) + \left( R_{AS}\dot{\omega}_{SB} \right) \times I_{iP\{A\}} \left( R_{AS}\dot{\omega}_{SB} \right) \right]$$

This wrench (a six element column vector) is defined at point $$C$$ and expressed in reference frame $$\{A\}$$. It can be translated (i.e. moved) to joint $$P$$ (or any other point) and transformed (i.e. rotated) to another reference frame (e.g. $$\{S\}$$) by using the force component $$F_{dyn\{A\}}$$ to adjust the moment component:

$$W_{dyn\{S\}@P} = R_{SA} \left( W_{dyn\{A\}@C} + \left[ 0 ;\quad r_{CP\{A\}} \times F_{dyn\{A\}} \right] \right)$$

Translation and Transformation work analogously for all coming wrenches.

### Right Hand Side: External Wrenches

The change of momentum must be caused by external wrenches. These are collected on the right hand side of the balance equation.

#### Tip Wrench

In inverse dynamics, we often know the force at the very tip of the system. It is either zero (in the swing phase) or known (e.g. by force plate measurement in case of ground reaction forces).

Either way, it enters the balance equation on the right handside as a six element vector (three force components, three moment components).

#### Euler Wrench

The Euler wrench is defined at the center of mass, where its moment component becomes zero:

$$W_{E\{A\}@C} = \left[ -m_i \left(R_{AS}\ddot{\omega}_{SA}\right) \times r_{PC\{A\}} \ ;\quad 0 \right]$$

For all the dynamic wrenches, angular kinematics refer to the rotation of the calculation frame ($$\{A\}$$) in the static frame ($$\{S\}$$), but transformed to the calculation frame ($$\{A\}$$). These are different parameters than those used for the dynamic wrench above.

#### Centrifugal Wrench

Analogously, the detailed formula for the centrifugal wrench at $$C$$ is:

$$W_{Cf\{A\}@C} = \left[ -m_i \left(R_{AS}\dot{\omega}_{SA}\right) \times \left( \left(R_{AS}\dot{\omega}_{SA}\right) \times r_{PC\{A\}} \right) \ ;\quad 0 \right]$$

#### Coriolis Wrench

Finally, Coriolis gave us this beautiful formula (don't forget factor 2):

$$W_{Cor\{A\}@C} = \left[ -2m_i \left(R_{AS}\dot{\omega}_{SA}\right) \times \dot{r}_{PC\{A\}} \ ;\quad 0 \right]$$

#### Joint Wrench

The Joint Wrench is the sum of all unknown external wrenches that acted on the body, evaluated at the proximal joint of the segment. It is the quantity we are interested in. An accurate determination of the joint wrench requires all wrenches to be known (see below).

#### Other External Wrenches

Other external wrenches have to be added, for example the gravitational wrench, which acts on the center of mass.

$$W_{grav\{S\}@C} = \left[ m_{i} g \ ;\quad 0 \right]$$

Friction can occur (either in the joints, or in interaction with the surrounding medium) and can be added if a plausible model can be inferred.

However, note that one should strive to include all such effects. If ignoring the friction wrench, it will nevertheless enter the equation on the left handside because it is a real external effect affecting the dynamics. Hence, external wrenches that are not explicitly modeled will be part of the Joint Wrench.

For illustration, imagine working under gravity, but forgetting the gravitational wrench. If one sees a segment of the limb swing due to gravity, an observer unaware of gravity would think that the joints produce the observed movement by a joint wrench.

## Computation

The calculation above might be repeated several times, which is a good indication that a function can be defined to facilitate the procedure. Following the calculations outlined in previous notebooks, and given the system definition above, here is the general function to calculate the joint wrench of a selected segment, in a selected frame.

def Kinetics(segment_id, calc_frame = None, tip_wrench = None, verbose = True):
# This function calculates the joint wrench for a given segment, with the system defined above.
# The user has to specify a segment id, and can also give a frame of calculation; default is the segment frame

# store the calc_frame of the focal segment
segment_frame = frames[segment_id]
com = coms[segment_id]
joint = joints[segment_id]

if calc_frame is None:
calc_frame = segment_frame

# if no tip wrench is given, use a zero wrench
if tip_wrench is None:
tip_wrench = WT.Wrench(calc_frame, joints[segment_id+1])

### Dynamic Wrench
# dynamic force
dynamic_force = m[segment_id]*x[com][joint][calc_frame][a]

# dynamic moment
omega_dot = trafo[calc_frame][world] * ω[world][segment_frame][v]
omega_ddot = trafo[calc_frame][world] * ω[world][segment_frame][a]

dynamic_moment = SYM.simplify(I[segment_id][joint][calc_frame]*omega_ddot \
+ omega_dot.cross(I[segment_id][joint][calc_frame]*omega_dot) \
)
if verbose:
print(f'\nDynamic wrench of {com} in {calc_frame} frame:')
SYM.pprint(PlugAllIn(dynamic_moment).T)

# assembling the dynamic wrench
dynamic_wrench = WT.Wrench.FromMatrices(calc_frame, com, dynamic_force, dynamic_moment \
)

# Translate wrench from center of mass to joint
dynamic_wrench = dynamic_wrench.Translate(joint)

if verbose:
SYM.pprint(PlugAllIn(dynamic_wrench.Matrix()).T)

### Joint Wrench
force_components = SYM.symbols(f'f_{{J{segment_frame}in{calc_frame}1:4}}', real = True)
moment_components = SYM.symbols(f'm_{{J{segment_frame}in{calc_frame}1:4}}', real = True)

joint_wrench = WT.Wrench.FromComponents(calc_frame, joint, force_components, moment_components)

if verbose:
SYM.pprint(joint_wrench.Matrix().T)

components =  [*force_components, *moment_components]

### Fictitious Wrenches
if verbose:
print(f'\nFictitious forces of {joint}/{segment_frame} frame in {calc_frame} frame:')
## Euler
euler_wrench = WT.Wrench(calc_frame, com \
, MatrixToVector(-m[segment_id]*(trafo[calc_frame][world] * ω[world][calc_frame][a] \
).cross(x[com][joint][calc_frame][p]) \
, calc_frame) \
).Translate(joint)

if verbose:
SYM.pprint(PlugAllIn(euler_wrench.Matrix()).T)

## Centrifugal
omega_dot = trafo[calc_frame][world] * ω[world][calc_frame][v]
centrifugal_wrench = WT.Wrench(calc_frame, com \
, MatrixToVector( \
-m[segment_id]*(omega_dot).cross( \
(omega_dot).cross(x[com][joint][calc_frame][p]) \
) \
, calc_frame) \
).Translate(joint)

if verbose:
SYM.pprint(PlugAllIn(centrifugal_wrench.Matrix()).T)

## Coriolis
coriolis_wrench = WT.Wrench(calc_frame, com \
, MatrixToVector(-2*m[segment_id]*(trafo[calc_frame][world] * ω[world][calc_frame][v] \
).cross(x[com][joint][calc_frame][v]), calc_frame) \
).Translate(joint)

if verbose:
SYM.pprint(PlugAllIn(coriolis_wrench.Matrix()).T)

### Tip Wrench
tip_wrench = tip_wrench.Translate(joint)

### Equations
equations = dynamic_wrench.Equate( \
joint_wrench  \
+ euler_wrench  \
+ centrifugal_wrench  \
+ coriolis_wrench  \
+ tip_wrench \
)

### Solve
solutions = {}
for param, sol in SYM.solve(equations \
, components \
).items():
solutions[param] = sol

W_J = WT.Wrench.FromMatrix(calc_frame, joint, SYM.Matrix([[solutions[cmp]] for cmp in components]) )
if verbose:
print(f'\nW at {joint} in the {segment_frame} frame:')
SYM.pprint(PlugAllIn(W_J.express(segment_frame).Matrix()).T)

return W_J, {'E': euler_wrench, 'Cf': centrifugal_wrench, 'Cor': coriolis_wrench}

The first useful way to use this function is to calculate the joint wrench of the distalmost segment. As a bonus, calculation can be performed in an arbitrary reference frame.

seg = n_segments-1 # calculate distalmost segment...
rf = frames[seg-1] # in the penultimate segment.

# here the calculation is performed, displaying intermediate results
W_J, fictitious_wrenches = Kinetics(seg, rf, verbose = True)

print(f'\nresult wrench at P_{seg}, calculated in the {rf} frame:')
SYM.pprint(PlugAllIn(W_J.Matrix()).T)
Dynamic wrench of C_2 in B frame:
[0  0  0]
⎡24⋅sin(4⋅t)  -24⋅cos(4⋅t)             ⎤
⎢───────────  ─────────────  0  0  0  0⎥
⎣     25            25                 ⎦
[f_{JCinB1}  f_{JCinB2}  f_{JCinB3}  m_{JCinB1}  m_{JCinB2}  m_{JCinB3}]

Fictitious forces of P_2/C frame in B frame:
[0  0  0  0  0  0]
⎡-27⋅sin(4⋅t)   27⋅cos(4⋅t)            ⎤
⎢─────────────  ───────────  0  0  0  0⎥
⎣      50            50                ⎦
⎡-36⋅sin(4⋅t)   36⋅cos(4⋅t)            ⎤
⎢─────────────  ───────────  0  0  0  0⎥
⎣      25            25                ⎦

W at P_2 in the C frame:
[0  -2.94  0  0  0  0]

result wrench at P_2, calculated in the B frame:
⎡147⋅sin(4⋅t)  -147⋅cos(4⋅t)             ⎤
⎢────────────  ──────────────  0  0  0  0⎥
⎣     50             50                  ⎦

Check with the static frame calculation:


SYM.pprint(SYM.simplify((W_JS.express(frames[seg])-W_J.express(frames[seg])).Matrix()).T)
[0  0  0  0  0  0]

Note that there was no PlugAllIn, so the check is true zero.

# Wrenching Up The Chain

The second useful use case is actual inverse dynamics. Make sure to start at the tip, and iterate up to the most proximal joint. At each joint, remember to apply Newton's Last Law newton_laws[-1] (usually the third one, but I keep forgetting the order).

seg = n_segments-1
W_tip, _ = Kinetics(seg, verbose = False)
SYM.pprint(PlugAllIn(W_tip.Matrix()).T)
[0  -2.94  0  0  0  0]
seg = n_segments-2
W_J, sample_fictitious = Kinetics(  seg \
, tip_wrench = W_tip.express(frames[seg])*-1 \
, verbose = False \
)

SYM.pprint(PlugAllIn(W_tip.express(frames[seg]).Matrix()).T)
SYM.pprint(PlugAllIn(W_J.Matrix()).T)
⎡147⋅sin(4⋅t)  -147⋅cos(4⋅t)             ⎤
⎢────────────  ──────────────  0  0  0  0⎥
⎣     50             50                  ⎦
⎡147⋅sin(4⋅t)    147⋅cos(4⋅t)   27           -441⋅sin(4⋅t) ⎤
⎢────────────  - ──────────── - ──  0  0  0  ──────────────⎥
⎣     50              50        20                250      ⎦

We can look at the fictitious wrenches on the way, for example the centrifugal wrench:

SYM.pprint(PlugAllIn(sample_fictitious['Cf'].Matrix()).T )
[0  1.35  0  0  0  0]

Next segment:

seg = n_segments-3
if not(seg < 0):
W_O, _ = Kinetics(  seg \
, tip_wrench = W_J.express(frames[seg])*-1 \
, verbose = False \
)

SYM.pprint(PlugAllIn(W_J.express(frames[seg]).Matrix()).T)
SYM.pprint(PlugAllIn(W_O.Matrix()).T)
⎡27⋅sin(2⋅t)   147⋅sin(6⋅t)    27⋅cos(2⋅t)   147⋅cos(6⋅t)           -441⋅sin(4
⎢─────────── + ────────────  - ─────────── - ────────────  0  0  0  ──────────
⎣     20            50              20            50                     250

⋅t) ⎤
────⎥
⎦
⎡27⋅sin(2⋅t)   147⋅sin(6⋅t)    27⋅cos(2⋅t)   147⋅cos(6⋅t)   7              27⋅
⎢─────────── + ────────────  - ─────────── - ──────────── - ──  0  0  0  - ───
⎣     20            50              20            50        25

sin(2⋅t)   441⋅sin(4⋅t)   294⋅sin(6⋅t)⎤
──────── - ──────────── - ────────────⎥
25           250            125     ⎦

... and so forth.

Note how each segment adds its inertia, so the equation becomes longer in the process.

# Summary

These were actual inverse dynamics calculations! Some simplifications have made this manageable, but could be reverted easily:

• gravity was neglected (just add a wrench for it)
• planar setting (instead, one could relate reference frames by quaternions)
• trivial inertia's (one could use real matrices, though)
• no friction (how dare me)
• kinematics were analytically determined

However, all the ingredients are there.

The bonus using the tools above is that we got the fictitious forces at each element of the chain.

So please, feel free to download this notebook, adjust it, play around with it, and calculate your own inverse dynamics problems. And feel free to contact me if you encounter anything which does not work as expected!