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 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()