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_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 setUp(self):
self.joint_name = 'test_joint'
self.limits = Limits(-1, 1, 2)
self.duration = 2.0
self.times = [0, self.duration / 2.0, self.duration]
self.setpoints = [Setpoint(t, 2 * t, t / 2.0) for t in self.times]
self.joint_trajectory = JointTrajectory(self.joint_name, self.limits,
self.setpoints, self.duration)
def test_invalid_boundary_points_nonzero_end_nonzero_speed(self):
# Last setpoint at t = 1 =/= duration has nonzero speed.
setpoints = [
Setpoint(0.5 * t, (self.duration - t), t / 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())
def test_invalid_boundary_points_nonzero_start_nonzero_speed(self):
# First setpoint at t = 1 has nonzero speed.
setpoints = [
Setpoint(0.5 * t + 1, (self.duration - t), (self.duration - t) * 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_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(0.5 * t + 0.5, (self.duration - t), t * 2 - t**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_interpolate_trajectories_correct_duration(self):
parameter = 0.5
other_duration = self.duration + 1
other_times = [0, other_duration / 2.0, other_duration]
other_setpoints = [Setpoint(t, 2 * t, t / 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 * parameter + (1 - parameter) * other_duration)
def create_subgait_of_trajectory(self, joint_trajectory, subgait_name):
"""Create a subgait using the joint trajectory generated by the capture point pose.
:param joint_trajectory: The capture point pose trajectory
:param subgait_name: the normal subgait name
:return: a populated subgait object generated from the joint trajectory
"""
normal_subgait = deepcopy(self.default_walk[subgait_name])
balance_duration = joint_trajectory.points[-1].time_from_start.to_sec()
balance_joints = []
for joint_index, joint_name in enumerate(joint_trajectory.joint_names):
normal_joint = normal_subgait.get_joint(joint_name)
setpoints = []
for joint_trajectory_point in joint_trajectory.points:
time = joint_trajectory_point.time_from_start.to_sec()
position = joint_trajectory_point.positions[joint_index]
velocity = joint_trajectory_point.velocities[joint_index]
setpoints.append(Setpoint(time, position, velocity))
balance_joints.append(
JointTrajectory(joint_name, normal_joint.limits, setpoints,
balance_duration))
balance_subgait = BalanceGait.to_subgait(balance_joints,
balance_duration,
subgait_name=subgait_name)
return balance_subgait
def test_invalid_joint_transition_velocity(self):
next_setpoints = [
Setpoint(t, 2 * (self.duration - t), (self.duration - t))
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_final_position(self):
position = 1.0
subgait = Subgait(
[
JointTrajectory(
"test",
Limits(0.0, 0.0, 0.0),
[
Setpoint(0.0, 0.0, 0.0),
Setpoint(0.5, position, 0.0),
],
1.0,
)
],
1.0,
)
self.assertDictEqual(subgait.final_position, {"test": position})
def _transition_joints(robot, old_subgait, new_subgait):
"""Calculate a transition trajectory which starts at the old gait and ends with the endpoints of the new gait.
:returns:
list of joints which hold the transition setpoints including position, velocity and duration
"""
max_duration = max(old_subgait.duration, new_subgait.duration)
old_subgait.scale_timestamps_subgait(max_duration)
new_subgait.scale_timestamps_subgait(max_duration)
all_timestamps = old_subgait.get_unique_timestamps(
) + new_subgait.get_unique_timestamps()
all_timestamps = sorted(set(all_timestamps))
old_subgait.create_interpolated_setpoints(all_timestamps)
new_subgait.create_interpolated_setpoints(all_timestamps)
joints = []
for old_joint in old_subgait.joints:
joint_name = old_joint.name
new_joint = new_subgait.get_joint(joint_name)
setpoints = []
number_setpoints = len(new_subgait[0].setpoints)
for transition_index in range(number_setpoints):
factor = transition_index / (number_setpoints - 1.0)
old_setpoint = old_joint[transition_index]
new_setpoint = new_joint[transition_index]
transition_setpoint = TransitionSubgait._transition_setpoint(
old_setpoint, new_setpoint, factor)
setpoints.append(transition_setpoint)
joints.append(
JointTrajectory(joint_name, old_joint.limits, setpoints,
old_joint.duration))
return joints
class JointTrajectoryTest(unittest.TestCase):
def setUp(self):
self.joint_name = "test_joint"
self.limits = Limits(-1, 1, 2)
self.duration = 2.0
self.times = [0, self.duration / 2.0, self.duration]
self.setpoints = [Setpoint(t, 2 * t, t / 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,
(self.times, [2 * t for t in self.times], [t / 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, 2 * (self.duration - t), (self.duration - t) / 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), (self.duration - t) / 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), (self.duration - t))
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(0.5 * t + 0.5, (self.duration - t), t * 2 - t ** 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(0.5 * t + 1, (self.duration - t), (self.duration - t) * 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(0.5 * t, (self.duration - t), t / 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(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 + 1)
self.assertEqual(
setpoint, Setpoint(self.duration + 1, self.setpoints[-1].position, 0)
)
def test_get_interpolated_setpoints_invalid_time_too_low(self):
setpoint = self.joint_trajectory.get_interpolated_setpoint(-1)
self.assertEqual(setpoint, Setpoint(-1, self.setpoints[0].position, 0))
def test_get_interpolated_setpoints_home_subgait(self):
self.joint_trajectory.setpoints = [Setpoint(3, 1, 1)]
setpoint = self.joint_trajectory.get_interpolated_setpoint(1)
self.assertEqual(setpoint, Setpoint(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 + 1
other_times = [0, other_duration / 2.0, other_duration]
other_setpoints = [Setpoint(t, 2 * t, t / 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 * parameter + (1 - parameter) * other_duration,
)