def draw(self):
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glPushMatrix()
#looking at x/y plane - down z axis
glTranslatef(self.transX, self.transY, self.transZ)
glRotatef(self.angleY, 0.0, 1.0, 0.0)
glRotatef(self.angleX, 1.0, 0.0, 0.0)
if config.floor_on:
self.room.render()
glColor3f(0, 0, 0)
#display.draw_axes(20, '1')
self.robot.render()
glPopMatrix()
# reference axis
glPushMatrix()
glTranslatef(self.zoom * .07 * self.width,
self.zoom * .07 * self.height, 400.0)
glRotatef(self.angleY, 0.0, 1.0, 0.0)
glRotatef(self.angleX, 1.0, 0.0, 0.0)
display.draw_axes(self.zoom * 13, '')
glPopMatrix()
if self.aux_msg_enabled:
glPushMatrix()
glTranslatef(-self.zoom * .094 * self.width,
-self.zoom * .094 * self.height, 400.0)
glColor4f(0.0, 0.0, 0.0, 1.0)
glPushMatrix()
display.text_at_pos(0,
self.zoom * 1 * 3,
0,
self.aux_msg,
font=GLUT_BITMAP_9_BY_15)
glPopMatrix()
glPopMatrix()
if not self.hide_cli:
self.render_cli()
glutSwapBuffers()
def draw(self):
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glPushMatrix()
#looking at x/y plane - down z axis
glTranslatef(self.transX, self.transY, self.transZ)
glRotatef(self.angleY, 0.0, 1.0, 0.0)
glRotatef(self.angleX, 1.0, 0.0, 0.0)
if config.floor_on:
self.room.render()
glColor3f(0, 0, 0)
#display.draw_axes(20, '1')
self.robot.render()
glPopMatrix()
# reference axis
glPushMatrix()
glTranslatef(self.zoom*.07*self.width, self.zoom*.07*self.height, 400.0)
glRotatef(self.angleY, 0.0, 1.0, 0.0)
glRotatef(self.angleX, 1.0, 0.0, 0.0)
display.draw_axes(self.zoom*13, '')
glPopMatrix()
if self.aux_msg_enabled:
glPushMatrix()
glTranslatef(-self.zoom*.094*self.width, -self.zoom*.094*self.height, 400.0)
glColor4f(0.0, 0.0, 0.0, 1.0)
glPushMatrix()
display.text_at_pos(0, self.zoom*1*3, 0, self.aux_msg, font=GLUT_BITMAP_9_BY_15)
glPopMatrix()
glPopMatrix()
if not self.hide_cli:
self.render_cli()
glutSwapBuffers()
def render(self):
for link in self.links:
R = link.R
P = link.P
h = link.h
if config.enable_lighting:
material.black()
else:
glColor3f(0, 0, 0)
glPushMatrix()
glLineWidth(15)
glBegin(GL_LINES)
if not link.is_prismatic():
glVertex3f(0, 0, 0)
glVertex3f(P[0], P[1], P[2])
else:
glVertex3f(0, 0, 0)
glVertex3f(P[0] - link.q * link.h[0],
P[1] - link.q * link.h[1],
P[2] - link.q * link.h[2])
glEnd()
glTranslate(P[0], P[1], P[2])
if config.enable_axis:
if link.index != self.N:
display.draw_axes(20, str(link.index))
else:
display.draw_axes(20, 'T')
# draw joint
if link.is_prismatic(): # prismatic joint
display.draw_prismatic_joint([[0], [0], [0]], link.q * link.h,
10)
elif link.is_rotational():
if config.enable_lighting:
material.green()
else:
glColor3f(0, 0.6, 0)
display.draw_rotational_joint(h * 10, h * -10, 8,
link.q * 180 / PI)
if config.enable_lighting:
material.grey()
else:
glColor3f(0.3, 0.3, 0.3)
glRotate(link.q * 180 / PI, link.h[0], link.h[1], link.h[2])
elif (R == eye(3)).all(): # link - no joint
pass
# pop all joints off
for matPop in self.links:
glPopMatrix()
glLineWidth(5)
glPointSize(10)
# only save the last whatever points
self.trace = self.trace[-config.max_trace:]
if config.enable_ghost:
if config.enable_lighting:
material.grey()
else:
glColor3f(0.4, 0.4, 0.4)
glPointSize(8)
for verts, i in zip(self.trace, range(len(self.trace))):
if i % config.ghost_interval == 1:
glBegin(GL_POINTS)
for vert in verts:
glVertex3f(vert[0], vert[1], vert[2])
glEnd()
if config.enable_lighting:
material.grey()
else:
glColor3f(0.7, 0.7, 0.7)
for verts, i in zip(self.trace, range(len(self.trace))):
if i % config.ghost_interval == 1:
glBegin(GL_LINE_STRIP)
for vert in verts:
glVertex3f(vert[0], vert[1], vert[2])
glEnd()
if config.enable_trace:
# saved tool positions
if config.enable_lighting:
material.red()
else:
glColor3f(1.0, 0.0, 0)
glBegin(GL_LINE_STRIP)
for verts in self.trace:
vert = verts[-1]
glVertex3f(vert[0], vert[1], vert[2])
glEnd()
def render(self):
for link in self.links:
R = link.R
P = link.P
h = link.h
if config.enable_lighting:
material.black()
else:
glColor3f(0, 0, 0)
glPushMatrix()
glLineWidth(15)
glBegin(GL_LINES)
if not link.is_prismatic():
glVertex3f(0, 0, 0)
glVertex3f(P[0], P[1], P[2])
else:
glVertex3f(0, 0, 0)
glVertex3f(P[0]-link.q*link.h[0], P[1] - link.q*link.h[1], P[2] - link.q*link.h[2])
glEnd()
glTranslate(P[0], P[1], P[2])
if config.enable_axis:
if link.index != self.N:
display.draw_axes(20, str(link.index))
else:
display.draw_axes(20, 'T')
# draw joint
if link.is_prismatic(): # prismatic joint
display.draw_prismatic_joint([[0], [0], [0]], link.q*link.h, 10)
elif link.is_rotational():
if config.enable_lighting:
material.green()
else:
glColor3f(0, 0.6, 0)
display.draw_rotational_joint(h*10, h*-10, 8, link.q*180/PI)
if config.enable_lighting:
material.grey()
else:
glColor3f(0.3, 0.3, 0.3)
glRotate(link.q * 180 / PI, link.h[0], link.h[1], link.h[2])
elif (R == eye(3)).all(): # link - no joint
pass
# pop all joints off
for matPop in self.links:
glPopMatrix()
glLineWidth(5)
glPointSize(10)
# only save the last whatever points
self.trace = self.trace[-config.max_trace:]
if config.enable_ghost:
if config.enable_lighting:
material.grey()
else:
glColor3f(0.4, 0.4, 0.4)
glPointSize(8)
for verts, i in zip(self.trace, range(len(self.trace))):
if i % config.ghost_interval == 1:
glBegin(GL_POINTS)
for vert in verts:
glVertex3f(vert[0], vert[1], vert[2])
glEnd()
if config.enable_lighting:
material.grey()
else:
glColor3f(0.7, 0.7, 0.7)
for verts, i in zip(self.trace, range(len(self.trace))):
if i % config.ghost_interval == 1:
glBegin(GL_LINE_STRIP)
for vert in verts:
glVertex3f(vert[0], vert[1], vert[2])
glEnd()
if config.enable_trace:
# saved tool positions
if config.enable_lighting:
material.red()
else:
glColor3f(1.0, 0.0, 0)
glBegin(GL_LINE_STRIP)
for verts in self.trace:
vert = verts[-1]
glVertex3f(vert[0], vert[1], vert[2])
glEnd()
