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 _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 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 vertex(self): if self._vertex is None: self._vertex = apply_matrix_to_origin(self.t) return self._vertex