def _draw_foot(self, ankle_t, color='c-', draw=True, get_lines=False): ankle = apply_matrix_to_origin(ankle_t) foot_center_t = ankle_t.dot(get_translate_matrix(0, 0, -foot_height)) foot_center = apply_matrix_to_origin(foot_center_t) if draw or get_lines: foot_right_t = foot_center_t.dot( get_translate_matrix(-half_foot_width, 0, 0)) foot_right = apply_matrix_to_origin(foot_right_t) foot_left_t = foot_center_t.dot( get_translate_matrix(half_foot_width, 0, 0)) foot_left = apply_matrix_to_origin(foot_left_t) foot_front_t = foot_center_t.dot( get_translate_matrix(0, -half_foot_length, 0)) foot_front = apply_matrix_to_origin(foot_front_t) foot_back_t = foot_center_t.dot( get_translate_matrix(0, half_foot_length, 0)) foot_back = apply_matrix_to_origin(foot_back_t) ankle_foot_links = self._painter.draw_line(ankle, foot_center, color=color),\ self._painter.draw_line(foot_right, foot_left, color=color),\ self._painter.draw_line(foot_front, foot_back, color=color) if get_lines: return ankle_foot_links else: return foot_center
def _draw_foot(self, ankle_t, color='c-', draw=True, get_lines=False): ankle = apply_matrix_to_origin(ankle_t) foot_center_t = ankle_t.dot(get_translate_matrix(0, 0, -foot_height)) foot_center = apply_matrix_to_origin(foot_center_t) if draw or get_lines: foot_right_t = foot_center_t.dot(get_translate_matrix(-half_foot_width, 0, 0)) foot_right = apply_matrix_to_origin(foot_right_t) foot_left_t = foot_center_t.dot(get_translate_matrix(half_foot_width, 0, 0)) foot_left = apply_matrix_to_origin(foot_left_t) foot_front_t = foot_center_t.dot(get_translate_matrix(0, -half_foot_length, 0)) foot_front = apply_matrix_to_origin(foot_front_t) foot_back_t = foot_center_t.dot(get_translate_matrix(0, half_foot_length, 0)) foot_back = apply_matrix_to_origin(foot_back_t) ankle_foot_links = self._painter.draw_line(ankle, foot_center, color=color),\ self._painter.draw_line(foot_right, foot_left, color=color),\ self._painter.draw_line(foot_front, foot_back, color=color) if get_lines: return ankle_foot_links else: return foot_center
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 _draw_head(self, head_t): head_bottom = apply_matrix_to_origin(head_t) head_top_t = head_t.dot(get_translate_matrix(0, head_height, 0)) head_top = apply_matrix_to_origin(head_top_t) self._painter.draw_line(head_bottom, head_top) head_left_t = head_top_t.dot(get_translate_matrix(0, 0, half_head_width)) head_left = apply_matrix_to_origin(head_left_t) head_right_t = head_top_t.dot(get_translate_matrix(0, 0, -half_head_width)) head_right = apply_matrix_to_origin(head_right_t) self._painter.draw_line(head_left, head_right)
def _draw_head(self, head_t): head_bottom = apply_matrix_to_origin(head_t) head_top_t = head_t.dot(get_translate_matrix(0, head_height, 0)) head_top = apply_matrix_to_origin(head_top_t) self._painter.draw_line(head_bottom, head_top) head_left_t = head_top_t.dot( get_translate_matrix(0, 0, half_head_width)) head_left = apply_matrix_to_origin(head_left_t) head_right_t = head_top_t.dot( get_translate_matrix(0, 0, -half_head_width)) head_right = apply_matrix_to_origin(head_right_t) self._painter.draw_line(head_left, head_right)
def draw_left_arm(self, left_shoulder_lateral_radians=0, left_shoulder_frontal_radians=0, left_elbow_lateral_radians=0, color='c-', draw=True): left_shoulder_lateral = Joint( dh_params=(-half_pi + left_shoulder_lateral_radians, shoulder_length, 0, -half_pi), initial_transformation=self._get_torso_corner_transformation( left_shoulder_position, 'x', -half_pi)) left_shoulder_frontal = Joint( prev_joint=left_shoulder_lateral, dh_params=(half_pi - left_shoulder_frontal_radians, 0, -upper_arm_length, -half_pi)) left_elbow_lateral = Joint(prev_joint=left_shoulder_frontal, dh_params=(-left_elbow_lateral_radians, 0, 0, 0)) left_wrist_t = left_elbow_lateral.link_t.dot( get_translate_matrix(-lower_arm_length, 0, 0)) left_wrist = apply_matrix_to_origin(left_wrist_t) if draw: self.draw_link(left_shoulder_lateral, color) self.draw_link(left_shoulder_frontal, color) self._painter.draw_line(left_elbow_lateral.vertex, left_wrist, color=color) return left_shoulder_lateral, left_shoulder_frontal, left_elbow_lateral, left_wrist
def _get_torso_corner_transformation(translation_vector, rotate_along, rotate_radians): r = get_rotate_matrix_along_axis(rotate_along, rotate_radians) t = get_translate_matrix(*translation_vector) transformation = t.dot(r) return transformation
def draw_neck_and_head(self, neck_transversal_radians=0, neck_lateral_radians=0): neck_transversal = Joint(dh_params=(0 + neck_transversal_radians, neck_length, 0, half_pi), initial_transformation=get_translate_matrix(0, 0, torso_height)) neck_lateral = Joint(prev_joint=neck_transversal, dh_params=(-neck_lateral_radians, 0, 0, 0)) self.draw_link(neck_transversal) self._draw_head(head_t=neck_lateral.link_t)
def draw_neck_and_head(self, neck_transversal_radians=0, neck_lateral_radians=0): neck_transversal = Joint( dh_params=(0 + neck_transversal_radians, neck_length, 0, half_pi), initial_transformation=get_translate_matrix(0, 0, torso_height)) neck_lateral = Joint(prev_joint=neck_transversal, dh_params=(-neck_lateral_radians, 0, 0, 0)) self.draw_link(neck_transversal) self._draw_head(head_t=neck_lateral.link_t)
def draw_left_arm(self, left_shoulder_lateral_radians=0, left_shoulder_frontal_radians=0, left_elbow_lateral_radians=0, color='c-', draw=True): left_shoulder_lateral = Joint(dh_params=(-half_pi + left_shoulder_lateral_radians, shoulder_length, 0, -half_pi), initial_transformation=self._get_torso_corner_transformation(left_shoulder_position, 'x', -half_pi)) left_shoulder_frontal = Joint(prev_joint=left_shoulder_lateral, dh_params=(half_pi - left_shoulder_frontal_radians, 0, -upper_arm_length, -half_pi)) left_elbow_lateral = Joint(prev_joint=left_shoulder_frontal, dh_params=(-left_elbow_lateral_radians, 0, 0, 0)) left_wrist_t = left_elbow_lateral.link_t.dot(get_translate_matrix(-lower_arm_length, 0, 0)) left_wrist = apply_matrix_to_origin(left_wrist_t) if draw: self.draw_link(left_shoulder_lateral, color) self.draw_link(left_shoulder_frontal, color) self._painter.draw_line(left_elbow_lateral.vertex, left_wrist, color=color) return left_shoulder_lateral, left_shoulder_frontal, left_elbow_lateral, left_wrist