def test_find_known_position_sit(self):
setpoint_dict = {
"left_hip_aa": Setpoint(0, 0, 0),
"left_hip_fe": Setpoint(0, pi / 2, 0),
"left_knee": Setpoint(0, pi / 2, 0),
"right_hip_aa": Setpoint(0, 0, 0),
"right_hip_fe": Setpoint(0, pi / 2, 0),
"right_knee": Setpoint(0, pi / 2, 0),
}
[
l_ul,
l_ll,
_,
_,
r_ul,
r_ll,
_,
_,
_,
] = get_lengths_robot_for_inverse_kinematics(Side.both)
resulting_state = FeetState.from_setpoints(setpoint_dict)
expected_right_foot = Foot(Side.right,
Vector3d(r_ul + 0.1395, 0.1705, r_ll),
Vector3d(0, 0, 0))
expected_left_foot = Foot(Side.right,
Vector3d(l_ul + 0.1395, -0.1705, l_ll),
Vector3d(0, 0, 0))
expected_state = FeetState(expected_right_foot, expected_left_foot, 0)
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 save(self) -> None:
"""Check and save value while closing, close if successful."""
if self.footSideComboBox.currentText() == "Right":
self.foot_side = Side.right
else:
self.foot_side = Side.left
self.z_axis = self.zAxisComboBox.currentText()
self.position_input = (
Vector3d(
self.xCoordinateSpinBox.value(),
self.yCoordinateSpinBox.value(),
self.zCoordinateSpinBox.value(),
) / 100 # Convert the input in cm to meters for calculation
)
self.velocity_input = (
Vector3d(
self.xVelocitySpinBox.value(),
self.yVelocitySpinBox.value(),
self.zVelocitySpinBox.value(),
) /
100 # Convert the input in cm / s to meters / s for calculation
)
self.time = self.timeSpinBox.value()
self.accept()
def show_pop_up(self) -> any:
"""Show pop up."""
self.foot_side = ""
self.z_axis = ""
self.position_input = Vector3d(0, 0, 0)
self.velocity_input = Vector3d(0, 0, 0)
self.time = 0
self.cancelled = False
return super(InverseKinematicsPopUpWindow, self).exec_()
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, 0.1)
new_setpoints = FeetState.feet_state_to_setpoints(desired_state)
resulting_position = FeetState.from_setpoints(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 calculate_foot_position(haa, hfe, kfe, side):
"""Calculates the foot position given the relevant angles, lengths and a specification of the foot.
:param side: The side of the exoskeleton to which the angles belong
:param haa: The angle of the hip_aa joint on the specified side
:param hfe: The angle of the hip_fe joint on the specified side
:param kfe: The angle of the knee joint on the specified side
:return: The location of the foot (ankle) of the specified side which corresponds to the given angles
"""
# x is positive in the walking direction, z is in the downward direction, y is directed to the right side
# the origin in the middle of the hip structure. The calculations are supported by
# https://confluence.projectmarch.nl:8443/display/62tech/%28Inverse%29+kinematics
ul, ll, hl, ph, base = get_lengths_robot_for_inverse_kinematics(side)
haa_to_foot_length = ul * cos(hfe) + ll * cos(hfe - kfe)
z_position = -sin(haa) * ph + cos(haa) * haa_to_foot_length
x_position = hl + sin(hfe) * ul + sin(hfe - kfe) * ll
if side == side.left:
y_position = -cos(haa) * ph - sin(
haa) * haa_to_foot_length - base / 2.0
elif side == side.right:
y_position = cos(haa) * ph + sin(
haa) * haa_to_foot_length + base / 2.0
else:
raise SideSpecificationError(side)
return Foot(side, Vector3d(x_position, y_position, z_position))
def test_set_forward_backward_swing_inverse_kinematics(self):
[
l_ul,
l_ll,
_,
_,
r_ul,
r_ll,
_,
_,
_,
] = get_lengths_robot_for_inverse_kinematics(Side.both)
parameter = 0.99
base_setpoint_dict = {
"left_hip_aa": Setpoint(0, 0, 0),
"left_hip_fe": Setpoint(0, 0, 0),
"left_knee": Setpoint(0, 0, 0),
"right_hip_aa": Setpoint(0, 0, 0),
"right_hip_fe": Setpoint(0, 0, 0),
"right_knee": Setpoint(0, 0, 0),
}
other_setpoint_dict = {
"left_hip_aa": Setpoint(0, 0, 0),
"left_hip_fe": Setpoint(0, pi / 2, 0),
"left_knee": Setpoint(0, 0, 0),
"right_hip_aa": Setpoint(0, 0, 0),
"right_hip_fe": Setpoint(0, pi / 2, 0),
"right_knee": Setpoint(0, 0, 0),
}
base_state = FeetState.from_setpoints(base_setpoint_dict)
other_state = FeetState.from_setpoints(other_setpoint_dict)
resulting_state = FeetState.weighted_average_states(
base_state, other_state, parameter)
base_expected_right_foot = Foot(Side.right,
Vector3d(0.1395, 0.1705, r_ul + r_ll),
Vector3d(0, 0, 0))
base_expected_left_foot = Foot(Side.right,
Vector3d(0.1395, -0.1705, l_ul + l_ll),
Vector3d(0, 0, 0))
base_expected_state = FeetState(base_expected_right_foot,
base_expected_left_foot, 0)
other_expected_right_foot = Foot(
Side.right, Vector3d(r_ul + r_ll + 0.1395, 0.1705, 0),
Vector3d(0, 0, 0))
other_expected_left_foot = Foot(
Side.right, Vector3d(l_ul + l_ll + 0.1395, -0.1705, 0),
Vector3d(0, 0, 0))
other_expected_state = FeetState(other_expected_right_foot,
other_expected_left_foot, 0)
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 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, 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, 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)