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_elbow_lateral_radians(self): # print('\n###### elbow lateral ######\n') d1 = distance_btw_outer_shoulder_position_and_target = distance_between_2_vertices_in_3d( self.outer_shoulder_position, self.target_position) # print('d1', d1, 'arm_length', arm_length, 'lower_upper_arm_difference', abs(lower_arm_length - upper_arm_length)) assert is_float_equal(d1, arm_length) or d1 < arm_length if is_float_equal(d1, lower_and_upper_arm_difference): _elbow_interior_radiance = 0 elif is_float_equal(d1, arm_length): _elbow_interior_radiance = pi else: _elbow_interior_radiance = get_angle_with_3_sides_known(d1, upper_arm_length, lower_arm_length) # print('_elbow_interior_degree', degrees(_elbow_interior_radiance)) _elbow_exterior_radiance = pi - _elbow_interior_radiance # print('_elbow_exterior_degree', degrees(_elbow_exterior_radiance)) assert 0 <= _elbow_interior_radiance <= pi if self.is_left: elbow_lateral_radians = -_elbow_exterior_radiance else: elbow_lateral_radians = _elbow_exterior_radiance # print('elbow_lateral_radians', degrees(elbow_lateral_radians)) return elbow_lateral_radians, _elbow_interior_radiance, _elbow_exterior_radiance
def _get_elbow_lateral_radians(self): # print('\n###### elbow lateral ######\n') d1 = distance_btw_outer_shoulder_position_and_target = distance_between_2_vertices_in_3d( self.outer_shoulder_position, self.target_position) # print('d1', d1, 'arm_length', arm_length, 'lower_upper_arm_difference', abs(lower_arm_length - upper_arm_length)) assert is_float_equal(d1, arm_length) or d1 < arm_length if is_float_equal(d1, lower_and_upper_arm_difference): _elbow_interior_radiance = 0 elif is_float_equal(d1, arm_length): _elbow_interior_radiance = pi else: _elbow_interior_radiance = get_angle_with_3_sides_known( d1, upper_arm_length, lower_arm_length) # print('_elbow_interior_degree', degrees(_elbow_interior_radiance)) _elbow_exterior_radiance = pi - _elbow_interior_radiance # print('_elbow_exterior_degree', degrees(_elbow_exterior_radiance)) assert 0 <= _elbow_interior_radiance <= pi if self.is_left: elbow_lateral_radians = -_elbow_exterior_radiance else: elbow_lateral_radians = _elbow_exterior_radiance # print('elbow_lateral_radians', degrees(elbow_lateral_radians)) return elbow_lateral_radians, _elbow_interior_radiance, _elbow_exterior_radiance
def __get_shoulder_lateral(self, shoulder_frontal_radians, elbow_lateral_radians): is_target_above_shoulder = self.target_position.z > outer_shoulder_z_offset # print('shoulder_frontal_radians', degrees(shoulder_frontal_radians)) def _project_on_outer_shoulder_natural_plane(point): return (point[0], self.outer_shoulder_position[1], point[2]) current_wrist_with_shoulder_lateral_be_0 = self.draw_arm(0, shoulder_frontal_radians, elbow_lateral_radians, draw=False)[-1] the_current_wrist_on_outer_shoulder_natural_plane = _project_on_outer_shoulder_natural_plane(current_wrist_with_shoulder_lateral_be_0) # print('wrist current:', current_wrist_with_shoulder_lateral_be_0) # print('wrist on shoulder natural plane:', the_current_wrist_on_outer_shoulder_natural_plane) target_on_outer_shoulder_natural_plane = _project_on_outer_shoulder_natural_plane(self.target_position) # print('wrist & target on shoulder natural plane:', the_current_wrist_on_outer_shoulder_natural_plane, target_on_outer_shoulder_natural_plane) # painter.draw_link(left_outer_shoulder_position, the_current_wrist_on_outer_shoulder_natural_plane, color='g-') # painter.draw_link(left_outer_shoulder_position, target_on_outer_shoulder_natural_plane, color='g-') distance_btw_outer_shoulder_position_and_target_on_outer_shoulder_natural_plane = distance_between_2_vertices_in_3d(self.outer_shoulder_position, target_on_outer_shoulder_natural_plane) distance_btw_outer_shoulder_position_and_current_wrist_on_outer_shoulder_natural_plane = distance_between_2_vertices_in_3d(self.outer_shoulder_position, the_current_wrist_on_outer_shoulder_natural_plane) assert is_float_equal(distance_btw_outer_shoulder_position_and_target_on_outer_shoulder_natural_plane, distance_btw_outer_shoulder_position_and_current_wrist_on_outer_shoulder_natural_plane),\ '{}, {}'.format(distance_btw_outer_shoulder_position_and_target_on_outer_shoulder_natural_plane, distance_btw_outer_shoulder_position_and_current_wrist_on_outer_shoulder_natural_plane) outer_shoulder_working_circle_radius__on_outer_shoulder_natural_plane = distance_btw_outer_shoulder_position_and_target_on_outer_shoulder_natural_plane if is_float_equal(0, outer_shoulder_working_circle_radius__on_outer_shoulder_natural_plane): _shoulder_lateral_radians = 0 else: # print(outer_shoulder_working_circle_radius__on_outer_shoulder_natural_plane, distance_between_2_vertices_in_3d(left_outer_shoulder_position, the_current_wrist_on_outer_shoulder_natural_plane)) assert is_float_equal(distance_between_2_vertices_in_3d(self.outer_shoulder_position, the_current_wrist_on_outer_shoulder_natural_plane), outer_shoulder_working_circle_radius__on_outer_shoulder_natural_plane) distance_between_above_2_projected_on_outer_shoulder_natural_plane = distance_between_2_vertices_in_3d(the_current_wrist_on_outer_shoulder_natural_plane, target_on_outer_shoulder_natural_plane) # if is_float_equal(distance_between_above_2_projected_on_outer_shoulder_natural_plane, 2 * outer_shoulder_working_circle_radius__on_outer_shoulder_natural_plane): # else: _shoulder_lateral_radians = get_angle_with_3_sides_known(distance_between_above_2_projected_on_outer_shoulder_natural_plane, outer_shoulder_working_circle_radius__on_outer_shoulder_natural_plane, outer_shoulder_working_circle_radius__on_outer_shoulder_natural_plane) if not is_target_above_shoulder: _shoulder_lateral_radians *= -1 if not self.is_left: _shoulder_lateral_radians *= -1 # print('_shoulder_lateral_radians', degrees(_shoulder_lateral_radians)) return _shoulder_lateral_radians
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_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
def __get_shoulder_lateral(self, shoulder_frontal_radians, elbow_lateral_radians): is_target_above_shoulder = self.target_position.z > outer_shoulder_z_offset # print('shoulder_frontal_radians', degrees(shoulder_frontal_radians)) def _project_on_outer_shoulder_natural_plane(point): return (point[0], self.outer_shoulder_position[1], point[2]) current_wrist_with_shoulder_lateral_be_0 = self.draw_arm( 0, shoulder_frontal_radians, elbow_lateral_radians, draw=False)[-1] the_current_wrist_on_outer_shoulder_natural_plane = _project_on_outer_shoulder_natural_plane( current_wrist_with_shoulder_lateral_be_0) # print('wrist current:', current_wrist_with_shoulder_lateral_be_0) # print('wrist on shoulder natural plane:', the_current_wrist_on_outer_shoulder_natural_plane) target_on_outer_shoulder_natural_plane = _project_on_outer_shoulder_natural_plane( self.target_position) # print('wrist & target on shoulder natural plane:', the_current_wrist_on_outer_shoulder_natural_plane, target_on_outer_shoulder_natural_plane) # painter.draw_link(left_outer_shoulder_position, the_current_wrist_on_outer_shoulder_natural_plane, color='g-') # painter.draw_link(left_outer_shoulder_position, target_on_outer_shoulder_natural_plane, color='g-') distance_btw_outer_shoulder_position_and_target_on_outer_shoulder_natural_plane = distance_between_2_vertices_in_3d( self.outer_shoulder_position, target_on_outer_shoulder_natural_plane) distance_btw_outer_shoulder_position_and_current_wrist_on_outer_shoulder_natural_plane = distance_between_2_vertices_in_3d( self.outer_shoulder_position, the_current_wrist_on_outer_shoulder_natural_plane) assert is_float_equal(distance_btw_outer_shoulder_position_and_target_on_outer_shoulder_natural_plane, distance_btw_outer_shoulder_position_and_current_wrist_on_outer_shoulder_natural_plane),\ '{}, {}'.format(distance_btw_outer_shoulder_position_and_target_on_outer_shoulder_natural_plane, distance_btw_outer_shoulder_position_and_current_wrist_on_outer_shoulder_natural_plane) outer_shoulder_working_circle_radius__on_outer_shoulder_natural_plane = distance_btw_outer_shoulder_position_and_target_on_outer_shoulder_natural_plane if is_float_equal( 0, outer_shoulder_working_circle_radius__on_outer_shoulder_natural_plane ): _shoulder_lateral_radians = 0 else: # print(outer_shoulder_working_circle_radius__on_outer_shoulder_natural_plane, distance_between_2_vertices_in_3d(left_outer_shoulder_position, the_current_wrist_on_outer_shoulder_natural_plane)) assert is_float_equal( distance_between_2_vertices_in_3d( self.outer_shoulder_position, the_current_wrist_on_outer_shoulder_natural_plane), outer_shoulder_working_circle_radius__on_outer_shoulder_natural_plane ) distance_between_above_2_projected_on_outer_shoulder_natural_plane = distance_between_2_vertices_in_3d( the_current_wrist_on_outer_shoulder_natural_plane, target_on_outer_shoulder_natural_plane) # if is_float_equal(distance_between_above_2_projected_on_outer_shoulder_natural_plane, 2 * outer_shoulder_working_circle_radius__on_outer_shoulder_natural_plane): # else: _shoulder_lateral_radians = get_angle_with_3_sides_known( distance_between_above_2_projected_on_outer_shoulder_natural_plane, outer_shoulder_working_circle_radius__on_outer_shoulder_natural_plane, outer_shoulder_working_circle_radius__on_outer_shoulder_natural_plane ) if not is_target_above_shoulder: _shoulder_lateral_radians *= -1 if not self.is_left: _shoulder_lateral_radians *= -1 # print('_shoulder_lateral_radians', degrees(_shoulder_lateral_radians)) return _shoulder_lateral_radians