def display(self, pose: util.Pose):
"""Displays the precomputed view at a specific pose in 3d space.
:param pose: Where to display the cube.
"""
glPushMatrix()
# TODO if cube_pose.is_accurate is False, render half-translucent?
# (This would require using a shader, or having duplicate objects)
cube_matrix = pose.to_matrix()
glMultMatrixf(cube_matrix.in_row_order)
# Cube is drawn slightly larger than the 10mm to 1 cm scale, as the model looks small otherwise
cube_scale_amt = 10.7
glScalef(cube_scale_amt, cube_scale_amt, cube_scale_amt)
self.display_all()
glPopMatrix()
def display(self, pose: util.Pose, head_angle: util.Angle,
lift_position: util.Distance):
"""Displays the precomputed view at a specific pose in 3d space.
:param pose: Where to display the robot.
"""
if not self._display_lists:
return
robot_matrix = pose.to_matrix()
head_angle_degrees = head_angle.degrees
# Get the angle of Vector's lift for rendering - we subtract the angle
# of the lift in the default pose in the object, and apply the inverse
# rotation
sin_angle = (lift_position.distance_mm -
LIFT_PIVOT_HEIGHT_MM) / LIFT_ARM_LENGTH_MM
angle_radians = math.asin(sin_angle)
lift_angle = -(angle_radians - LIFT_ANGLE_IN_DEFAULT_POSE)
lift_angle_degrees = math.degrees(lift_angle)
glPushMatrix()
glEnable(GL_LIGHTING)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glEnable(GL_BLEND)
glMultMatrixf(robot_matrix.in_row_order)
robot_scale_amt = 10.0 # cm to mm
glScalef(robot_scale_amt, robot_scale_amt, robot_scale_amt)
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL)
self._display_vector_body()
self._display_vector_lift(lift_angle_degrees)
self._display_vector_head(head_angle_degrees)
glDisable(GL_LIGHTING)
glPopMatrix()