def _get_is_swing_leg_left_and_walk_trajectory_four_positions__with_current_working_set(self, index):
current_walking_set = self.walking_sets[index]
is_swing_leg_left = current_walking_set['is_swing_leg_left']
step_length = current_walking_set['step_length']
current_position_relative_to_initial_foot_position = current_walking_set['current_position_relative_to_initial_foot_position']
initial_foot_position = Point(get_initial_foot_position(is_left=is_swing_leg_left))
initial_position = initial_foot_position.translate(current_position_relative_to_initial_foot_position.x, current_position_relative_to_initial_foot_position.y, current_position_relative_to_initial_foot_position.z)
lift_up_front_position, lift_up_front_forward_position, put_down_position\
= self._lower_limp_kinematics_engine.get_walk_trajectory_three_positions(is_swing_leg_left=is_swing_leg_left, step_length=step_length, current_position_relative_to_initial_foot_position=current_position_relative_to_initial_foot_position)
return is_swing_leg_left, initial_position, lift_up_front_position, lift_up_front_forward_position, put_down_position
def get_left_leg_target_position():
# return get_leg_position((l_hip_transversal_degrees, 0, -0, -0), is_left=True)
initial_foot_position = Point(initial_left_foot_position)
up = initial_foot_position.translate(dz=2*foot_height)
_front_up = up.translate(dx=2*foot_height)
_back_up = up.translate(dx=-2*foot_height)
up_out = up.translate(dy=2*foot_height)
out_down = up_out.translate(dz=-foot_height)
front_out_up = up_out.translate(dx=2*foot_height)
_back_out_up = up_out.translate(dx=-2*foot_height)
front_out_down = out_down.translate(dx=2*foot_height)
_back_out_down = out_down.translate(dx=-2*foot_height)
for p in [initial_foot_position, up, _front_up, _back_up, up_out, out_down, front_out_up, _back_out_up, front_out_down, _back_out_down]:
painter.draw_point(p)
return _back_up
def get_right_leg_target_position():
# return get_leg_position((r_hip_transversal_degrees, RightHipFrontal().outward(20).degrees, 0, 0), is_left=False)
initial_foot_position = Point(initial_right_foot_position)
up = initial_foot_position.translate(dz=2*foot_height)
_front_up = up.translate(dx=2*foot_height)
_back_up = up.translate(dx=-2*foot_height)
up_out = up.translate(dy=-2*foot_height)
out_down = up_out.translate(dz=-foot_height)
front_out_up = up_out.translate(dx=2*foot_height)
_back_out_up = up_out.translate(dx=-2*foot_height)
front_out_down = out_down.translate(dx=2*foot_height)
_back_out_down = out_down.translate(dx=-2*foot_height)
for p in [initial_foot_position, up, _front_up, _back_up, up_out, out_down, front_out_up, _back_out_up, front_out_down, _back_out_down]:
painter.draw_point(p)
# right...
return _back_up
def get_left_leg_target_position():
# return get_leg_position((l_hip_transversal_degrees, 0, -0, -0), is_left=True)
initial_foot_position = Point(initial_left_foot_position)
up = initial_foot_position.translate(dz=2 * foot_height)
_front_up = up.translate(dx=2 * foot_height)
_back_up = up.translate(dx=-2 * foot_height)
up_out = up.translate(dy=2 * foot_height)
out_down = up_out.translate(dz=-foot_height)
front_out_up = up_out.translate(dx=2 * foot_height)
_back_out_up = up_out.translate(dx=-2 * foot_height)
front_out_down = out_down.translate(dx=2 * foot_height)
_back_out_down = out_down.translate(dx=-2 * foot_height)
for p in [
initial_foot_position, up, _front_up, _back_up, up_out,
out_down, front_out_up, _back_out_up, front_out_down,
_back_out_down
]:
painter.draw_point(p)
return _back_up
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))
def get_right_leg_target_position():
# return get_leg_position((r_hip_transversal_degrees, RightHipFrontal().outward(20).degrees, 0, 0), is_left=False)
initial_foot_position = Point(initial_right_foot_position)
up = initial_foot_position.translate(dz=2 * foot_height)
_front_up = up.translate(dx=2 * foot_height)
_back_up = up.translate(dx=-2 * foot_height)
up_out = up.translate(dy=-2 * foot_height)
out_down = up_out.translate(dz=-foot_height)
front_out_up = up_out.translate(dx=2 * foot_height)
_back_out_up = up_out.translate(dx=-2 * foot_height)
front_out_down = out_down.translate(dx=2 * foot_height)
_back_out_down = out_down.translate(dx=-2 * foot_height)
for p in [
initial_foot_position, up, _front_up, _back_up, up_out,
out_down, front_out_up, _back_out_up, front_out_down,
_back_out_down
]:
painter.draw_point(p)
# right...
return _back_up
