def test_desired_state_size(self):
t = 0
n = 9
v = 0.1 # m/s
waypoints = trajGen3D.get_helix_waypoints(t, n)
desired_state = trajGen3D.generate_trajectory(1.3, v, waypoints)
self.assertEqual(desired_state.pos.shape, (3, ))
self.assertEqual(desired_state.vel.shape, (3, ))
self.assertEqual(desired_state.acc.shape, (3, ))
des_x_dot, des_y_dot, des_z_dot = desired_state.vel
print "desired_state", desired_state
def test_get_helix_waypoints_value(self):
n = 8
start_t = 0
T = np.linspace(start_t, 2 * np.pi, n)
expected = np.zeros((n, 3))
for i, t in enumerate(T):
expected[i] = np.array([np.cos(t), np.sin(t), t])
waypoints = trajGen3D.get_helix_waypoints(start_t, n)
np.testing.assert_array_equal(waypoints, expected)
def get_full_trajectory():
T = np.linspace(0, 100, 1000)
waypoints = trajGen3D.get_helix_waypoints(0, 9)
print "T", T
print "waypoints", waypoints
full_traj = np.zeros((T.size,3))
for i, t in enumerate(T):
desired_state = trajGen3D.generate_trajectory(t, 0.1, waypoints)
full_traj[i] = desired_state.pos
return full_traj
def test_MST_output_size(self):
t = 0
n = 8
waypoints = trajGen3D.get_helix_waypoints(t, n)
C = trajGen3D.MST(waypoints[:, 0], t)
self.assertEqual(C.shape, (8 * (n - 1), 1))
def test_get_helix_waypoints_size(self):
t = 0
n = 8
waypoints = trajGen3D.get_helix_waypoints(t, n)
expected_shape = (8, 3)
self.assertEqual(waypoints.shape, expected_shape)