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()