def test_inverse_kinematics_position(self):
feet_state = FeetState.from_setpoint_dict(self.setpoint_dict)
new_setpoints = FeetState.feet_state_to_setpoints(feet_state)
for key in new_setpoints.keys():
self.assertAlmostEqual(
new_setpoints[key].position, self.setpoint_dict[key].position, places=4
)
def test_inverse_kinematics_velocity(self):
feet_state = FeetState.from_setpoint_dict(self.setpoint_dict)
new_setpoints = FeetState.feet_state_to_setpoints(feet_state)
for key in new_setpoints.keys():
self.assertAlmostEqual(
new_setpoints[key].velocity,
round_setpoint(self.setpoint_dict[key].velocity),
places=3,
)
def test_inverse_kinematics_reversed_position(self):
right_foot = Foot(Side.right, Vector3d(0.18, 0.08, 0.6), Vector3d(0, 0, 0))
left_foot = Foot(Side.left, Vector3d(0.18, -0.08, 0.6), Vector3d(0, 0, 0))
desired_state = FeetState(right_foot, left_foot, Duration(seconds=0.1))
new_setpoints = FeetState.feet_state_to_setpoints(desired_state)
resulting_position = FeetState.from_setpoint_dict(new_setpoints)
dif_left = (
desired_state.left_foot.position - resulting_position.left_foot.position
)
dif_right = (
desired_state.right_foot.position - resulting_position.right_foot.position
)
self.assertLess(dif_left.norm(), 0.0001)
self.assertLess(dif_right.norm(), 1 / 0.0001)
def test_find_known_position_forward(self):
setpoint_dict = {
"left_ankle": Setpoint(Duration(), 0, 0),
"left_hip_aa": Setpoint(Duration(), 0, 0),
"left_hip_fe": Setpoint(Duration(), pi / 2, 0),
"left_knee": Setpoint(Duration(), 0, 0),
"right_ankle": Setpoint(Duration(), 0, 0),
"right_hip_aa": Setpoint(Duration(), 0, 0),
"right_hip_fe": Setpoint(Duration(), pi / 2, 0),
"right_knee": Setpoint(Duration(), 0, 0),
}
[
l_ul,
l_ll,
l_hl,
l_ph,
r_ul,
r_ll,
r_hl,
r_ph,
base,
] = get_lengths_robot_for_inverse_kinematics(Side.both)
resulting_state = FeetState.from_setpoint_dict(setpoint_dict)
expected_right_foot = Foot(
Side.right,
Vector3d(r_ul + r_ll + r_hl, base / 2.0 + r_ph, 0),
Vector3d(0, 0, 0),
)
expected_left_foot = Foot(
Side.right,
Vector3d(l_ul + l_ll + l_hl, -base / 2.0 - l_ph, 0),
Vector3d(0, 0, 0),
)
expected_state = FeetState(expected_right_foot, expected_left_foot, Duration())
self.assertTrue(
Vector3d.is_close_enough(
expected_state.left_foot.position, resulting_state.left_foot.position
)
)
self.assertTrue(
Vector3d.is_close_enough(
expected_state.right_foot.position, resulting_state.right_foot.position
)
)
def test_weighted_average_states(self):
parameter = 0.8
base_right_foot = Foot(Side.right, Vector3d(0, 0, 0), Vector3d(0, 0, 0))
base_left_foot = Foot(Side.left, Vector3d(0, 0, 0), Vector3d(0, 0, 0))
base_state = FeetState(base_right_foot, base_left_foot, Duration(seconds=0.1))
other_right_foot = Foot(Side.right, Vector3d(1, 1, 1), Vector3d(1, 1, 1))
other_left_foot = Foot(Side.left, Vector3d(1, 1, 1), Vector3d(1, 1, 1))
other_state = FeetState(
other_right_foot, other_left_foot, Duration(seconds=0.1)
)
resulting_state = FeetState.weighted_average_states(
base_state, other_state, parameter
)
self.assertEqual(Vector3d(0.8, 0.8, 0.8), resulting_state.left_foot.position)
self.assertEqual(Vector3d(0.8, 0.8, 0.8), resulting_state.left_foot.velocity)
self.assertEqual(Vector3d(0.8, 0.8, 0.8), resulting_state.right_foot.position)
self.assertEqual(Vector3d(0.8, 0.8, 0.8), resulting_state.right_foot.velocity)
def test_set_forward_backward_swing_inverse_kinematics(self):
[
l_ul,
l_ll,
l_hl,
l_ph,
r_ul,
r_ll,
r_hl,
r_ph,
base,
] = get_lengths_robot_for_inverse_kinematics(Side.both)
parameter = 0.99
base_setpoint_dict = {
"left_hip_aa": Setpoint(Duration(), 0, 0),
"left_hip_fe": Setpoint(Duration(), 0, 0),
"left_knee": Setpoint(Duration(), 0, 0),
"right_hip_aa": Setpoint(Duration(), 0, 0),
"right_hip_fe": Setpoint(Duration(), 0, 0),
"right_knee": Setpoint(Duration(), 0, 0),
}
other_setpoint_dict = {
"left_hip_aa": Setpoint(Duration(), 0, 0),
"left_hip_fe": Setpoint(Duration(), pi / 2, 0),
"left_knee": Setpoint(Duration(), 0, 0),
"right_hip_aa": Setpoint(Duration(), 0, 0),
"right_hip_fe": Setpoint(Duration(), pi / 2, 0),
"right_knee": Setpoint(Duration(), 0, 0),
}
base_state = FeetState.from_setpoint_dict(base_setpoint_dict)
other_state = FeetState.from_setpoint_dict(other_setpoint_dict)
resulting_state = FeetState.weighted_average_states(
base_state, other_state, parameter
)
base_expected_right_foot = Foot(
Side.right,
Vector3d(r_hl, base / 2.0 + r_ph, r_ul + r_ll),
Vector3d(0, 0, 0),
)
base_expected_left_foot = Foot(
Side.right,
Vector3d(l_hl, -base / 2.0 - l_ph, l_ul + l_ll),
Vector3d(0, 0, 0),
)
base_expected_state = FeetState(
base_expected_right_foot, base_expected_left_foot, Duration()
)
other_expected_right_foot = Foot(
Side.right,
Vector3d(r_ul + r_ll + r_hl, base / 2.0 + r_ph, 0),
Vector3d(0, 0, 0),
)
other_expected_left_foot = Foot(
Side.right,
Vector3d(l_ul + l_ll + l_hl, -base / 2.0 - l_ph, 0),
Vector3d(0, 0, 0),
)
other_expected_state = FeetState(
other_expected_right_foot, other_expected_left_foot, Duration()
)
expected_state = FeetState.weighted_average_states(
base_expected_state, other_expected_state, parameter
)
self.assertTrue(
Vector3d.is_close_enough(
expected_state.left_foot.position, resulting_state.left_foot.position
)
)
self.assertTrue(
Vector3d.is_close_enough(
expected_state.right_foot.position, resulting_state.right_foot.position
)
)
def get_foot_position_interpolated_joint_trajectories(
base_subgait: Subgait, other_subgait: Subgait,
parameter: float) -> List[JointTrajectory]:
"""Create a list of joint trajectories by interpolating foot locations.
The foot location corresponding to the resulting trajectories is equal
to the weighted average (with the parameter) of the foot locations
corresponding to the base and other subgait.
:param base_subgait: base subgait, return value if parameter is equal to zero
:param other_subgait: other subgait, return value if parameter is equal to one
:param parameter: Parameter for interpolation, between 0 and 1
:return: A list of interpolated joint trajectories
"""
if JOINT_NAMES_IK is None:
return base_subgait.joints
Subgait.check_foot_position_interpolation_is_safe(
base_subgait, other_subgait)
# The inverse kinematics needs access to the 'ith' setpoints of all joints
(
base_setpoints_to_interpolate,
other_setpoints_to_interpolate,
) = Subgait.prepare_subgaits_for_inverse_kinematics(
base_subgait, other_subgait)
number_of_setpoints = len(base_setpoints_to_interpolate)
new_setpoints: dict = {joint.name: [] for joint in base_subgait.joints}
# fill all joints in new_setpoints except the ankle joints using
# the inverse kinematics
for setpoint_index in range(number_of_setpoints):
if (base_setpoints_to_interpolate[setpoint_index] ==
other_setpoints_to_interpolate[setpoint_index]):
setpoints_to_add = base_setpoints_to_interpolate[
setpoint_index]
else:
base_feet_state = FeetState.from_setpoint_dict(
base_setpoints_to_interpolate[setpoint_index])
other_feet_state = FeetState.from_setpoint_dict(
other_setpoints_to_interpolate[setpoint_index])
new_feet_state = FeetState.weighted_average_states(
base_feet_state, other_feet_state, parameter)
setpoints_to_add = FeetState.feet_state_to_setpoints(
new_feet_state)
for joint_name in JOINT_NAMES_IK:
new_setpoints[joint_name].append(setpoints_to_add[joint_name])
# fill the ankle joint using the angle based linear interpolation
for ankle_joint in ["left_ankle", "right_ankle"]:
base_setpoint = base_setpoints_to_interpolate[setpoint_index][
ankle_joint]
other_setpoint = other_setpoints_to_interpolate[
setpoint_index][ankle_joint]
new_ankle_setpoint_to_add = Setpoint.interpolate_setpoints(
base_setpoint, other_setpoint, parameter)
new_setpoints[ankle_joint].append(new_ankle_setpoint_to_add)
duration = base_subgait.duration.weighted_average(
other_subgait.duration, parameter)
interpolated_joint_trajectories = [None] * len(base_subgait.joints)
for index, joint in enumerate(base_subgait.joints):
interpolated_joint_trajectory_to_add = JointTrajectory(
joint.name, joint.limits, new_setpoints[joint.name], duration)
interpolated_joint_trajectories[
index] = interpolated_joint_trajectory_to_add
return interpolated_joint_trajectories