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)