def draw_rotational_joint_endCap(r, sides):
if config.enable_lighting:
material.blue()
else:
glColor3f(0, 1.0, 1.0)
glBegin(GL_TRIANGLE_FAN)
glVertex3f(0, r / 2, 0)
for angle in numpy.arange(0, 2 * PI, 2 * PI / sides):
glVertex3f(math.cos(angle) * r, math.sin(angle) * r, 0)
glVertex3f(r, 0, 0)
glEnd()
if config.enable_lighting:
material.black()
else:
glColor3f(0, 0, 0)
glLineWidth(1.5)
glBegin(GL_LINE_LOOP)
for angle in numpy.arange(0, 2 * PI, 2 * PI / sides):
glVertex3f(math.cos(angle) * r, math.sin(angle) * r, 0)
glEnd()
def arrow(z):
glVertex3f(0, 0, z)
glVertex3f(r, 0, z)
glVertex3f(r, 0, z)
glVertex3f(r - r * .5, r * .5, z)
glVertex3f(r, 0, z)
glVertex3f(r - r * .5, -r * .5, z)
glBegin(GL_LINES)
arrow(0.1)
arrow(-0.1)
glEnd()
glLineWidth(1.0)
def draw_rotational_joint_endCap(r, sides):
if config.enable_lighting:
material.blue()
else:
glColor3f(0, 1.0, 1.0)
glBegin(GL_TRIANGLE_FAN)
glVertex3f(0, r/2, 0)
for angle in numpy.arange(0, 2*PI, 2*PI/sides):
glVertex3f(math.cos(angle)*r, math.sin(angle)*r,0)
glVertex3f(r, 0, 0)
glEnd()
if config.enable_lighting:
material.black()
else:
glColor3f(0, 0, 0)
glLineWidth(1.5)
glBegin(GL_LINE_LOOP)
for angle in numpy.arange(0, 2*PI, 2*PI/sides):
glVertex3f(math.cos(angle)*r, math.sin(angle)*r, 0)
glEnd()
def arrow(z):
glVertex3f(0, 0, z)
glVertex3f(r, 0, z)
glVertex3f(r, 0, z)
glVertex3f(r-r*.5, r*.5, z)
glVertex3f(r, 0, z)
glVertex3f(r-r*.5, -r*.5, z)
glBegin(GL_LINES)
arrow(0.1)
arrow(-0.1)
glEnd()
glLineWidth(1.0)
def text_at_pos(x, y, z, text, font=GLUT_BITMAP_TIMES_ROMAN_24):
if config.enable_lighting:
material.black()
else:
glColor3f(0, 0, 0)
glRasterPos3f(x, y, z)
draw_text(text, font)
def text_at_pos(x, y, z, text, font=GLUT_BITMAP_TIMES_ROMAN_24):
if config.enable_lighting:
material.black()
else:
glColor3f(0, 0, 0)
glRasterPos3f(x, y, z)
draw_text(text, font)
def draw_prismatic_joint(startP, endP, size):
glPushMatrix()
# This is the default direction for the rectangular-prism to face in OpenGL - z axis
z = [0.0, 0.0, -1.0]
# Get diff between two points you want rectangular-prism along
p = numpy.array(startP - endP, float).transpose()[0]
# Get CROSS product (the axis of rotation)
t = numpy.cross(z, p)
# Get angle. LENGTH is magnitude of the vector
length = math.sqrt(numpy.dot(p, p))
angle = 180 / PI * math.acos(numpy.dot(z, p) / length)
# glTranslate(endP[0],endP[1],endP[2])
glRotate(angle, t[0], t[1], t[2])
if (length < 5):
length = 5
length = -length
if config.enable_lighting:
material.magenta()
else:
glColor3f(1.0, 0.0, 1.0)
def quickv(v):
glVertex3f(v[0], v[1], v[2])
v1 = (-size / 2, -size / 2, 0)
v2 = (size / 2, -size / 2, 0)
v3 = (-size / 2, -size / 2, length)
v4 = (size / 2, -size / 2, length)
v5 = (-size / 2, size / 2, length)
v6 = (size / 2, size / 2, length)
v7 = (-size / 2, size / 2, 0)
v8 = (size / 2, size / 2, 0)
q1234 = [v2, v1, v4, v3, v6, v5, v8, v7, v2, v1]
q56 = [v8, v6, v4, v2, v7, v5, v3, v1]
outline = [(v1, v2), (v2, v8), (v8, v7), (v7, v1)]
outline += [(v1, v3), (v2, v4), (v7, v5), (v8, v6)]
outline += [(v5, v6), (v6, v4), (v4, v3), (v3, v5)]
#Quads 1 2 3 4
glBegin(GL_QUAD_STRIP)
for v in q1234:
quickv(v)
glEnd()
#Quad 5 & 6
glBegin(GL_QUADS)
for v in q56:
quickv(v)
glEnd()
# outline
glLineWidth(1.5)
if config.enable_lighting:
material.black()
else:
glColor3f(0, 0, 0)
glBegin(GL_LINES)
for line in outline:
quickv(line[0])
quickv(line[1])
glEnd()
glLineWidth(1.0)
glPopMatrix()
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()
def draw_prismatic_joint(startP, endP, size):
glPushMatrix()
# This is the default direction for the rectangular-prism to face in OpenGL - z axis
z = [0.0, 0.0, -1.0]
# Get diff between two points you want rectangular-prism along
p = numpy.array(startP - endP, float).transpose()[0]
# Get CROSS product (the axis of rotation)
t = numpy.cross(z, p)
# Get angle. LENGTH is magnitude of the vector
length = math.sqrt(numpy.dot(p, p))
angle = 180 / PI * math.acos(numpy.dot(z, p) / length)
# glTranslate(endP[0],endP[1],endP[2])
glRotate(angle, t[0], t[1], t[2])
if (length < 5):
length = 5
length =- length
if config.enable_lighting:
material.magenta()
else:
glColor3f(1.0, 0.0, 1.0)
def quickv(v):
glVertex3f(v[0], v[1], v[2])
v1 = (-size/2, -size/2, 0)
v2 = (size/2, -size/2, 0)
v3 = (-size/2, -size/2, length)
v4 = (size/2, -size/2, length)
v5 = (-size/2, size/2, length)
v6 = (size/2, size/2, length)
v7 = (-size/2, size/2, 0)
v8 = (size/2, size/2, 0)
q1234 = [v2, v1, v4, v3, v6, v5, v8, v7, v2, v1]
q56 = [v8, v6, v4, v2, v7, v5, v3, v1]
outline = [(v1, v2), (v2, v8), (v8, v7), (v7, v1)]
outline += [(v1, v3), (v2, v4), (v7, v5), (v8, v6)]
outline += [(v5, v6), (v6, v4), (v4, v3), (v3, v5)]
#Quads 1 2 3 4
glBegin(GL_QUAD_STRIP)
for v in q1234:
quickv(v)
glEnd()
#Quad 5 & 6
glBegin(GL_QUADS)
for v in q56:
quickv(v)
glEnd()
# outline
glLineWidth(1.5)
if config.enable_lighting:
material.black()
else:
glColor3f(0, 0, 0)
glBegin(GL_LINES)
for line in outline:
quickv(line[0])
quickv(line[1])
glEnd()
glLineWidth(1.0)
glPopMatrix()
