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 empty_subgait(cls, gait_generator, robot, duration=8, gait_type='walk_like', gait_name='test_gait',
subgait_name='test_subgait', version='test_subgait_1', description='Just a simple gait'):
if robot is None:
rospy.logerr('Cannot create gait without a loaded robot.')
return None
joint_list = []
for i in range(0, len(robot.joints)):
urdf_joint = robot.joints[i]
if urdf_joint.type == 'fixed':
rospy.loginfo('Skipping fixed joint ' + urdf_joint.name)
continue
if urdf_joint.limit is None:
rospy.logwarn('Skipping joint ' + urdf_joint.name + ' because it has no limits.')
continue
default_setpoints = [
ModifiableSetpoint(0, 0, 0),
ModifiableSetpoint(duration, 0, 0),
]
joint = ModifiableJointTrajectory(urdf_joint.name,
Limits(urdf_joint.safety_controller.soft_lower_limit,
urdf_joint.safety_controller.soft_upper_limit,
urdf_joint.limit.velocity),
default_setpoints,
duration,
gait_generator,
)
joint_list.append(joint)
return cls(joint_list, duration, gait_type, gait_name, subgait_name, version, description)
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 setUp(self):
self.gait_generator = Mock()
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 = [ModifiableSetpoint(t, 0.5 * t, t) for t in self.times]
self.joint_trajectory = ModifiableJointTrajectory(self.joint_name, self.limits, copy.deepcopy(self.setpoints),
self.duration, self.gait_generator)
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 setUp(self):
self.limits = Limits(0, 1, 2, 3, 4, 5)
def test_unequal_operator(self):
# sixth entry different
self.assertNotEqual(self.limits, Limits(0, 1, 2, 3, 4, 6))
def test_unequal_operator_5(self):
# fifth entry different
self.assertNotEqual(self.limits, Limits(0, 1, 2, 3, 5, 5))
def test_unequal_operator_4(self):
# fourth entry different
self.assertNotEqual(self.limits, Limits(0, 1, 2, 4, 4, 5))
def test_unequal_operator_3(self):
# third entry different
self.assertNotEqual(self.limits, Limits(0, 1, 3, 3, 4, 5))
def test_unequal_operator_2(self):
# second entry different
self.assertNotEqual(self.limits, Limits(0, 2, 2, 3, 4, 5))
def test_unequal_operator_1(self):
# first entry different
self.assertNotEqual(self.limits, Limits(1, 1, 2, 3, 4, 5))
def test_equal_operator(self):
self.assertEqual(self.limits, Limits(0, 1, 2, 3, 4, 5))