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(startP, endP, r, link_rotation): #draws cylinder from sP to eP
glPushMatrix()
# This is the default direction for the cylinders to face in OpenGL - z axis
z = numpy.array([0.0, 0.0, 1.0])
# Get diff between two points you want cylinder along
startP = numpy.array(startP, float).transpose()[0]
endP = numpy.array(endP, float).transpose()[0]
p = startP - endP
# 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)
glRotate(angle, t[0], t[1], t[2])
glRotate(link_rotation + 90.0, 0.0, 0.0, 1.0)
if config.enable_lighting:
material.blue()
else:
glColor3f(0, 1.0, 1.0)
sides = r*5
glTranslate(0, 0, -length/2)
quad = gluNewQuadric()
gluQuadricOrientation(quad, GLU_OUTSIDE);
gluCylinder(quad, r, r, length, sides, 1);
gluDeleteQuadric(quad)
draw_rotational_joint_endCap(r, sides)
glTranslate(0, 0, length)
draw_rotational_joint_endCap(r, sides)
glPopMatrix()
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(startP, endP, r, link_rotation): #draws cylinder from sP to eP
glPushMatrix()
# This is the default direction for the cylinders to face in OpenGL - z axis
z = numpy.array([0.0, 0.0, 1.0])
# Get diff between two points you want cylinder along
startP = numpy.array(startP, float).transpose()[0]
endP = numpy.array(endP, float).transpose()[0]
p = startP - endP
# 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)
glRotate(angle, t[0], t[1], t[2])
glRotate(link_rotation + 90.0, 0.0, 0.0, 1.0)
if config.enable_lighting:
material.blue()
else:
glColor3f(0, 1.0, 1.0)
sides = r*5
glTranslate(0, 0, -length/2)
quad = gluNewQuadric()
gluQuadricOrientation(quad, GLU_OUTSIDE);
gluCylinder(quad, r, r, length, sides, 1);
gluDeleteQuadric(quad)
draw_rotational_joint_endCap(r, sides)
glTranslate(0, 0, length)
draw_rotational_joint_endCap(r, sides)
glPopMatrix()
def draw_axes(axes_l=10, number=''):
glLineWidth(5.0)
glBegin(GL_LINES)
# x axis, red
if config.enable_lighting:
material.red()
else:
glColor3f(1.0, 0, 0)
glVertex3f(0, 0, 0)
glVertex3f(axes_l, 0, 0)
# y axis, green
if config.enable_lighting:
material.green()
else:
glColor3f(0, 1.0, 0)
glVertex3f(0, 0, 0)
glVertex3f(0, axes_l, 0)
# z axis
if config.enable_lighting:
material.blue()
else:
glColor3f(0, 0, 1.0)
glVertex3f(0, 0, 0)
glVertex3f(0, 0, axes_l)
glEnd()
glLineWidth(1.0)
offset = axes_l / 7
text_at_pos(offset + axes_l, 0, 0, 'X' + number)
text_at_pos(0, offset + axes_l, 0, 'Y' + number)
text_at_pos(0, 0, offset + axes_l, 'Z' + number)
def draw_axes(axes_l = 10, number=''):
glLineWidth(5.0)
glBegin(GL_LINES)
# x axis, red
if config.enable_lighting:
material.red()
else:
glColor3f(1.0, 0, 0)
glVertex3f(0, 0, 0)
glVertex3f(axes_l, 0, 0)
# y axis, green
if config.enable_lighting:
material.green()
else:
glColor3f(0, 1.0, 0)
glVertex3f(0, 0, 0)
glVertex3f(0, axes_l, 0)
# z axis
if config.enable_lighting:
material.blue()
else:
glColor3f(0, 0, 1.0)
glVertex3f(0, 0, 0)
glVertex3f(0, 0, axes_l)
glEnd()
glLineWidth(1.0)
offset = axes_l/7
text_at_pos(offset + axes_l, 0, 0, 'X' + number)
text_at_pos(0, offset + axes_l, 0, 'Y' + number)
text_at_pos(0, 0, offset + axes_l, 'Z' + number)