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 distance_to(self, point_b):
return distance_between_2_vertices_in_3d(self, point_b)
def distance_to(self, point_b):
return distance_between_2_vertices_in_3d(self, point_b)
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
