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 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 transform_inverse_kinematics_setpoints_inputs(self) -> None:
"""Transform the input coordinates (relative to a default foot location) to coordinates relative to the exo."""
foot_side = self.view.inverse_kinematics_pop_up.foot_side
[
upper_leg_length,
lower_leg_length,
haa_to_leg_length,
haa_arm,
base,
] = get_lengths_robot_for_inverse_kinematics(foot_side)
self.transform_inverse_kinematics_setpoints_x_coordinate(
haa_to_leg_length)
self.transform_inverse_kinematics_setpoints_y_coordinate(
haa_arm, base, foot_side)
self.transform_inverse_kinematics_setpoints_z_coordinate(
upper_leg_length, lower_leg_length)
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 calculate_joint_angles_from_foot_position(foot_position, foot_side,
time):
"""Calculates the angles of the joints corresponding to a certain position of the right or left foot.
More information on the calculations of the haa, hfe and kfe angles, aswell as on the velocity calculations can
be found at https://confluence.projectmarch.nl:8443/display/62tech/%28Inverse%29+kinematics. This function
assumes that the desired z position of the foot is positive.
:param foot_position: A Vecor3d object which specifies the desired position of the foot.
:param foot_side: A string which specifies to which side the foot_position belongs and thus which joint angles
should be computed.
:param time: The time of the foot_position and the resulting setpoints.
:return:
A dictionary of Setpoints for each joint on the requested side with the correct angle at the provided time.
"""
# Get relevant lengths from robot model, ul = upper leg etc. see get_lengths_robot_for_inverse_kinematics()
# and unpack desired position
[ul, ll, hl, ph,
base] = get_lengths_robot_for_inverse_kinematics(foot_side)
x_position = foot_position.x
y_position = foot_position.y
z_position = foot_position.z
# Change y positive direction to the desired foot, change origin to pivot of haa joint, for ease of calculation.
if foot_side == Side.left:
y_position = -(y_position + base / 2.0)
elif foot_side == Side.right:
y_position = y_position - base / 2.0
else:
raise SideSpecificationError(foot_side)
# Check if the HAA can be calculated (if the desired position is not too close to the origin)
if ph / sqrt(z_position * z_position + y_position * y_position) > 1:
raise SubgaitInterpolationError(
"The desired {foot} foot position, ({x}, {y}, {z}), is out of reach"
.format(foot=foot_side,
x=x_position,
y=y_position,
z=z_position))
# Then calculate the haa angle. This calculation assumes that pos_z > 0
haa = Foot.calculate_haa_angle(z_position, y_position, ph)
# Transform the desired x and z position to arrive at an easier system to calculate
# the hfe and kfe angles
transformed_x = round(x_position - hl,
MID_CALCULATION_PRECISION_DIGITS)
transformed_z = round(
sqrt(-ph * ph + y_position * y_position + z_position * z_position),
MID_CALCULATION_PRECISION_DIGITS,
)
if transformed_x * transformed_x + transformed_z * transformed_z > (
ll + ul) * (ll + ul):
raise SubgaitInterpolationError(
"The desired {foot} foot position, ({x}, {y}, {z}), is out of reach"
.format(foot=foot_side,
x=x_position,
y=y_position,
z=z_position))
hfe, kfe = Foot.calculate_hfe_kfe_angles(transformed_x, transformed_z,
ul, ll)
# angle positions
return {
foot_side.value + "_hip_aa": Setpoint(time, haa),
foot_side.value + "_hip_fe": Setpoint(time, hfe),
foot_side.value + "_knee": Setpoint(time, kfe),
}