class JointTrajectoryTest(unittest.TestCase): def setUp(self): self.joint_name = "test_joint" self.limits = Limits(-1, 1, 2) self.duration = Duration(seconds=2.0) self.times = [Duration(), self.duration / 2.0, self.duration] self.setpoints = [ Setpoint(t, 2 * t.seconds, t.seconds / 2.0) for t in self.times ] self.joint_trajectory = JointTrajectory(self.joint_name, self.limits, self.setpoints, self.duration) # get_setpoints_unzipped tests def test_get_setpoints_unzipped_time(self): output = self.joint_trajectory.get_setpoints_unzipped() self.assertEqual( output, ( [t.seconds for t in self.times], [2 * t.seconds for t in self.times], [t.seconds / 2.0 for t in self.times], ), ) # set_duration tests def test_set_duration(self): expected = Setpoint( self.joint_trajectory.setpoints[-1].time * 2, self.joint_trajectory.setpoints[-1].position, self.joint_trajectory.setpoints[-1].velocity / 2, ) self.joint_trajectory.set_duration(self.duration * 2) self.assertEqual(self.joint_trajectory.setpoints[-1], expected) # validate_joint_transition() tests def test_valid_joint_transition(self): next_setpoints = [ Setpoint(t, (self.duration - t).seconds * 2, (self.duration - t).seconds / 2.0) for t in self.times ] next_joint_trajectory = JointTrajectory(self.joint_name, self.limits, next_setpoints, self.duration) self.assertTrue( self.joint_trajectory.validate_joint_transition( next_joint_trajectory)) def test_invalid_joint_transition_position(self): next_setpoints = [ Setpoint(t, (self.duration - t).seconds, (self.duration - t).seconds / 2.0) for t in self.times ] next_joint_trajectory = JointTrajectory(self.joint_name, self.limits, next_setpoints, self.duration) self.assertFalse( self.joint_trajectory.validate_joint_transition( next_joint_trajectory)) def test_invalid_joint_transition_velocity(self): next_setpoints = [ Setpoint(t, 2 * (self.duration - t).seconds, (self.duration - t).seconds) for t in self.times ] next_joint_trajectory = JointTrajectory(self.joint_name, self.limits, next_setpoints, self.duration) self.assertFalse( self.joint_trajectory.validate_joint_transition( next_joint_trajectory)) # _validate_boundary_points tests def test_valid_boundary_points_nonzero_start_end_zero_speed(self): # First setpoint at t = 0.5 and last setpoint at t = 1.5 =/= duration have zero speed. setpoints = [ Setpoint( t * 0.5 + Duration(seconds=0.5), (self.duration - t).seconds, t.seconds * 2 - t.seconds**2, ) for t in self.times ] joint_trajectory = JointTrajectory(self.joint_name, self.limits, setpoints, self.duration) self.assertTrue(joint_trajectory._validate_boundary_points()) def test_invalid_boundary_points_nonzero_start_nonzero_speed(self): # First setpoint at t = 1 has nonzero speed. setpoints = [ Setpoint( t * 0.5 + Duration(seconds=1), (self.duration - t).seconds, (self.duration - t).seconds * 2, ) for t in self.times ] joint_trajectory = JointTrajectory(self.joint_name, self.limits, setpoints, self.duration) self.assertFalse(joint_trajectory._validate_boundary_points()) def test_invalid_boundary_points_nonzero_end_nonzero_speed(self): # Last setpoint at t = 1 =/= duration has nonzero speed. setpoints = [ Setpoint(t * 0.5, (self.duration - t).seconds, t.seconds / 2.0) for t in self.times ] joint_trajectory = JointTrajectory(self.joint_name, self.limits, setpoints, self.duration) self.assertFalse(joint_trajectory._validate_boundary_points()) # interpolate_setpoints tests def test_interpolation_start_point(self): interpolated_setpoint = self.joint_trajectory.get_interpolated_setpoint( Duration(seconds=0)) self.assertEqual(interpolated_setpoint, self.setpoints[0]) def test_interpolation_end_point(self): interpolated_setpoint = self.joint_trajectory.get_interpolated_setpoint( self.duration) self.assertEqual(interpolated_setpoint, self.setpoints[-1]) def test_interpolation_mid_point_position(self): interpolated_setpoint = self.joint_trajectory.get_interpolated_setpoint( self.duration / 2) self.assertEqual(interpolated_setpoint, self.setpoints[1]) def test_get_interpolated_setpoints_invalid_time_too_high(self): setpoint = self.joint_trajectory.get_interpolated_setpoint( self.duration + Duration(seconds=1)) self.assertEqual( setpoint, Setpoint(self.duration + Duration(seconds=1), self.setpoints[-1].position, 0), ) def test_get_interpolated_setpoints_invalid_time_too_low(self): setpoint = self.joint_trajectory.get_interpolated_setpoint( Duration(seconds=-1)) self.assertEqual( setpoint, Setpoint(Duration(seconds=-1), self.setpoints[0].position, 0)) def test_get_interpolated_setpoints_home_subgait(self): self.joint_trajectory.setpoints = [Setpoint(Duration(seconds=3), 1, 1)] setpoint = self.joint_trajectory.get_interpolated_setpoint( Duration(seconds=1)) self.assertEqual(setpoint, Setpoint(Duration(seconds=1), 1, 1)) def test_interpolate_trajectories_unequal_limits(self): different_limits = Limits(-2, 3, 2) other_trajectory = JointTrajectory(self.joint_name, different_limits, self.setpoints, self.duration) with self.assertRaises(SubgaitInterpolationError): JointTrajectory.interpolate_joint_trajectories( self.joint_trajectory, other_trajectory, 0.5) def test_interpolate_trajectories_unequal_amount_setpoints(self): other_trajectory = JointTrajectory( self.joint_name, self.limits, [self.setpoints[0], self.setpoints[-1]], self.duration, ) with self.assertRaises(SubgaitInterpolationError): JointTrajectory.interpolate_joint_trajectories( self.joint_trajectory, other_trajectory, 0.5) def test_interpolate_trajectories_correct_duration(self): parameter = 0.5 other_duration = self.duration + Duration(seconds=1) other_times = [Duration(), other_duration / 2.0, other_duration] other_setpoints = [ Setpoint(t, 2 * t.seconds, t.seconds / 2.0) for t in other_times ] other_trajectory = JointTrajectory(self.joint_name, self.limits, other_setpoints, other_duration) new_trajectory = JointTrajectory.interpolate_joint_trajectories( self.joint_trajectory, other_trajectory, parameter) self.assertEqual( new_trajectory.duration, self.duration.weighted_average(other_duration, parameter), )
def test_weighted_average(self): duration_1 = Duration(seconds=4) duration_2 = Duration(seconds=8) self.assertEqual( duration_1.weighted_average(duration_2, 0.75).nanoseconds, 7000000000 )