def test_cubic(self):
t = [0., 1., 2.]
x = np.diag([4., 5., 6.])
periodic_end = False
# Just test the spelling for these.
pp1 = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
breaks=t, samples=x, periodic_end=periodic_end)
pp2 = PiecewisePolynomial.CubicHermite(
breaks=t, samples=x, samples_dot=np.identity(3))
pp3 = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
breaks=t, samples=x, sample_dot_at_start=[0., 0., 0.],
sample_dot_at_end=[0., 0., 0.])
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)
def main():
loadedTrajs = lcm_trajectory.LcmTrajectory()
# loadedTrajs.LoadFromFile(
# "/home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/jumping_0.15h_0.3d_processed")
# loadedTrajs.LoadFromFile(
# "/home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/walking_0.16.0_processed")
loadedTrajs.LoadFromFile(
"/home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/running_0.25_processed")
lcm_left_foot_traj = loadedTrajs.GetTrajectory("left_foot_trajectory0")
lcm_right_foot_traj = loadedTrajs.GetTrajectory("right_foot_trajectory0")
lcm_pelvis_traj = loadedTrajs.GetTrajectory("pelvis_trans_trajectory0")
# lcm_pelvis_traj = loadedTrajs.GetTrajectory("pelvis_rot_trajectory0")
# import pdb; pdb.set_trace()
# left_foot_traj = PiecewisePolynomial.CubicHermite(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints[0:6],
# lcm_left_foot_traj.datapoints[3:9])
# right_foot_traj = PiecewisePolynomial.CubicHermite(lcm_right_foot_traj.time_vector,
# lcm_right_foot_traj.datapoints[0:6],
# lcm_right_foot_traj.datapoints[3:9])
# pelvis_traj = PiecewisePolynomial.CubicHermite(lcm_pelvis_traj.time_vector, lcm_pelvis_traj.datapoints[0:6],
# lcm_pelvis_traj.datapoints[3:9])
x_slice = slice(0,3)
xdot_slice = slice(3,6)
left_foot_traj = PiecewisePolynomial.CubicHermite(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints[x_slice],
lcm_left_foot_traj.datapoints[xdot_slice])
right_foot_traj = PiecewisePolynomial.CubicHermite(lcm_right_foot_traj.time_vector,
lcm_right_foot_traj.datapoints[x_slice],
lcm_right_foot_traj.datapoints[xdot_slice])
pelvis_traj = PiecewisePolynomial.CubicHermite(lcm_pelvis_traj.time_vector, lcm_pelvis_traj.datapoints[x_slice],
lcm_pelvis_traj.datapoints[xdot_slice])
for mode in range(1, 6):
lcm_left_foot_traj = loadedTrajs.GetTrajectory("left_foot_trajectory" + str(mode))
lcm_right_foot_traj = loadedTrajs.GetTrajectory("right_foot_trajectory" + str(mode))
lcm_pelvis_traj = loadedTrajs.GetTrajectory("pelvis_trans_trajectory" + str(mode))
left_foot_traj.ConcatenateInTime(
PiecewisePolynomial.CubicHermite(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints[x_slice],
lcm_left_foot_traj.datapoints[xdot_slice]))
right_foot_traj.ConcatenateInTime(
PiecewisePolynomial.CubicHermite(lcm_right_foot_traj.time_vector, lcm_right_foot_traj.datapoints[x_slice],
lcm_right_foot_traj.datapoints[xdot_slice]))
pelvis_traj.ConcatenateInTime(
PiecewisePolynomial.CubicHermite(lcm_pelvis_traj.time_vector, lcm_pelvis_traj.datapoints[x_slice],
lcm_pelvis_traj.datapoints[xdot_slice]))
# plt.figure('accel')
# plt.plot(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints.T[:,2:3])
# plt.plot(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints.T[:,5:6])
# plt.plot(times, accel[:, -1])
# plt.figure("left_foot pos")
# plt.plot(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints.T[:,0:3])
# plt.legend(['x','y','z'])
# plt.figure("left_foot vel")
# plt.plot(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints.T[:,3:6])
# plt.legend(['x','y','z'])
# plt.figure("right_foot pos")
# plt.plot(lcm_right_foot_traj.time_vector, lcm_right_foot_traj.datapoints.T[:,0:3])
# plt.legend(['x','y','z'])
# plt.figure("right_foot vel")
# plt.plot(lcm_right_foot_traj.time_vector, lcm_right_foot_traj.datapoints.T[:,3:6])
# plt.legend(['x','y','z'])
plot_trajectory(pelvis_traj, 'pelvis')
# plot_trajectory(left_foot_traj, 'left_foot')
# plot_trajectory(right_foot_traj, 'right_foot')
plt.show()
