def makeSpline(self,waypointTrajectory): """Computes natural velocities for a standard configuration- space Trajectory to make it smoother.""" velocities = [] t = waypointTrajectory d = len(t.milestones[0]) if len(t.milestones)==1: velocities.append([0]*d) elif len(t.milestones)==2: v = vectorops.mul(vectorops.sub(t.milestones[1],t.milestones[0]),1.0/(t.times[1]-t.times[0])) velocities.append(v) velocities.append(v) else : for i in range(1,len(waypointTrajectory.milestones)-1): v = vectorops.mul(vectorops.sub(t.milestones[i+1],t.milestones[i]),1.0/(t.times[i+1]-t.times[i-1])) velocities.append(v) #start velocity as quadratic x2 = vectorops.madd(t.milestones[1],velocities[0],-1.0/3.0) x1 = vectorops.madd(x2,vectorops.sub(t.milestones[1],t.milestones[0]),-1.0/3.0) v0 = vectorops.mul(vectorops.sub(x1,t.milestones[0]),3.0) #terminal velocity as quadratic xn_2 = vectorops.madd(t.milestones[-2],velocities[-1],1.0/3.0) xn_1 = vectorops.madd(xn_2,vectorops.sub(t.milestones[-1],t.milestones[-2]),1.0/3.0) vn = vectorops.mul(vectorops.sub(t.milestones[-1],xn_1),3.0) velocities = [v0]+velocities+[vn] self.__init__(waypointTrajectory.times[:],waypointTrajectory.milestones,velocities)
def deriv(self,t,endBehavior='halt'): """Evaluates the trajectory velocity using piecewise linear interpolation. If endBehavior='loop' then the trajectory loops forver.""" i,u = self.getSegment(t,endBehavior) if i<0: return [0.0]*len(self.milestones[0]) elif i>=len(self.milestones): return [0.0]*len(self.milestones[-1]) return vectorops.mul(self.difference(self.milestones[i+1],self.milestones[i],u),1.0/(self.times[i+1]-self.times[i]))
def drawRawLine():
glDisable(GL_LIGHTING)
glColor4f(*self.attributes.get("color",[0.5,0,0.5,1]))
glLineWidth(self.attributes.get("width",3.0))
glBegin(GL_LINES)
glVertex3f(0,0,0)
glVertex3f(*vectorops.mul(d,self.attributes.get("length",0.1)))
glEnd()
glLineWidth(1.0)
def triangle(a,b,c,lighting=True):
if lighting:
n = vectorops.cross(vectorops.sub(b,a),vectorops.sub(c,a))
n = vectorops.mul(n,1.0/vectorops.norm(n))
glNormal3f(*n)
glBegin(GL_TRIANGLES)
glVertex3f(*a)
glVertex3f(*b)
glVertex3f(*c)
glEnd();
def drawRaw():
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
L = self.attributes.get("length",0.15)
source = [0,0,0]
glColor4f(*self.attributes.get("color",[0,1,1,1]))
glBegin(GL_LINES)
glVertex3f(*source)
glVertex3f(*vectorops.mul(item.localCoordinates(),L))
glEnd()
glEnable(GL_DEPTH_TEST)
def sample():
"""Returns a uniformly distributed rotation matrix."""
import random
q = [random.gauss(0,1),random.gauss(0,1),random.gauss(0,1),random.gauss(0,1)]
q = vectorops.unit(q)
theta = math.acos(q[3])*2.0
if abs(theta) < 1e-8:
m = [0,0,0]
else:
m = vectorops.mul(vectorops.unit(q[0:3]),theta)
return from_moment(m)
def triangle(a,b,c,lighting=True):
"""Draws a 3D triangle with points a,b,c. If lighting is true, computes
a GL normal vector dynamically"""
if lighting:
n = vectorops.cross(vectorops.sub(b,a),vectorops.sub(c,a))
n = vectorops.mul(n,1.0/vectorops.norm(n))
glNormal3f(*n)
glBegin(GL_TRIANGLES)
glVertex3f(*a)
glVertex3f(*b)
glVertex3f(*c)
glEnd();
def triangle(a, b, c, lighting=True):
"""Draws a 3D triangle with points a,b,c. If lighting is true, computes
a GL normal vector dynamically"""
if lighting:
n = vectorops.cross(vectorops.sub(b, a), vectorops.sub(c, a))
n = vectorops.mul(n, 1.0 / vectorops.norm(n))
glNormal3f(*n)
glBegin(GL_TRIANGLES)
glVertex3f(*a)
glVertex3f(*b)
glVertex3f(*c)
glEnd()
def segment_closest_point(s, x):
"""Given a segment s=(a,b) and a point x, returns a tuple containing
the closest point parameter in [0,1] and the closest point on s"""
dir = vectorops.sub(s[1], s[0])
d = vectorops.sub(x, s[0])
dp = vectorops.dot(d, dir)
dnorm2 = vectorops.dot(dir, dir)
if dp < 0:
return (0.0, s[0])
if dp > dnorm2:
return (1.0, s[1])
proj = vectorops.add(s[0], vectorops.mul(dir, dp / dnorm2))
return (dp / dnorm2, proj)
def rotation(axis, angle):
"""Given a unit axis and an angle in radians, returns the rotation
matrix."""
cm = math.cos(angle)
sm = math.sin(angle)
#m = s[r]-c[r][r]+rrt = s[r]-c(rrt-I)+rrt = cI + rrt(1-c) + s[r]
R = vectorops.mul(cross_product(axis), sm)
for i in xrange(3):
for j in xrange(3):
R[i * 3 + j] += axis[i] * axis[j] * (1. - cm)
R[0] += cm
R[4] += cm
R[8] += cm
return R
def rotation(axis,angle):
"""Given a unit axis and an angle in radians, returns the rotation
matrix."""
cm = math.cos(angle)
sm = math.sin(angle)
#m = s[r]-c[r][r]+rrt = s[r]-c(rrt-I)+rrt = cI + rrt(1-c) + s[r]
R = vectorops.mul(cross_product(axis),sm)
for i in xrange(3):
for j in xrange(3):
R[i*3+j] += axis[i]*axis[j]*(1.-cm)
R[0] += cm
R[4] += cm
R[8] += cm
return R
def triangle(a,b,c,lighting=True,filled=True):
"""Draws a 3D triangle with points a,b,c. If lighting is true, computes
a GL normal vector dynamically. If filled=true, it is drawn filled,
otherwise, it is drawn as a wireframe."""
if lighting:
n = vectorops.cross(vectorops.sub(b,a),vectorops.sub(c,a))
n = vectorops.mul(n,1.0/vectorops.norm(n))
glNormal3f(*n)
if filled:
glBegin(GL_TRIANGLES)
else:
glBegin(GL_LINE_LOOP)
glVertex3f(*a)
glVertex3f(*b)
glVertex3f(*c)
glEnd();
def sample():
"""Returns a uniformly distributed rotation matrix."""
import random
q = [
random.gauss(0, 1),
random.gauss(0, 1),
random.gauss(0, 1),
random.gauss(0, 1)
]
q = vectorops.unit(q)
theta = math.acos(q[3]) * 2.0
if abs(theta) < 1e-8:
m = [0, 0, 0]
else:
m = vectorops.mul(vectorops.unit(q[0:3]), theta)
return from_moment(m)
def triangle(a, b, c, lighting=True, filled=True):
"""Draws a 3D triangle with points a,b,c. If lighting is true, computes
a GL normal vector dynamically. If filled=true, it is drawn filled,
otherwise, it is drawn as a wireframe."""
if lighting:
n = vectorops.cross(vectorops.sub(b, a), vectorops.sub(c, a))
n = vectorops.mul(n, 1.0 / vectorops.norm(n))
glNormal3f(*n)
if filled:
glBegin(GL_TRIANGLES)
else:
glBegin(GL_LINE_LOOP)
glVertex3f(*a)
glVertex3f(*b)
glVertex3f(*c)
glEnd()
def vector_rotation(v1,v2):
"""Finds the minimal-angle matrix that rotates v1 to v2. v1 and v2
are assumed to be nonzero"""
a1 = vectorops.unit(v1)
a2 = vectorops.unit(v2)
cp = vectorops.cross(a1,a2)
dp = vectorops.dot(a1,a2)
if abs(vectorops.norm(cp)) < 1e-4:
if dp < 0:
R0 = canonical(a1)
#return a rotation 180 degrees about the canonical y axis
return rotation(R0[3:6],math.pi)
else:
return identity()
else:
angle = math.acos(max(min(dp,1.0),-1.0))
axis = vectorops.mul(cp,1.0/vectorops.norm(cp))
return rotation(axis,angle)
def vector_rotation(v1, v2):
"""Finds the minimal-angle matrix that rotates v1 to v2. v1 and v2
are assumed to be nonzero"""
a1 = vectorops.unit(v1)
a2 = vectorops.unit(v2)
cp = vectorops.cross(a1, a2)
dp = vectorops.dot(a1, a2)
if abs(vectorops.norm(cp)) < 1e-4:
if dp < 0:
R0 = canonical(a1)
#return a rotation 180 degrees about the canonical y axis
return rotation(R0[3:6], math.pi)
else:
return identity()
else:
angle = math.acos(max(min(dp, 1.0), -1.0))
axis = vectorops.mul(cp, 1.0 / vectorops.norm(cp))
return rotation(axis, angle)
def scale(self,amount):
"""Scales this direction by a scalar amount"""
self._localCoordinates = vectorops.mul(self._localCoordinates,amount)
def path_tangent(self, eps=0.1):
p1 = self.eval_path(self.slider_param + eps)
p2 = self.eval_path(self.slider_param - eps)
return vectorops.mul(vectorops.sub(p1, p2),
1.0 / vectorops.distance(p1, p2))
def from_moment(w):
"""Converts a moment representation w to a 3D rotation matrix."""
length = vectorops.norm(w)
if length < 1e-7: return identity()
return rotation(vectorops.mul(w,1.0/length),length)
def path_velocity(self, eps=0.1):
p1 = self.eval_path(self.current_time)
p2 = self.eval_path(self.current_time + eps)
return vectorops.mul(vectorops.sub(p2, p1), 1.0 / eps)
def bezier_to_hermite(x1, x2, x3, x4):
"""Returns the cubic bezier representation of a hermite curve"""
v1 = vectorops.mul(vectorops.sub(x2, x1), 3.0)
v2 = vectorops.mul(vectorops.sub(x4, x3), 3.0)
return x1, v1, x4, v2
def from_moment(w):
"""Converts a moment representation w to a 3D rotation matrix."""
length = vectorops.norm(w)
if length < 1e-7: return identity()
return rotation(vectorops.mul(w, 1.0 / length), length)
def hermite_subdivide(x1, v1, x2, v2, u=0.5):
"""Subdivides a hermite curve into two hermite curves."""
xm = hermite_eval(x1, v1, x2, v2, u)
vm = hermite_deriv(x1, v1, x2, v2, u)
return [(x1, vectorops.mul(v1, u), xm, vectorops.mul(vm, u)),
(xm, vectorops.mul(vm, 1.0 - u), x2, vectorops.mul(v2, 1.0 - u))]
def scale(self, amount):
"""Scales this direction by a scalar amount"""
self._localCoordinates = vectorops.mul(self._localCoordinates, amount)
def bezier_to_hermite(x1,x2,x3,x4):
"""Returns the cubic bezier representation of a hermite curve"""
v1 = vectorops.mul(vectorops.sub(x2,x1),3.0)
v2 = vectorops.mul(vectorops.sub(x4,x3),3.0)
return x1,v1,x4,v2
def hermite_subdivide(x1,v1,x2,v2,u=0.5):
"""Subdivides a hermite curve into two hermite curves."""
xm = hermite_eval(x1,v1,x2,v2,u)
vm = hermite_deriv(x1,v1,x2,v2,u)
return [(x1,vectorops.mul(v1,u),xm,vectorops.mul(vm,u)),(xm,vectorops.mul(vm,1.0-u),x2,vectorops.mul(v2,1.0-u))]
def draw(self,world=None):
"""Draws the specified item in the specified world. If name
is given and text_hidden != False, then the name of the item is
shown."""
if self.hidden: return
item = self.item
name = self.name
#set appearance
if not self.useDefaultAppearance and hasattr(item,'appearance'):
if not hasattr(self,'oldAppearance'):
self.oldAppearance = item.appearance().clone()
if self.customAppearance != None:
print "Changing appearance of",name
item.appearance().set(self.customAppearance)
elif "color" in self.attributes:
print "Changing color of",name
item.appearance().setColor(*self.attributes["color"])
if hasattr(item,'drawGL'):
item.drawGL()
elif len(self.subAppearances)!=0:
for n,app in self.subAppearances.iteritems():
app.widget = self.widget
app.draw(world)
elif isinstance(item,coordinates.Point):
def drawRaw():
glDisable(GL_DEPTH_TEST)
glDisable(GL_LIGHTING)
glEnable(GL_POINT_SMOOTH)
glPointSize(self.attributes.get("size",5.0))
glColor4f(*self.attributes.get("color",[0,0,0,1]))
glBegin(GL_POINTS)
glVertex3f(0,0,0)
glEnd()
glEnable(GL_DEPTH_TEST)
#write name
self.displayCache[0].draw(drawRaw,[so3.identity(),item.worldCoordinates()])
if name != None:
self.drawText(name,vectorops.add(item.worldCoordinates(),[0,0,-0.05]))
elif isinstance(item,coordinates.Direction):
def drawRaw():
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
L = self.attributes.get("length",0.15)
source = [0,0,0]
glColor4f(*self.attributes.get("color",[0,1,1,1]))
glBegin(GL_LINES)
glVertex3f(*source)
glVertex3f(*vectorops.mul(item.localCoordinates(),L))
glEnd()
glEnable(GL_DEPTH_TEST)
#write name
self.displayCache[0].draw(drawRaw,item.frame().worldCoordinates(),parameters = item.localCoordinates())
if name != None:
self.drawText(name,vectorops.add(vectorops.add(item.frame().worldCoordinates()[1],item.worldCoordinates()),[0,0,-0.05]))
elif isinstance(item,coordinates.Frame):
t = item.worldCoordinates()
if item.parent() != None:
tp = item.parent().worldCoordinates()
else:
tp = se3.identity()
tlocal = item.relativeCoordinates()
def drawRaw():
glDisable(GL_DEPTH_TEST)
glDisable(GL_LIGHTING)
glLineWidth(2.0)
gldraw.xform_widget(tlocal,self.attributes.get("length",0.1),self.attributes.get("width",0.01))
glLineWidth(1.0)
#draw curve between frame and parent
if item.parent() != None:
d = vectorops.norm(tlocal[1])
vlen = d*0.5
v1 = so3.apply(tlocal[0],[-vlen]*3)
v2 = [vlen]*3
#glEnable(GL_BLEND)
#glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
#glColor4f(1,1,0,0.5)
glColor3f(1,1,0)
gldraw.hermite_curve(tlocal[1],v1,[0,0,0],v2,0.03)
#glDisable(GL_BLEND)
glEnable(GL_DEPTH_TEST)
#For some reason, cached drawing is causing OpenGL problems
#when the frame is rapidly changing
#self.displayCache[0].draw(drawRaw,transform=tp, parameters = tlocal)
glPushMatrix()
glMultMatrixf(sum(zip(*se3.homogeneous(tp)),()))
drawRaw()
glPopMatrix()
#write name
if name != None:
self.drawText(name,se3.apply(t,[-0.05]*3))
elif isinstance(item,coordinates.Transform):
#draw curve between frames
t1 = item.source().worldCoordinates()
if item.destination() != None:
t2 = item.destination().worldCoordinates()
else:
t2 = se3.identity()
d = vectorops.distance(t1[1],t2[1])
vlen = d*0.5
v1 = so3.apply(t1[0],[-vlen]*3)
v2 = so3.apply(t2[0],[vlen]*3)
def drawRaw():
glDisable(GL_DEPTH_TEST)
glDisable(GL_LIGHTING)
glColor3f(1,1,1)
gldraw.hermite_curve(t1[1],v1,t2[1],v2,0.03)
glEnable(GL_DEPTH_TEST)
#write name at curve
self.displayCache[0].draw(drawRaw,transform=None,parameters = (t1,t2))
if name != None:
self.drawText(name,spline.hermite_eval(t1[1],v1,t2[1],v2,0.5))
else:
types = resource.objectToTypes(item,world)
if isinstance(types,(list,tuple)):
#ambiguous, still need to figure out what to draw
validtypes = []
for t in types:
if t == 'Config':
if world != None and len(t) == world.robot(0).numLinks():
validtypes.append(t)
elif t=='Vector3':
validtypes.append(t)
elif t=='RigidTransform':
validtypes.append(t)
if len(validtypes) > 1:
print "Unable to draw item of ambiguous types",validtypes
return
if len(validtypes) == 0:
print "Unable to draw any of types",types
return
types = validtypes[0]
if types == 'Config':
if world:
robot = world.robot(0)
if not self.useDefaultAppearance:
oldAppearance = [robot.link(i).appearance().clone() for i in xrange(robot.numLinks())]
for i in xrange(robot.numLinks()):
robot.link(i).appearance().set(self.customAppearance)
oldconfig = robot.getConfig()
robot.setConfig(item)
robot.drawGL()
robot.setConfig(oldconfig)
if not self.useDefaultAppearance:
for (i,app) in enumerate(oldAppearance):
robot.link(i).appearance().set(app)
else:
print "Unable to draw Config's without a world"
elif types == 'Vector3':
def drawRaw():
glDisable(GL_LIGHTING)
glEnable(GL_POINT_SMOOTH)
glPointSize(self.attributes.get("size",5.0))
glColor4f(*self.attributes.get("color",[0,0,0,1]))
glBegin(GL_POINTS)
glVertex3f(0,0,0)
glEnd()
self.displayCache[0].draw(drawRaw,[so3.identity(),item])
if name != None:
self.drawText(name,vectorops.add(item,[0,0,-0.05]))
elif types == 'RigidTransform':
def drawRaw():
gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01))
self.displayCache[0].draw(drawRaw,transform=item)
if name != None:
self.drawText(name,se3.apply(item,[-0.05]*3))
elif types == 'IKGoal':
if hasattr(item,'robot'):
#need this to be built with a robot element.
#Otherwise, can't determine the correct transforms
robot = item.robot
elif world:
if world.numRobots() >= 1:
robot = world.robot(0)
else:
robot = None
else:
robot = None
if robot != None:
link = robot.link(item.link())
dest = robot.link(item.destLink()) if item.destLink()>=0 else None
while len(self.displayCache) < 3:
self.displayCache.append(CachedGLObject())
self.displayCache[1].name = self.name+" target position"
self.displayCache[2].name = self.name+" curve"
if item.numPosDims() != 0:
lp,wp = item.getPosition()
#set up parameters of connector
p1 = se3.apply(link.getTransform(),lp)
if dest != None:
p2 = se3.apply(dest.getTransform(),wp)
else:
p2 = wp
d = vectorops.distance(p1,p2)
v1 = [0.0]*3
v2 = [0.0]*3
if item.numRotDims()==3: #full constraint
R = item.getRotation()
def drawRaw():
gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01))
t1 = se3.mul(link.getTransform(),(so3.identity(),lp))
t2 = (R,wp) if dest==None else se3.mul(dest.getTransform(),(R,wp))
self.displayCache[0].draw(drawRaw,transform=t1)
self.displayCache[1].draw(drawRaw,transform=t2)
vlen = d*0.1
v1 = so3.apply(t1[0],[-vlen]*3)
v2 = so3.apply(t2[0],[vlen]*3)
elif item.numRotDims()==0: #point constraint
def drawRaw():
glDisable(GL_LIGHTING)
glEnable(GL_POINT_SMOOTH)
glPointSize(self.attributes.get("size",5.0))
glColor4f(*self.attributes.get("color",[0,0,0,1]))
glBegin(GL_POINTS)
glVertex3f(0,0,0)
glEnd()
self.displayCache[0].draw(drawRaw,transform=(so3.identity(),p1))
self.displayCache[1].draw(drawRaw,transform=(so3.identity(),p2))
#set up the connecting curve
vlen = d*0.5
d = vectorops.sub(p2,p1)
v1 = vectorops.mul(d,0.5)
#curve in the destination
v2 = vectorops.cross((0,0,0.5),d)
else: #hinge constraint
p = [0,0,0]
d = [0,0,0]
def drawRawLine():
glDisable(GL_LIGHTING)
glEnable(GL_POINT_SMOOTH)
glPointSize(self.attributes.get("size",5.0))
glColor4f(*self.attributes.get("color",[0,0,0,1]))
glBegin(GL_POINTS)
glVertex3f(*p)
glEnd()
glColor4f(*self.attributes.get("color",[0.5,0,0.5,1]))
glLineWidth(self.attributes.get("width",3.0))
glBegin(GL_LINES)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p,d,self.attributes.get("length",0.1)))
glEnd()
glLineWidth(1.0)
ld,wd = item.getRotationAxis()
p = lp
d = ld
self.displayCache[0].draw(drawRawLine,transform=link.getTransform(),parameters=(p,d))
p = wp
d = wd
self.displayCache[1].draw(drawRawLine,transform=dest.getTransform() if dest else se3.identity(),parameters=(p,d))
#set up the connecting curve
d = vectorops.sub(p2,p1)
v1 = vectorops.mul(d,0.5)
#curve in the destination
v2 = vectorops.cross((0,0,0.5),d)
def drawConnection():
glDisable(GL_DEPTH_TEST)
glDisable(GL_LIGHTING)
glColor3f(1,0.5,0)
gldraw.hermite_curve(p1,v1,p2,v2,0.03)
glEnable(GL_DEPTH_TEST)
self.displayCache[2].draw(drawConnection,transform=None,parameters = (p1,v1,p2,v2))
if name != None:
self.drawText(name,vectorops.add(wp,[-0.05]*3))
else:
wp = link.getTransform()[1]
if item.numRotDims()==3: #full constraint
R = item.getRotation()
def drawRaw():
gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01))
self.displayCache[0].draw(drawRaw,transform=link.getTransform())
self.displayCache[1].draw(drawRaw,transform=se3.mul(link.getTransform(),(R,[0,0,0])))
elif item.numRotDims() > 0:
#axis constraint
d = [0,0,0]
def drawRawLine():
glDisable(GL_LIGHTING)
glColor4f(*self.attributes.get("color",[0.5,0,0.5,1]))
glLineWidth(self.attributes.get("width",3.0))
glBegin(GL_LINES)
glVertex3f(0,0,0)
glVertex3f(*vectorops.mul(d,self.attributes.get("length",0.1)))
glEnd()
glLineWidth(1.0)
ld,wd = item.getRotationAxis()
d = ld
self.displayCache[0].draw(drawRawLine,transform=link.getTransform(),parameters=d)
d = wd
self.displayCache[1].draw(drawRawLine,transform=(dest.getTransform()[0] if dest else so3.identity(),wp),parameters=d)
else:
#no drawing
pass
if name != None:
self.drawText(name,se3.apply(wp,[-0.05]*3))
else:
print "Unable to draw item of type",types
#revert appearance
if not self.useDefaultAppearance and hasattr(item,'appearance'):
item.appearance().set(self.oldAppearance)
def difference(self,a,b,u): if u > 0.5: #do this from the b side return vectorops.mul(self.difference(b,a,1.0-u),-1.0) x = self.interpolate(a,b,u) return vectorops.mul(self.geodesic.difference(x,b),1.0/(1.0-u))