def __init__(self, is_left, hip_transversal_radians, target_position, robot_model, painter):
self.robot_model = robot_model
self.painter = painter
self.is_left = is_left
if self.DEBUG: print('is_left', is_left)
if self.is_left:
self.hip_top_position = Point(left_hip_top_position)
self.hip_bottom_position = Point(left_hip_bottom_position)
else:
self.hip_top_position = Point(right_hip_top_position)
self.hip_bottom_position = Point(right_hip_bottom_position)
if self.DEBUG: print('hip top & bottom\n\t', self.hip_top_position, '\n\t', self.hip_bottom_position)
self.hip_transversal_radians = hip_transversal_radians
self.target_position = Point(target_position)
self.target_at_ankle = Point(translate_3d_point(target_position, dz=foot_height))
if self.DEBUG: print('target & at ankle\n\t', self.target_position, '\n\t', self.target_at_ankle)
# counteract hip transversal
self.target_at_ankle__with_hip_transversal_counteracted = self._get_target_with_hip_transversal_counteracted()
if self.DEBUG: print('target_at_ankle__with_hip_transversal_counteracted\n\t', self.target_at_ankle__with_hip_transversal_counteracted)
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))
from robot_spec.length_data import *
from util.transformations import translate_3d_point
head_initial = (0, 0, torso_height + neck_length + head_height)
head_forward_bowed_90 = (head_height, 0, torso_height + neck_length)
head_backward_bowed_90 = (head_height, 0, torso_height + neck_length)
head_left_bowed_90 = (0, head_height, torso_height + neck_length)
head_right_bowed_90 = (0, -head_height, torso_height + neck_length)
left_shoulder_position = (0, half_torso_width, torso_height)
left_outer_shoulder_position = translate_3d_point(left_shoulder_position, dy=shoulder_length)
right_shoulder_position = (0, -half_torso_width, torso_height)
right_outer_shoulder_position = translate_3d_point(right_shoulder_position, dy=-shoulder_length)
left_hip_top_position = (0, half_torso_width, 0)
left_hip_bottom_position = (0, half_torso_width, -hip_length)
right_hip_top_position = (0, -half_torso_width, 0)
right_hip_bottom_position = (0, -half_torso_width, -hip_length)
initial_left_foot_position = (0, half_torso_width, initial_foot_z)
initial_right_foot_position = (0, -half_torso_width, initial_foot_z)
def get_initial_foot_position(is_left):
if is_left:
return initial_left_foot_position
else:
return initial_right_foot_position
from robot_spec.length_data import *
from util.transformations import translate_3d_point
head_initial = (0, 0, torso_height + neck_length + head_height)
head_forward_bowed_90 = (head_height, 0, torso_height + neck_length)
head_backward_bowed_90 = (head_height, 0, torso_height + neck_length)
head_left_bowed_90 = (0, head_height, torso_height + neck_length)
head_right_bowed_90 = (0, -head_height, torso_height + neck_length)
left_shoulder_position = (0, half_torso_width, torso_height)
left_outer_shoulder_position = translate_3d_point(left_shoulder_position,
dy=shoulder_length)
right_shoulder_position = (0, -half_torso_width, torso_height)
right_outer_shoulder_position = translate_3d_point(right_shoulder_position,
dy=-shoulder_length)
left_hip_top_position = (0, half_torso_width, 0)
left_hip_bottom_position = (0, half_torso_width, -hip_length)
right_hip_top_position = (0, -half_torso_width, 0)
right_hip_bottom_position = (0, -half_torso_width, -hip_length)
initial_left_foot_position = (0, half_torso_width, initial_foot_z)
initial_right_foot_position = (0, -half_torso_width, initial_foot_z)
def get_initial_foot_position(is_left):
if is_left:
return initial_left_foot_position
else:
