def test_interpolate_subgaits_wrong_parameter(self):
# should be 0 <= parameter <= 1
base_subgait, other_subgait = self.load_interpolatable_subgaits_ik()
with self.assertRaises(ValueError):
Subgait.interpolate_subgaits(base_subgait,
other_subgait,
2,
use_foot_position=True)
def test_interpolate_subgaits_interpolated(self):
# test whether each setpoint is correctly interpolated
parameter = 0.4
base_subgait, other_subgait = self.load_interpolatable_subgaits()
new_subgait = Subgait.interpolate_subgaits(base_subgait,
other_subgait,
parameter,
use_foot_position=False)
for i, joint in enumerate(new_subgait.joints):
for j, setpoint in enumerate(joint.setpoints):
base_setpoint = base_subgait.joints[i].setpoints[j]
other_setpoint = other_subgait.joints[i].setpoints[j]
self.assertAlmostEqual(
base_setpoint.time * (1 - parameter) +
parameter * other_setpoint.time,
setpoint.time,
places=4,
)
self.assertAlmostEqual(
base_setpoint.position * (1 - parameter) +
parameter * other_setpoint.position,
setpoint.position,
places=4,
)
self.assertAlmostEqual(
base_setpoint.velocity * (1 - parameter) +
parameter * other_setpoint.velocity,
setpoint.velocity,
places=4,
)
def test_interpolate_subgaits_parameter_one(self):
base_subgait, other_subgait = self.load_interpolatable_subgaits_ik()
new_subgait = Subgait.interpolate_subgaits(base_subgait,
other_subgait,
1,
use_foot_position=True)
self.assertEqual(other_subgait, new_subgait)
def test_interpolate_subgaits_duration(self):
base_subgait, other_subgait = self.load_interpolatable_subgaits()
parameter = 0.2
new_subgait = Subgait.interpolate_subgaits(base_subgait, other_subgait,
parameter)
new_duration = parameter * base_subgait.duration + (
1 - parameter) * other_subgait.duration
self.assertEqual(new_duration, new_subgait.duration)
def test_interpolate_subgaits_duration_ik(self):
base_subgait, other_subgait = self.load_interpolatable_subgaits_ik()
parameter = 0.2
new_subgait = Subgait.interpolate_subgaits(base_subgait,
other_subgait,
parameter,
use_foot_position=True)
new_duration = (parameter * base_subgait.duration +
(1 - parameter) * other_subgait.duration)
self.assertEqual(new_duration, new_subgait.duration)
def test_interpolate_subgaits_wrong_joint_names(self):
base_subgait, other_subgait = self.load_interpolatable_subgaits(
other_version='MV_walk_leftclose_v2_wrong_joint_name')
with self.assertRaises(SubgaitInterpolationError):
Subgait.interpolate_subgaits(base_subgait, other_subgait, 0.5)
def test_interpolate_subgaits_wrong_amount_of_joints(self):
base_subgait, other_subgait = self.load_interpolatable_subgaits(
'right_close', 'MV_walk_rightclose_v2',
'MV_walk_rightclose_v2_seven_joints')
with self.assertRaises(SubgaitInterpolationError):
Subgait.interpolate_subgaits(base_subgait, other_subgait, 0.5)
def test_interpolate_subgaits_parameter_one(self):
base_subgait, other_subgait = self.load_interpolatable_subgaits()
new_subgait = Subgait.interpolate_subgaits(base_subgait, other_subgait,
1)
self.assertEqual(other_subgait, new_subgait)