from copy import deepcopy
from typing import Union
from warnings import warn
import numpy
import sympy
from .DualQuaternion import DualQuaternion
from .PointHomogeneous import PointHomogeneous
from .Quaternion import Quaternion
from .RationalBezier import RationalBezier
from .RationalCurve import RationalCurve
from .TransfMatrix import TransfMatrix
from .utils_rust import motion_interp_x3
[docs]
class MotionInterpolation:
"""
Method for interpolation of poses by rational motion curve in SE(3).
There are two methods for interpolation of poses by rational motion curve, please
see the following examples for more details.
See Also
--------
:ref:`interpolation_background`
Background on interpolation methods.
:ref:`interpolation_examples`
Examples of interpolation.
Examples
--------
.. code-block:: python
# 4-pose interpolation
from rational_linkages import (DualQuaternion, Plotter,
MotionInterpolation, RationalMechanism)
# 4 poses
p0 = DualQuaternion() # identity
p1 = DualQuaternion([0, 0, 0, 1, 1, 0, 1, 0])
p2 = DualQuaternion([1, 2, 0, 0, -2, 1, 0, 0])
p3 = DualQuaternion([3, 0, 1, 0, 1, 0, -3, 0])
# obtain the interpolated motion curve
c = MotionInterpolation.interpolate([p0, p1, p2, p3])
# factorize the motion curve
f = c.factorize()
# create a mechanism from the factorization
m = RationalMechanism(f)
# create an interactive plotter object, with 1000 descrete steps
# for the input rational curves, and arrows scaled to 0.5 length
myplt = Plotter(mechanism=m, steps=1000, arrows_length=0.5)
# plot the poses
for pose in [p0, p1, p2, p3]:
myplt.plot(pose)
# show the plot
myplt.show()
.. clear-namespace
.. code-block:: python
# 3-pose interpolation
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()
.. clear-namespace
"""
def __init__(self):
"""Create a MotionInterpolation helper instance.
The class is stateless and provides interpolation helpers as
static methods; this constructor is present for API symmetry.
"""
pass
[docs]
@staticmethod
def interpolate(poses_or_points: list[Union[DualQuaternion, TransfMatrix, PointHomogeneous]],
lambda_val: Union[float, int] = 0,
motion_family: int = 0) -> RationalCurve:
"""Interpolate 2–4 poses or 5/7 points with a rational motion curve.
Parameters
----------
poses_or_points
Sequence of input poses or points. Supported types are
:class:`.DualQuaternion`, :class:`.TransfMatrix` and
:class:`.PointHomogeneous`. Supported counts are 2, 3, 4, 5 or 7.
lambda_val, optional
Lambda parameter used for cubic interpolation with four poses.
motion_family, optional
Motion family selector (0 or 1) for the cubic 4-pose case.
Returns
-------
RationalCurve
Rational motion curve representing the interpolant.
"""
# check number of poses
if not ((2 <= len(poses_or_points) <= 5) or len(poses_or_points) == 7):
raise ValueError('Only 2-4 poses or 5 or 7 points can be interpolated.')
p0_array = numpy.asarray(poses_or_points[0].array(), dtype='float64')
# check if the first pose is the identity matrix
if ((isinstance(poses_or_points[0], TransfMatrix)
and not numpy.allclose(p0_array, TransfMatrix().matrix))
or (isinstance(poses_or_points[0], DualQuaternion)
and not numpy.allclose(p0_array, DualQuaternion().array()))):
if len(poses_or_points) == 4:
raise ValueError('The first pose must be the identity matrix')
elif len(poses_or_points) == 3:
warn('The first pose IS NOT the identity. The interpolation '
'results may be unstable. They will yield non-univariate '
'polynomial which has to be transformed to visually '
'interpolate the curve.',
UserWarning)
rational_poses = []
# convert poses to rational dual quaternions
for pose in poses_or_points:
if isinstance(pose, TransfMatrix):
rational_poses.append(DualQuaternion(pose.matrix2dq(), rational=True))
elif isinstance(pose, DualQuaternion) and not pose.is_rational:
rational_poses.append(DualQuaternion(pose.array(), rational=True))
elif isinstance(pose, DualQuaternion) and pose.is_rational:
rational_poses.append(pose)
elif isinstance(pose, PointHomogeneous):
rational_poses.append(pose)
else:
raise TypeError('The given poses must be either TransfMatrix,'
' DualQuaternion, or PointHomogeneous.')
lambda_val = sympy.Rational(lambda_val)
# normalize the DQ poses on Study quadric
if len(rational_poses) != 5 and len(rational_poses) != 7:
rational_poses = [pose.back_projection() for pose in rational_poses]
# interpolate the rational poses
if len(rational_poses) == 4:
curve_eqs = MotionInterpolation.interpolate_cubic(rational_poses,
lambda_val=lambda_val,
motion_family=motion_family)
return RationalCurve(curve_eqs)
elif len(rational_poses) == 3:
curve_eqs = MotionInterpolation.interpolate_quadratic(rational_poses)
return RationalCurve(curve_eqs)
elif len(rational_poses) == 2:
curve_eqs = MotionInterpolation.interpolate_quadratic_2_poses(rational_poses)
return RationalCurve(curve_eqs)
elif len(rational_poses) == 5:
curve_eqs = MotionInterpolation.interpolate_points_quadratic(rational_poses)
return RationalCurve(curve_eqs)
elif len(rational_poses) == 7:
curve_eqs = MotionInterpolation.interpolate_points_cubic(rational_poses)
return RationalCurve(curve_eqs)
[docs]
@staticmethod
def interpolate_quadratic(poses: list[DualQuaternion]) -> list[sympy.Poly]:
"""Interpolate three rational poses by a quadratic motion curve.
Parameters
----------
poses
Sequence of three :class:`.DualQuaternion` poses.
Returns
-------
list[sympy.Poly]
Symbolic polynomial equations (SymPy) describing the motion curve.
"""
alpha = sympy.Symbol('alpha')
omega = sympy.Symbol('omega')
t = sympy.Symbol('t')
p0 = poses[0].array()
p1 = poses[1].array()
p2 = poses[2].array()
c = alpha * p2 + (p1 - alpha * p2 - omega * p0) * t + omega * p0 * t**2
symbolic_curve = DualQuaternion(c, rational=True)
# apply Stydy condition, i.e. obtain epsilon norm of the curve
study_norm = symbolic_curve.norm()
# simplify the norm
study_norm = study_norm[4] / (t * (t - sympy.Rational(1)))
study_norm = sympy.simplify(study_norm)
# obtain the equations for alpha and omega
eq0 = study_norm.subs(t, 0)
eq1 = study_norm.subs(t, 1)
# solve the equations symbolically
sols = sympy.solve([eq0, eq1], [alpha, omega], dict=True)
# get non zero solution
nonzero_sol = None
for sol in sols:
if sol[alpha] and sol[omega]:
if (not (sol[alpha] == 0 and sol[omega] == 0)
and sol[alpha].is_number
and sol[omega].is_number):
nonzero_sol = sol
if nonzero_sol is None:
raise ValueError('Interpolation failed for the given poses.')
al = nonzero_sol[alpha]
om = nonzero_sol[omega]
# obtain the interpolated curve
c_interp = al * p2 + (p1 - al * p2 - om * p0) * t + om * p0 * t**2
# list of polynomials
poly = [sympy.Poly(el, t) for el in c_interp]
return poly
[docs]
@staticmethod
def interpolate_quadratic_numerically(poses: list[DualQuaternion]) -> numpy.ndarray:
"""Numeric quadratic interpolation for three poses.
Parameters
----------
poses
Sequence of three :class:`.DualQuaternion` poses.
Returns
-------
numpy.ndarray
Numeric coefficient array for the quadratic motion curve.
"""
p0 = poses[0].array()
p1 = poses[1].array()
p2 = poses[2].array()
# split dual quaternion into real (first 4) and dual (last 4) parts
p0_r, p0_d = p0[:4], p0[4:]
p1_r, p1_d = p1[:4], p1[4:]
p2_r, p2_d = p2[:4], p2[4:]
# compute the common denominator
denom = numpy.dot(p2_r, p0_d) + numpy.dot(p0_r, p2_d)
if numpy.abs(denom) < 1e-12:
raise ValueError("Interpolation failed: denominator nearly zero. The poses p0 and p2 are dependent.")
# parameters from the Study condition
omega = (numpy.dot(p2_r, p1_d) + numpy.dot(p1_r, p2_d)) / denom
alpha = (numpy.dot(p1_r, p0_d) + numpy.dot(p0_r, p1_d)) / denom
c0 = alpha * p2
c1 = p1 - alpha * p2 - omega * p0
c2 = omega * p0
# return array of coefficients
return numpy.stack([c2, c1, c0], axis=1)
[docs]
@staticmethod
def interpolate_quadratic_2_poses(poses: list[DualQuaternion]) -> list[sympy.Poly]:
"""Quadratic interpolation when only two poses are provided.
The routine augments the input by a third helper pose (identity or
randomized/optimized) to form a solvable quadratic interpolation.
Parameters
----------
poses
Sequence of two :class:`.DualQuaternion` poses.
Returns
-------
list[sympy.Poly]
Symbolic polynomial representation of the interpolated curve.
"""
try:
return MotionInterpolation.interpolate_quadratic_2_poses_optimized(poses)
except Exception:
print('Failed to interpolate with a random pose optimized for shortest '
'path length. Trying to interpolate with other random poses...')
return MotionInterpolation.interpolate_quadratic_2_poses_random(poses)
[docs]
@staticmethod
def interpolate_quadratic_2_poses_random(poses: list[DualQuaternion]
) -> list[sympy.Poly]:
"""Randomized search for a third pose used in quadratic interpolation.
Parameters
----------
poses
Sequence of two :class:`.DualQuaternion` poses.
Returns
-------
list[sympy.Poly]
Chosen symbolic polynomial set for the interpolation.
"""
# Calculate the mid point between the two poses
p0 = PointHomogeneous(poses[0].array())
p1 = PointHomogeneous(poses[1].array())
mid_p = p0.linear_interpolation(p1, 0.5)
mid_pose = DualQuaternion(mid_p.array())
# get mean value of mid_pose coordinates
mean = sum(mid_pose.array()) / len(mid_pose.array())
shortest_curve_length = float('inf')
shortest_set = None
best_pose = None
for i in range(1, 10):
additional_pose = DualQuaternion(
DualQuaternion.random_on_study_quadric(
mean * 0.3 * i).array(), rational=True).back_projection()
new_poses = deepcopy(poses)
new_poses.append(additional_pose)
try:
polynomial_set = MotionInterpolation.interpolate_quadratic(
new_poses)
except Exception:
polynomial_set = None
# If interpolation was successful, check if it's the best so far
if polynomial_set is not None:
new_curve_length = RationalCurve(polynomial_set).get_path_length(
num_of_points=500)
if new_curve_length < shortest_curve_length:
shortest_set = polynomial_set
best_pose = additional_pose
shortest_curve_length = new_curve_length
if shortest_set is not None:
print('Chosen pose:')
print(best_pose)
return shortest_set
else: # if no solution was found
raise ValueError('Interpolation failed for the given poses.')
[docs]
@staticmethod
def interpolate_quadratic_2_poses_optimized(poses: list[DualQuaternion],
max_iter: int = 0,
) -> list[sympy.Poly]:
"""Optimized search for a helper third pose for quadratic interpolation.
.. deprecated::
:meth:`interpolate_quadratic_2_poses_optimized` is deprecated and will be
removed in a future release. Use
:meth:`interpolate_quadratic_2_poses_random` instead.
Parameters
----------
poses
Sequence of two :class:`.DualQuaternion` poses.
max_iter, optional
Maximum number of optimization iterations (0 = run until tolerance).
Returns
-------
list[sympy.Poly]
Symbolic polynomial representation for the interpolated curve.
"""
warn(
'interpolate_quadratic_2_poses_optimized is deprecated and will be '
'removed in a future release. Use interpolate_quadratic_2_poses_random '
'instead.',
DeprecationWarning,
stacklevel=2,
)
from scipy.optimize import minimize # lazy import
mid_pose = DualQuaternion.random_on_study_quadric()
mid_pose_tr = TransfMatrix(mid_pose.dq2matrix())
x0 = mid_pose_tr.t
def objective_func(x):
optim_pose = mid_pose_tr
optim_pose.t = x
new_poses = deepcopy(poses)
new_poses.append(DualQuaternion(optim_pose.matrix2dq()).back_projection())
length = RationalCurve.from_coeffs(
MotionInterpolation.interpolate_quadratic_numerically(new_poses)
).get_path_length(num_of_points=300)
return length
if max_iter == 0:
res = minimize(objective_func, x0, tol=1e-3)
else:
res = minimize(objective_func, x0, tol=1e-3, options={'maxiter': max_iter})
optimal_pose = mid_pose_tr
optimal_pose.t = res.x
optimal_pose_projected = DualQuaternion(
optimal_pose.matrix2dq(), rational=True).back_projection()
print('Optimal pose:')
print(optimal_pose_projected)
poses.append(optimal_pose_projected)
return MotionInterpolation.interpolate_quadratic(
[DualQuaternion(pose.array(), rational=True).back_projection() for pose in poses])
[docs]
@staticmethod
def interpolate_cubic(poses: list[DualQuaternion],
lambda_val: Union[float, int] = 0,
motion_family: int = 0) -> list[sympy.Poly]:
"""Interpolate four rational poses with a cubic motion curve.
The method computes a cubic curve lying on the intersection of the
projective span of the poses and the Study quadric.
Parameters
----------
poses
Sequence of four :class:`.DualQuaternion` poses.
lambda_val, optional
Lambda parameter selecting a family solution.
motion_family, optional
Motion family selector (0 or 1).
Returns
-------
list[sympy.Poly]
Symbolic polynomial equations for the cubic motion curve.
Raises
------
ValueError
If no valid interpolation solution exists.
See Also
--------
:ref:`interpolation_background`
Background on interpolation methods
"""
# obtain additional dual quaternions k1, k2
poses = deepcopy(poses)
try:
k = MotionInterpolation._obtain_k_dq(poses)
except Exception:
raise ValueError('Interpolation has no solution.')
# solve for t[i] - the parameter of the rational motion curve for i-th pose
t_sols = MotionInterpolation._solve_for_t(poses, k[motion_family])
# Lagrange's interpolation part
# lambdas for interpolation - scalar multiples of the poses
lams = sympy.symbols("lams1:5")
parametric_points = [sympy.Matrix(poses[0].array()),
sympy.Matrix(lams[0] * poses[1].array()),
sympy.Matrix(lams[1] * poses[2].array()),
sympy.Matrix(lams[2] * poses[3].array())]
# obtain the Lagrange interpolation for poses p0, p1, p2, p3
interp = MotionInterpolation._lagrange_poly_interpolation(parametric_points)
t = sympy.symbols("t:4")
x = sympy.symbols("x")
# to avoid calculation with infinity, substitute t[i] with 1/t[i]
temp = [element.subs(t[0], 0) for element in interp]
temp2 = [element.subs(x, 1 / x) for element in temp]
temp3 = [sympy.together(element * x ** 3) for element in temp2]
temp4 = [sympy.together(element.subs({t[1]: 1 / t_sols[0],
t[2]: 1 / t_sols[1],
t[3]: 1 / t_sols[2]}))
for element in temp3]
# obtain additional parametric pose p4
lam = sympy.symbols("lam")
poses.append(DualQuaternion([lam, 0, 0, 0, 0, 0, 0, 0]) - k[motion_family])
eqs_lambda = [element.subs(x, lam) - lams[-1] * poses[-1].array()[i]
for i, element in enumerate(temp4)]
sols_lambda = sympy.solve(eqs_lambda, lams, domain='RR')
# obtain the family of solutions
poly = [element.subs(sols_lambda) for element in temp4]
# choose one solution by setting lambda, default lam=0
poly = [element.subs(lam, lambda_val) for element in poly]
t = sympy.Symbol("t")
poly = [element.subs(x, t) for element in poly]
try:
curve = [sympy.Poly(element, t) for element in poly]
except Exception:
print('Rational evaluation failed, performing numerical evaluation...')
curve = [sympy.Poly(element.evalf(), t) for element in poly]
return curve
[docs]
@staticmethod
def interpolate_cubic_numerically(poses: list[DualQuaternion],
k_idx: int = 0,
lambda_val: Union[float, int] = 0) -> numpy.ndarray:
"""Numeric cubic interpolation for four poses.
Parameters
----------
poses
Sequence of four :class:`.DualQuaternion` poses.
k_idx, optional
Index selecting which auxiliary k-dual-quaternion to use.
lambda_val, optional
Lambda parameter for the cubic family.
Returns
-------
numpy.ndarray
Numeric coefficient array for the cubic motion curve.
Raises
------
ValueError
If the interpolation problem has no valid solution.
See Also
--------
:ref:`interpolation_background`
Background on interpolation methods.
"""
poses = deepcopy(poses)
lambda_val = 1e-16 if abs(lambda_val) < 1e-16 else lambda_val
p0 = poses[0].array()
p1 = poses[1].array()
p2 = poses[2].array()
p3 = poses[3].array()
x3_sols = motion_interp_x3(p1, p2, p3)
k = []
for x3 in x3_sols:
x2 = -(p1[0] * p3[4] * x3 - p1[4] * p3[0] * x3 - p1[4]) / (
p1[0] * p2[4] - p1[4] * p2[0])
x1 = (p2[0] * p3[4] * x3 - p2[4] * p3[0] * x3 - p2[4]) / (
p1[0] * p2[4] - p1[4] * p2[0])
k_array = p0 + x1 * p1 + x2 * p2 + x3 * p3
k.append(DualQuaternion(k_array))
# # solve for t[i] - the parameter of the rational motion curve for i-th pose
# t_sols = MotionInterpolation._solve_for_t_numerically(poses, k)
study_cond_mat = numpy.array([[0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0]])
# Calculate identity DQ product with study matrix once
k0_array = k[k_idx].array()
study_cond = [study_cond_mat @ poses[i].array() for i in range(1, 4)]
# Pre-compute results for the divisors
denominators = numpy.array([p0 @ study_cond[i]
for i in range(0, 3)])
# Pre-compute results for the numerators
numerators = numpy.array([k0_array @ study_cond[i]
for i in range(0, 3)])
# Perform division for all three parameters at once
safe_denominators = numpy.where(denominators == 0, 1e-16, denominators)
t_sols = numerators / safe_denominators
# Lagrange's interpolation part
# lambdas for interpolation - scalar multiples of the poses
lams = sympy.symbols("lams1:5")
x = sympy.symbols("x")
parametric_points = [numpy.array(poses[0].array()),
numpy.array(lams[0] * poses[1].array()),
numpy.array(lams[1] * poses[2].array()),
numpy.array(lams[2] * poses[3].array())]
# obtain the Lagrange interpolation for poses p0, p1, p2, p3
interp = MotionInterpolation.lagrange_interpolation_numerically(
parametric_points,
1 / x,
[0.0, 1/t_sols[0], 1/t_sols[1], 1/t_sols[2]])
temp_evaluated = [el * x ** 3 for el in interp]
# obtain additional parametric pose p4
poses.append(DualQuaternion([lambda_val, 0, 0, 0, 0, 0, 0, 0]) - k[k_idx])
eqs_lambda = [element.subs(x, lambda_val) - lams[-1] * poses[-1].array()[i]
for i, element in enumerate(temp_evaluated)]
try:
# solve the equations
sols_lambda = sympy.nsolve(eqs_lambda, lams, [1., 1., -1., -1.], dict=True)
except Exception:
raise ValueError('Interpolation has no solution. Alter the input poses.')
coeffs = [[expr.coeff(x, i) for i in range(3, -1, -1)] for expr in
(sympy.expand(element.subs(sols_lambda[0])) for element in temp_evaluated)]
return numpy.array(coeffs, dtype='float64')
@staticmethod
def _obtain_k_dq(poses: list[DualQuaternion]) -> list[DualQuaternion]:
"""Compute auxiliary dual quaternions k1 and k2 for cubic interpolation.
Parameters
----------
poses
Sequence of four :class:`.DualQuaternion` poses.
Returns
-------
list[DualQuaternion]
Two auxiliary dual quaternions used in the cubic construction.
"""
x = sympy.symbols("xx:3")
k = DualQuaternion(poses[0].array() + x[0] * poses[1].array()
+ x[1] * poses[2].array() + x[2] * poses[3].array())
eqs = [k[0], k[4], k.norm().array()[4]]
sol = sympy.solve(eqs, x, domain='RR')
k_as_expr = [sympy.Expr(el) for el in k]
k1 = [el.subs({x[0]: sol[0][0], x[1]: sol[0][1], x[2]: sol[0][2]})
for el in k_as_expr]
k2 = [el.subs({x[0]: sol[1][0], x[1]: sol[1][1], x[2]: sol[1][2]})
for el in k_as_expr]
k1_dq = DualQuaternion([el.args[0] for el in k1])
k2_dq = DualQuaternion([el.args[0] for el in k2])
return [k1_dq, k2_dq]
@staticmethod
def _solve_for_t(poses: list[DualQuaternion],
k: DualQuaternion) -> list:
"""Solve for the curve parameters t[i] corresponding to each pose.
Parameters
----------
poses
List of :class:`.DualQuaternion` poses (indices 1..3 are used).
k
Auxiliary :class:`.DualQuaternion` used in the solving system.
Returns
-------
list
Solutions for the parameters t[i].
"""
t = sympy.symbols("t:3")
study_cond_mat = sympy.Matrix([[0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0]])
t_dq = [DualQuaternion([t[i], 0, 0, 0, 0, 0, 0, 0]) for i in range(3)]
eqs = [sympy.Matrix((t_dq[0] - k).array()).transpose() @ study_cond_mat
@ sympy.Matrix(poses[1].array()),
sympy.Matrix((t_dq[1] - k).array()).transpose() @ study_cond_mat
@ sympy.Matrix(poses[2].array()),
sympy.Matrix((t_dq[2] - k).array()).transpose() @ study_cond_mat
@ sympy.Matrix(poses[3].array())]
sols_t = sympy.solve(eqs, t)
# covert to list and return
return [val for i, (key, val) in enumerate(sols_t.items())]
@staticmethod
def _lagrange_polynomial(degree, index, x, t):
"""Return the Lagrange basis polynomial for given nodes.
Parameters
----------
degree
Degree of the polynomial.
index
Index of the basis polynomial.
x
Indeterminate symbol.
t
Sequence of node symbols.
Returns
-------
sympy.Expr
The Lagrange basis polynomial expression.
"""
lagrange_poly = 1
for i in range(degree + 1):
if i != index:
lagrange_poly *= (x - t[i]) / (t[index] - t[i])
return lagrange_poly
@staticmethod
def _lagrange_poly_interpolation(poses: list[sympy.Matrix]):
"""Compute Lagrange polynomial interpolation for symbolic poses.
Parameters
----------
poses
List of SymPy Matrix poses representing parametric points.
Returns
-------
sympy.Matrix
Matrix of symbolic polynomial expressions forming the interpolant.
"""
# indeterminate x
x = sympy.symbols('x')
# interpolation nodes
t = sympy.symbols("t:4")
degree = len(poses) - 1
result = sympy.Matrix([0, 0, 0, 0, 0, 0, 0, 0])
for i in range(degree + 1):
result += poses[i] * MotionInterpolation._lagrange_polynomial(degree,
i, x, t)
return result
[docs]
@staticmethod
def lagrange_interpolation_numerically(poses, x, t):
"""Numerical Lagrange interpolation for 8-dimensional pose arrays.
Parameters
----------
poses
Array-like of 8-dimensional pose vectors.
x
Evaluation location (scalar or symbolic compatible value).
t
Interpolation nodes.
Returns
-------
numpy.ndarray
Interpolated 8-dimensional vector.
"""
def lagrange_polynomial(i, x, t):
p = 1.0
for j in range(len(t)):
if j != i:
p *= (x - t[j]) / (t[i] - t[j])
return p
poses = numpy.asarray(poses)
n = len(t)
# Ensure that the number of poses matches the number of nodes
if poses.shape[0] != n:
raise ValueError(
"The number of poses must equal the number of interpolation nodes.")
result = numpy.zeros(8)
for i in range(n):
li = lagrange_polynomial(i, x, t)
result = result + poses[i] * li
return result
[docs]
@staticmethod
def interpolate_points_quadratic(points: list[PointHomogeneous],
return_numeric: bool = False) -> (
Union[list[sympy.Poly], numpy.ndarray]):
"""
Interpolates the given 5 points by a quadratic curve in SE(3).
:param list[PointHomogeneous] points: The points to interpolate.
:param bool return_numeric: If True, the result will be returned as numpy
polynomials, otherwise as sympy polynomials.
:return: The rational motion curve.
:rtype: Union[list[sympy.Poly], numpy.ndarray]
"""
if not all(isinstance(p, PointHomogeneous) for p in points):
raise TypeError('The given points must be PointHomogeneous.')
if len(points) != 5:
raise ValueError('The number of points must be 5.')
# check if the points are Sympy Rational
perform_rational = True if all(p.is_rational for p in points) else False
points = [p if p[0] == 1 else p.normalize() for p in points]
# map to Quaternions, divide by -2 (Study mapping from 3D)
# and add 0 to the real part
a0, a1, a2, a3, a4 = [Quaternion([0,
p.array()[1] / -2,
p.array()[2] / -2,
p.array()[3] / -2]) for p in points]
d41 = a4 - a1
d21 = a2 - a1
d43 = a4 - a3
d23 = a2 - a3
d10 = a1 - a0
d30 = a3 - a0
d32 = a3 - a2
d14 = a1 - a4
if numpy.allclose(float((d43.inv() * d32 * d21.inv() * d14)[0]), -3.0):
raise ValueError("Not possible to interpolate")
w0 = Quaternion()
w2 = (-9 * d41.inv() * d21 - 3 * d43.inv() * d23).inv() * (
9 * d41.inv() * d10 - d43.inv() * d30) * w0
w4 = (-1 * d21.inv() * d41 - 3 * d23.inv() * d43).inv() * (
3 * d21.inv() * d10 + d23.inv() * d30) * w0
u0 = w0 / 2
p0 = a0
u1 = (w0 + 2*w2 + w4) / (-4)
p1 = (a0 * w0 + 2 * a2 * w2 + a4 * w4) * u1.inv() / (-4)
u2 = w4 / 2
p2 = a4
# get the control points of Bezier curve from constructed dual quaternions
cp0 = PointHomogeneous(numpy.concatenate((u0.array(), (p0 * u0).array())))#,
# rational=perform_rational)
cp1 = PointHomogeneous(numpy.concatenate((u1.array(), (p1 * u1).array())))#,
# rational=perform_rational)
cp2 = PointHomogeneous(numpy.concatenate((u2.array(), (p2 * u2).array())))#,
# rational=perform_rational)
if return_numeric:
return RationalBezier.get_numerical_coeffs([cp0, cp1, cp2])
else:
return RationalBezier([cp0, cp1, cp2]).set_of_polynomials
[docs]
@staticmethod
def interpolate_points_cubic(points: list[PointHomogeneous],
return_numeric: bool = False) -> (
Union[list[sympy.Poly], numpy.ndarray]):
"""
Interpolates the given 7 points by a cubic curve in SE(3).
:param list[Union[sympy.Poly, numpy.polynomial.Polynomial]] points: The points
to interpolate.
:param bool return_numeric: If True, the result will be returned as numpy
polynomials, otherwise as sympy polynomials.
:return: The rational motion curve.
:rtype: list[Union[sympy.Poly, numpy.polynomial.Polynomial]]
"""
if not all(isinstance(p, PointHomogeneous) for p in points):
raise TypeError('The given points must be PointHomogeneous.')
if len(points) != 7:
raise ValueError('The number of points must be 7.')
points = [p if p[0] == 1 else p.normalize() for p in points]
# Check if the points are Sympy Rational
perform_rational = True if all(p.is_rational for p in points) else False
# map to Quaternions, divide by -2 (Study mapping from 3D)
# and add 0 to the real part
a0, a1, a2, a3, a4, a5, a6 = [Quaternion([0,
p.array()[1] / -2,
p.array()[2] / -2,
p.array()[3] / -2]) for p in points]
def q_prod(q0, q1, q2, q3):
return q0.inv() * q1 - q2.inv() * q3
f12 = 15
f14 = 5
f16 = 3
f18 = -15
f22 = 9
f24 = -9
f26 = -3
f28 = 3
f32 = -5
f34 = -15
f36 = 15
f38 = -3
c12 = f12 * (a2 - a1)
c14 = f14 * (a4 - a1)
c16 = f16 * (a6 - a1)
c22 = f22 * (a2 - a3)
c24 = f24 * (a4 - a3)
c26 = f26 * (a6 - a3)
c32 = f32 * (a2 - a5)
c34 = f34 * (a4 - a5)
c36 = f36 * (a6 - a5)
c18 = f18 * (a1 - a0)
c28 = f28 * (a3 - a0)
c38 = f38 * (a5 - a0)
e24 = q_prod(c12, c14, c22, c24)
e26 = q_prod(c12, c16, c22, c26)
e28 = q_prod(c12, c18, c22, c28)
e34 = q_prod(c12, c14, c32, c34)
e36 = q_prod(c12, c16, c32, c36)
e38 = q_prod(c12, c18, c32, c38)
r24 = q_prod(e26, e24, e36, e34)
r28 = q_prod(e26, e28, e36, e38)
r36 = q_prod(e24, e26, e34, e36)
r38 = q_prod(e24, e28, e34, e38)
w0 = Quaternion()
if perform_rational:
w0 = Quaternion([sympy.Rational(coord) for coord in w0.array()])
w4 = r24.inv() * r28
w6 = r36.inv() * r38
w2 = c12.inv() * (c18 - c14 * w4 - c16 * w6)
u0 = -2 * w0 / 9
p0 = a0
u1 = 5 * w0 / 27 + 2 * w2 / 9 + w4 / 9 + 2 * w6 / 27
p1 = ((5 * a0 * w0 /27 + 2 * a2 * w2 / 9 + a4 * w4 / 9 + 2 * a6 * w6 / 27)
* u1.inv())
u2 = -2 * w0 / 27 - 1 * w2 / 9 - 2 * w4 / 9 - 5 * w6 / 27
p2 = ((-2 * a0 * w0 / 27 -1 * a2 * w2 / 9 -2 * a4 * w4 / 9 -5 * a6 * w6 / 27)
* u2.inv())
u3 = 2 * w6 / 9
p3 = a6
# get the control points of Bezier curve from constructed dual quaternions
cp0 = PointHomogeneous(numpy.concatenate((u0.array(), (p0 * u0).array())))#,
# rational=perform_rational)
cp1 = PointHomogeneous(numpy.concatenate((u1.array(), (p1 * u1).array())))#,
# rational=perform_rational)
cp2 = PointHomogeneous(numpy.concatenate((u2.array(), (p2 * u2).array())))#,
# rational=perform_rational)
cp3 = PointHomogeneous(numpy.concatenate((u3.array(), (p3 * u3).array())))#,
# rational=perform_rational)
if return_numeric:
return RationalBezier.get_numerical_coeffs([cp0, cp1, cp2, cp3])
else:
return RationalBezier([cp0, cp1, cp2, cp3]).set_of_polynomials