def test_lagrange_interpolating_polynomial(self):
t = [0., 1., 2.]
x = np.diag([4., 5., 6.])
pp = PiecewisePolynomial.LagrangeInterpolatingPolynomial(times=t,
samples=x)
self.assertEqual(pp.get_number_of_segments(), 1)
np.testing.assert_array_almost_equal(x[:, [1]], pp.value(1.), 1e-12)
def test_matrix_trajectories(self):
A0 = np.array([[1, 2, 3], [4, 5, 6]])
A1 = np.array([[7, 8, 9], [10, 11, 12]])
A2 = np.array([[13, 14, 15], [16, 17, 18]])
t = [0., 1., 2.]
pp = dict()
pp["zoh"] = PiecewisePolynomial.ZeroOrderHold(breaks=t,
samples=[A0, A1, A2])
pp["foh"] = PiecewisePolynomial.FirstOrderHold(breaks=t,
samples=[A0, A1, A2])
pp["hermite"] = PiecewisePolynomial.CubicShapePreserving(
breaks=t, samples=[A0, A1, A2], zero_end_point_derivatives=False)
pp["c1"] = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
breaks=t, samples=[A0, A1, A2], periodic_end=False)
pp["c2"] = PiecewisePolynomial.CubicHermite(
breaks=t,
samples=[A0, A1, A2],
samples_dot=[0 * A0, 0 * A1, 0 * A2])
pp["c3"] = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
breaks=t,
samples=[A0, A1, A2],
sample_dot_at_start=0 * A0,
sample_dot_at_end=0 * A0)
pp["lagrange"] = PiecewisePolynomial.LagrangeInterpolatingPolynomial(
times=t, samples=[A0, A1, A2])
for name, traj in pp.items():
if name == "lagrange":
self.assertEqual(traj.get_number_of_segments(), 1)
else:
self.assertEqual(traj.get_number_of_segments(), 2)
self.assertEqual(traj.start_time(), 0.)
self.assertEqual(traj.end_time(), 2.)
self.assertEqual(traj.rows(), 2)
self.assertEqual(traj.cols(), 3)
# Check the values for the easy cases:
np.testing.assert_equal(A0, pp["zoh"].value(.5))
np.testing.assert_allclose(0.5 * A0 + 0.5 * A1, pp["foh"].value(.5),
1e-15)
