Python translate_3d_point Examples

Example #1

File: lower_limp.py Project: Aiden-Wei/humanoid_walking_gait

    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)

Example #2

    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))

Example #3

File: position_data.py Project: umhan35/humanoid_walking_gait

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

Example #4

File: position_data.py Project: Aiden-Wei/humanoid_walking_gait

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: