Motion Interpolation
The package implements the method described in the paper by Hegedüs et al.[1] for 4 poses interpolation using cubic rational function, that yields 6-revolute linkages, and the method by Brunnthaler et al.[2] for 3 poses interpolation using quadratic rational functions, that yields 4-revolute linkage, i.e. the Bennett mechanism.
The previous methods interpolate poses (trasformations) in 3D space. The method by Zube[3] is interpolating 5 or 7 points in 3D space, and yields a quadratic or cubic rational function that represents a 4R (Bennett) or 6R linkage.
All methods have some geometrical constraints, and therefore the interpolation is not always possible. The package is also providing a method for 2 poses interpolation method that yields a spatial Bennett mechanism (4-revolute linkage).
For motion interpolation, the input can be both, DualQuaternion objects
or TransfMatrix objects.
Quadratic interpolation of 3 poses
The following example applies the method by Brunnthaler et al.[2].
# Quadratic interpolation of 3 poses
from rational_linkages import DualQuaternion, Plotter, MotionInterpolation
p0 = DualQuaternion([0, 17, -33, -89, 0, -6, 5, -3])
p1 = DualQuaternion([0, 84, -21, -287, 0, -30, 3, -9])
p2 = DualQuaternion([0, 10, 37, -84, 0, -3, -6, -3])
c = MotionInterpolation.interpolate([p0, p1, p2])
plt = Plotter(steps=500, arrows_length=0.05)
plt.plot(c, interval='closed')
for i, pose in enumerate([p0, p1, p2]):
plt.plot(pose, label='p{}'.format(i+1))
plt.show()
Cubic interpolation of 4 poses
This method does not work for any 4 poses - some geometrical constraints must be met. Please, refer to the original paper (Hegedüs et al.[1]) for more details, or see simplified description in Cubic Interpolation of Four Poses.
Here is presented an example of cubic interpolation of 4 poses.
"""
# Cubic interpolation of 4 poses
"""
from rational_linkages import DualQuaternion, Plotter, MotionInterpolation, RationalMechanism
# 4 poses
p0 = DualQuaternion() # identity
p1 = DualQuaternion.as_rational([0, 0, 0, 1, 1, 0, 1, 0])
p2 = DualQuaternion.as_rational([1, 2, 0, 0, -2, 1, 0, 0])
p3 = DualQuaternion.as_rational([3, 0, 1, 0, 1, 0, -3, 0])
# obtain the interpolated motion curve
c = MotionInterpolation.interpolate([p0, p1, p2, p3])
# factorize the motion curve
fs = c.factorize()
# create a mechanism from the factorization
m = RationalMechanism(fs)
# create an interactive plotter object, with 500 descrete steps
# for the input rational curves, and arrows scaled to 0.05 length
myplt = Plotter(mechanism=m, steps=500, arrows_length=0.5)
# plot the poses
for pose in [p0, p1, p2, p3]:
myplt.plot(pose)
# show the plot
myplt.show()
The input are 4 dual quaternions, \(p_0, p_1, p_2, p_3\), and the output is a parametric rational curve \(C(t)\) that interpolates the poses. Keep in mind that \(p_0\) is the identity.
4 given poses.
The curve equation is then of the form:
And can be plotted as shown in the following figure.
Curve \(C(t)\) that interpolates the poses.
The curve is then factorized, and the resulting mechanism is plotted.
6R mechanism whose tool frame (purple link) follows the curve \(C(t)\).
Quadratic interpolation of 5 points
The following example applies the method by Zube[3]. The result is non-monic polynomial, i.e. the factorized mechanism will be transformed by a static transformation.
"""
# Quadratic interpolation of 5 points
"""
from rational_linkages import (Plotter, MotionInterpolation, PointHomogeneous,
DualQuaternion, RationalMechanism)
# Define 5 points in PR3 space (1st coordinate is projective, then x, y, z)
a0 = PointHomogeneous([1, 0, 0, 0])
a1 = PointHomogeneous([1, 1, 0, -2])
a2 = PointHomogeneous([1, 2, -1, 0])
a3 = PointHomogeneous([1, -3, 0, 3])
a4 = PointHomogeneous([1, 2, 1, -1])
points = [a0, a1, a2, a3, a4]
interpolated_curve = MotionInterpolation.interpolate(points)
m = RationalMechanism(interpolated_curve.factorize())
# due to non-monic solution, to transform the given points and plot them in mechanism
# path, get static transform 'rebase'
rebase = DualQuaternion(interpolated_curve.evaluate(1e12))
p = Plotter(mechanism=m, base=rebase, arrows_length=0.5)
p.plot(interpolated_curve, interval='closed')
for i, pt in enumerate(points):
p.plot(pt, label=f'a{i}')
p.show()
The resulting curve is plotted in the following figure.
Rational quadratic curve that interpolates 5 points.
Cubic interpolation of 7 points
The follwoing example applies the extended method by Zube[3] and interpolates 7 points (3D points) with a cubic rational motion. The result is again non-monic polynomial, i.e. the factorized mechanism will be transformed by a static transformation.
"""
# Cubic interpolation of 7 points
"""
from rational_linkages import (Plotter, MotionInterpolation, PointHomogeneous,
DualQuaternion, RationalMechanism)
# Define 7 points in PR3 space (1st coordinate is projective, then x, y, z)
a0 = PointHomogeneous([1, 0, 0, 0])
a1 = PointHomogeneous([1, 1, 0, -2])
a2 = PointHomogeneous([1, 2, -1, 0])
a3 = PointHomogeneous([1, -3, 0, 3])
a4 = PointHomogeneous([1, 2, 1, -1])
a5 = PointHomogeneous([1, 2, 3, -3])
a6 = PointHomogeneous([1, 1, 1, 1])
points = [a0, a1, a2, a3, a4, a5, a6]
interpolated_curve = MotionInterpolation.interpolate(points)
m = RationalMechanism(interpolated_curve.factorize())
# due to non-monic solution, to transform the given points and plot them in mechanism
# path, get static transform 'rebase' and uncomment the line in for loop bellow
rebase = DualQuaternion(interpolated_curve.evaluate(1e12)).normalize()
p = Plotter(mechanism=m, steps=1000, arrows_length=0.5)
p.plot(interpolated_curve, interval='closed')
for i, pt in enumerate(points):
# pt = rebase.inv().act(pt) # uncomment to plot the points in the mechanism path
p.plot(pt, label=f'a{i}')
p.show()
The resulting curve is plotted in the following figure.
Rational cubic curve that interpolates 7 points.
References