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