def assertArmAnglesEqual(self, arm_angles, is_left=True):
"""
Given shoulder and elbow angles A for arm, calculate the end points P,
and call inverse kinematics (IK) with P to see if IK returns A exactly.
:param arm_angles:
:return:
"""
# print('\n'*6 + 'aAE', arm_angles)
arm_angles = to_radians(arm_angles)
draw_arm = self.r.draw_left_arm if is_left else self.r.draw_right_arm
expected_shoulder_lateral, expected_shoulder_frontal, expected_elbow_lateral, expected_wrist = draw_arm(
arm_angles[0], arm_angles[1], arm_angles[2], draw=False
)
def is_target_behind_body():
target_x = expected_wrist[0]
return not is_float_equal(target_x, 0) and target_x < 0
if is_target_behind_body():
return
shoulder_lateral_radians, shoulder_frontal_radians, elbow_lateral_radians = InverseKinematicsArm(
expected_wrist, self.r, self.painter, is_left=is_left
).get_angles()
actual_shoulder_lateral, actual_shoulder_frontal, actual_elbow_lateral, actual_wrist = draw_arm(
shoulder_lateral_radians, shoulder_frontal_radians, elbow_lateral_radians, draw=False
)
for i in [0, 1, 2]: # x, y, z
self.assertAlmostEqual(expected_wrist[i], actual_wrist[i])
def assertArmAnglesEqual(self, arm_angles, is_left=True):
"""
Given shoulder and elbow angles A for arm, calculate the end points P,
and call inverse kinematics (IK) with P to see if IK returns A exactly.
:param arm_angles:
:return:
"""
# print('\n'*6 + 'aAE', arm_angles)
arm_angles = to_radians(arm_angles)
draw_arm = self.r.draw_left_arm if is_left else self.r.draw_right_arm
expected_shoulder_lateral, expected_shoulder_frontal, expected_elbow_lateral, expected_wrist\
= draw_arm(arm_angles[0], arm_angles[1], arm_angles[2], draw=False)
def is_target_behind_body():
target_x = expected_wrist[0]
return not is_float_equal(target_x, 0) and target_x < 0
if is_target_behind_body():
return
shoulder_lateral_radians, shoulder_frontal_radians, elbow_lateral_radians\
= InverseKinematicsArm(expected_wrist, self.r, self.painter, is_left=is_left).get_angles()
actual_shoulder_lateral, actual_shoulder_frontal, actual_elbow_lateral, actual_wrist\
= draw_arm(shoulder_lateral_radians, shoulder_frontal_radians, elbow_lateral_radians, draw=False)
for i in [0, 1, 2]: # x, y, z
self.assertAlmostEqual(expected_wrist[i], actual_wrist[i])
def assertLowerLimpAnglesEqual(self, leg_angles, is_left=True):
"""
Given shoulder and elbow angles A for arm, calculate the end points P,
and call inverse kinematics (IK) with P to see if IK returns A exactly.
:param arm_angles:
:return:
"""
print('\n'*6 + 'aAE', leg_angles)
lower_limp_angles = to_radians(leg_angles) + (0, 0)
draw_lower_limp = self.r.draw_left_lower_limp if is_left else self.r.draw_right_lower_limp
expected_hip_transversal, expected_hip_frontal, expected_hip_lateral, \
expected_knee_lateral, \
expected_ankle_lateral, expected_ankle_frontal, \
expected_foot_center \
= draw_lower_limp(*lower_limp_angles, color='r-')
expected_ankle = Point(expected_ankle_frontal.vertex)
target = translate_3d_point(expected_ankle, dz=-foot_height)
hip_transversal_radians = lower_limp_angles[0]
hip_frontal_radians, hip_lateral_radians, knee_lateral_radians, ankle_lateral_radians, ankle_frontal_radians \
= InverseKinematicsLeg(hip_transversal_radians, target, self.r, self.painter, is_left).get_angles()
actual_hip_transversal, actual_hip_frontal, actual_hip_lateral, \
actual_knee_lateral, \
actual_ankle_lateral, actual_ankle_frontal, \
actual_foot_center \
= draw_lower_limp(hip_transversal_radians, hip_frontal_radians, hip_lateral_radians, knee_lateral_radians, ankle_lateral_radians, ankle_frontal_radians, draw=False)
actual_ankle =Point(actual_ankle_frontal.vertex)
actual_foot_center = Point(actual_foot_center)
self.assertTrue(actual_ankle.float_equals(expected_ankle), [actual_ankle, expected_ankle])
self.assertTrue(actual_ankle.translate(dz=-foot_height).float_equals(actual_foot_center))