def move_head(target_position, painter):
target_position = Point(target_position)
painter.draw_point(target_position)
head_bottom_position = Point((0, 0, torso_height + neck_length))
head_top_position = Point((0, 0, torso_height + neck_length + head_height))
distance_between_head_bottom_and_target = head_bottom_position.distance_to(target_position)
assert distance_between_head_bottom_and_target == head_height
distance_between_head_top_and_target = head_top_position.distance_to(target_position)
neck_lateral_radians = get_angle_with_3_sides_known(distance_between_head_top_and_target, head_height, head_height)
def get_neck_transversal_radians(target_x, target_y):
if target_x != 0:
radiance = atan(target_y / target_x)
else: # perpendicular
if target_y > 0:
radiance = half_pi
elif target_y < 0:
radiance = -half_pi
else:
radiance = 0
return radiance
neck_transversal_radians = get_neck_transversal_radians(target_x=target_position[0], target_y=target_position[1])
return neck_transversal_radians, neck_lateral_radians
def _get_hip_lateral(self, hip_frontal_radians, knee_lateral_radians):
if self.is_left:
current_ankle = Point(self.robot_model.draw_left_lower_limp(left_hip_frontal_radians=hip_frontal_radians, left_knee_lateral_radians=knee_lateral_radians, draw=False)[-2].vertex)
else:
current_ankle = Point(self.robot_model.draw_right_lower_limp(right_hip_frontal_radians=hip_frontal_radians, right_knee_lateral_radians=knee_lateral_radians, draw=False)[-2].vertex)
opposite_side_length = current_ankle.distance_to(self.target_at_ankle__with_hip_transversal_counteracted)
if is_float_equal(opposite_side_length, 0):
return 0
target_at_ankle_with_hip_transversal_counteracted__radius = self.hip_bottom_position.distance_to(self.target_at_ankle__with_hip_transversal_counteracted)
current_ankle_radius = self.hip_bottom_position.distance_to(current_ankle)
assert is_float_equal(target_at_ankle_with_hip_transversal_counteracted__radius, current_ankle_radius), '{} {}'.format(target_at_ankle_with_hip_transversal_counteracted__radius, current_ankle_radius)
assert opposite_side_length < current_ankle_radius + target_at_ankle_with_hip_transversal_counteracted__radius
hip_lateral_radians = get_angle_with_3_sides_known(opposite_side_length, current_ankle_radius, target_at_ankle_with_hip_transversal_counteracted__radius)
if self.is_left:
return hip_lateral_radians
else:
return -hip_lateral_radians
class LineSegment:
def __init__(self, initial, end):
self.initial = Point(initial)
self.end = Point(end)
self._mid_point = None
self._length = None
def get_point_at(self, fraction):
x = self.initial.x + (self.end.x - self.initial.x) * fraction
y = self.initial.y + (self.end.y - self.initial.y) * fraction
z = self.initial.z + (self.end.z - self.initial.z) * fraction
return Point((x, y, z))
def has_point(self, p):
if self.initial.x <= p.x <= self.end.x\
and self.initial.y <= p.y <= self.end.y\
and self.initial.z <= p.x <= self.end.x:
return True
else:
return False
@property
def mid_point(self):
if self._mid_point is None:
self._mid_point = self.get_point_at(0.5)
return self._mid_point
@property
def length(self):
if self._length is None:
self._length = self.initial.distance_to(self.end)
return self._length
class LineSegment:
def __init__(self, initial, end):
self.initial = Point(initial)
self.end = Point(end)
self._mid_point = None
self._length = None
def get_point_at(self, fraction):
x = self.initial.x + (self.end.x - self.initial.x) * fraction
y = self.initial.y + (self.end.y - self.initial.y) * fraction
z = self.initial.z + (self.end.z - self.initial.z) * fraction
return Point((x, y, z))
def has_point(self, p):
if self.initial.x <= p.x <= self.end.x\
and self.initial.y <= p.y <= self.end.y\
and self.initial.z <= p.x <= self.end.x:
return True
else:
return False
@property
def mid_point(self):
if self._mid_point is None:
self._mid_point = self.get_point_at(0.5)
return self._mid_point
@property
def length(self):
if self._length is None:
self._length = self.initial.distance_to(self.end)
return self._length
class InverseKinematicsLeg():
DEBUG = False
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 get_angles(self):
if self.DEBUG: self.painter.draw_point(self.target_position, color='y.')
if self.DEBUG: self.painter.draw_point(self.target_at_ankle, color='y.')
knee_lateral_radians, knee_interior_radians, knee_exterior_radians = self._get_knee_lateral()
if self.DEBUG: print('knee_lateral_radians', degrees(knee_lateral_radians))
hip_frontal_radians = self._get_hip_frontal()
if self.DEBUG: print('hip_frontal_radians', degrees(hip_frontal_radians))
hip_lateral_radians = - self._get_hip_lateral(hip_frontal_radians, knee_lateral_radians)
if self.DEBUG: print('hip_lateral_radians', degrees(hip_lateral_radians))
ankle_lateral_radians = -(abs(hip_lateral_radians) - knee_exterior_radians)
if not self.is_left:
ankle_lateral_radians *= -1
if self.DEBUG: print('ankle_lateral_radians', ankle_lateral_radians)
ankle_frontal_radians = hip_frontal_radians
if self.DEBUG: print('ankle_frontal_radians', ankle_frontal_radians)
if self.DEBUG: print('fd (hip_transversal, hip_frontal, hip_lateral, knee_lateral)')
if self.DEBUG: print(' ', to_degrees((self.hip_transversal_radians, hip_frontal_radians, hip_lateral_radians, knee_lateral_radians)))
return hip_frontal_radians, hip_lateral_radians, knee_lateral_radians, ankle_lateral_radians, ankle_frontal_radians
def _get_target_with_hip_transversal_counteracted(self):
move_back_to_origin = get_translate_matrix(-self.hip_top_position.x, -self.hip_top_position.y, -self.hip_top_position.z)
rotate_along_z_axis = get_rotate_matrix_along_axis('z', self.hip_transversal_radians)
move_back_to_point = get_translate_matrix(self.hip_top_position.x, self.hip_top_position.y, self.hip_top_position.z)
target_at_ankle__with_hip_transversal_counteracted = apply_matrix_to_vertex(self.target_at_ankle, move_back_to_point.dot(rotate_along_z_axis.dot(move_back_to_origin)))
return Point(target_at_ankle__with_hip_transversal_counteracted)
def _get_knee_lateral(self):
d1 = self.hip_bottom_position.distance_to(self.target_at_ankle__with_hip_transversal_counteracted)
if self.DEBUG: print('d1', d1, self.hip_bottom_position, self.target_at_ankle__with_hip_transversal_counteracted)
assert is_float_equal(d1, leg_length) or d1 < leg_length, '{} {} {}'.format(d1, leg_length, d1-leg_length)
if is_float_equal(d1, leg_length):
knee_interior_radians = pi
else:
knee_interior_radians = get_angle_with_3_sides_known(d1, upper_leg_length, lower_leg_length)
knee_exterior_radians = pi - knee_interior_radians
if self.DEBUG: print('knee interior & exterior angles', degrees(knee_interior_radians), degrees(knee_exterior_radians))
if self.is_left:
knee_lateral_radians = knee_exterior_radians
else:
knee_lateral_radians = -knee_exterior_radians
return knee_lateral_radians, knee_interior_radians, knee_exterior_radians
def _get_hip_frontal(self):
target_at_ankle_with_hip_transversal_counteracted__y_offset_to_hip_bottom = abs(self.target_at_ankle__with_hip_transversal_counteracted.y - self.hip_bottom_position.y)
target_at_ankle_with_hip_transversal_counteracted__z_offset_to_hip_bottom = abs(self.target_at_ankle__with_hip_transversal_counteracted.z - self.hip_bottom_position.z)
hip_frontal_radians = atan(
target_at_ankle_with_hip_transversal_counteracted__y_offset_to_hip_bottom
/
target_at_ankle_with_hip_transversal_counteracted__z_offset_to_hip_bottom)
if self.is_left:
return -hip_frontal_radians
else:
return hip_frontal_radians
def _get_hip_lateral(self, hip_frontal_radians, knee_lateral_radians):
if self.is_left:
current_ankle = Point(self.robot_model.draw_left_lower_limp(left_hip_frontal_radians=hip_frontal_radians, left_knee_lateral_radians=knee_lateral_radians, draw=False)[-2].vertex)
else:
current_ankle = Point(self.robot_model.draw_right_lower_limp(right_hip_frontal_radians=hip_frontal_radians, right_knee_lateral_radians=knee_lateral_radians, draw=False)[-2].vertex)
opposite_side_length = current_ankle.distance_to(self.target_at_ankle__with_hip_transversal_counteracted)
if is_float_equal(opposite_side_length, 0):
return 0
target_at_ankle_with_hip_transversal_counteracted__radius = self.hip_bottom_position.distance_to(self.target_at_ankle__with_hip_transversal_counteracted)
current_ankle_radius = self.hip_bottom_position.distance_to(current_ankle)
assert is_float_equal(target_at_ankle_with_hip_transversal_counteracted__radius, current_ankle_radius), '{} {}'.format(target_at_ankle_with_hip_transversal_counteracted__radius, current_ankle_radius)
assert opposite_side_length < current_ankle_radius + target_at_ankle_with_hip_transversal_counteracted__radius
hip_lateral_radians = get_angle_with_3_sides_known(opposite_side_length, current_ankle_radius, target_at_ankle_with_hip_transversal_counteracted__radius)
if self.is_left:
return hip_lateral_radians
else:
return -hip_lateral_radians
