def __init__(self):
Context.__init__(self)
self.Rtype = Type('V',9)
self.ttype = Type('V',3)
self.type = Type('L',2,[self.Rtype,self.ttype])
T = Variable("T",self.type)
R = Variable("R",self.Rtype)
t = Variable("t",self.ttype)
self.make = self.declare(array(R,t),"make",['R','t'])
self.identity = self.declare(se3.identity,"identity")
self.homogeneous = self.declare(se3.homogeneous,"homogeneous")
self.homogeneous.addSimplifier(['se3.identity'],lambda T:eye(4))
Rinv = so3.inv(T[0])
self.inv = self.declare(array(Rinv,neg(so3.apply(Rinv,T[1]))),"inv",['T'])
self.inv.autoSetJacobians()
self.inv.properties['inverse'] = weakref.proxy(self.inv)
self.inv.addSimplifier(['se3.identity'],lambda T:T)
self.mul = self.declare(se3.mul,"mul")
self.mul.setDeriv(0,lambda T1,T2,dT1:self.mul(dT1,T2),asExpr=True)
self.mul.setDeriv(1,lambda T1,T2,dT2:self.mul(T1,dT2),asExpr=True)
self.mul.addSimplifier(['se3.identity',None],(lambda T1,T2:T2),pre=True)
self.mul.addSimplifier([None,'se3.identity'],(lambda T1,T2:T1),pre=True)
self.mul.properties['associative'] = True
pt = Variable('pt',self.ttype)
self.apply = self.declare(so3.apply(T[0],pt)+T[1],"apply",['T','pt'])
#self.apply.setDeriv(0,lambda T,pt,dT:array(so3.apply(dT[0],pt),dT[1]))
#self.apply.setDeriv(1,lambda T,pt,dx:so3.apply(T[0],dx))
self.apply.addSimplifier([None,'zero'],lambda T,pt:T[1])
self.apply.addSimplifier(['se3.identity',None],lambda T,pt:pt)
self.apply.autoSetJacobians()
self.rotation = self.declare(T[0],"rotation",['T'])
self.translation = self.declare(T[1],"translation",['T'])
self.make.returnType = self.type
self.homogeneous.returnType = Type('M',(4,4))
self.homogeneous.argTypes = [self.type]
self.homogeneous.setDeriv(0,lambda T,dT:array([[dT[0][0],dT[0][3],dT[0][6],dT[1][0]],
[dT[0][1],dT[0][4],dT[0][7],dT[1][1]],
[dT[0][2],dT[0][5],dT[0][8],dT[1][2]],
[0.,0.,0.,0.]]),asExpr=True)
M = Variable("M",Type('M',(4,4)))
self.from_homogeneous = self.declare(array(array(M[0,0],M[1,0],M[2,0],M[0,1],M[1,1],M[2,1],M[0,2],M[1,2],M[2,2]),array(M[0,3],M[1,3],M[2,3])),'from_homogeneous',['M'])
self.from_homogeneous.autoSetJacobians()
self.matrix = self.declare(self.homogeneous(T),"matrix",['T'])
self.make.autoSetJacobians()
self.matrix.autoSetJacobians()
self.rotation.autoSetJacobians()
self.translation.autoSetJacobians()
self.identity.returnType = self.type
self.inv.returnType = self.type
self.inv.argTypes = [self.type]
self.mul.returnType = self.type
self.mul.argTypes = [self.type,self.type]
self.apply.returnType = self.ttype
self.apply.argTypes = [self.type,self.ttype]
self.rotation.returnType = self.Rtype
self.translation.returnType = self.ttype
def mul(T1,T2):
"""Composes two transformations."""
(R1,t1) = T1
(R2,t2) = T2
R = so3.mul(R1,R2)
t = vectorops.add(so3.apply(R1,t2),t1)
return (R,t)
def mul(T1,T2):
"""Composes two transformations."""
(R1,t1) = T1
(R2,t2) = T2
R = so3.mul(R1,R2)
t = vectorops.add(so3.apply(R1,t2),t1)
return (R,t)
def to(self,newframe):
"""Returns a Direction representing the same direction in space, but
in a different reference frame"""
if newframe == None or newframe=='world':
return self.toWorld()
newlocal = so3.apply(so3.inv(newframe.worldCoordinates()[0]),self.worldCoordinates())
return Direction(newlocal,newframe)
def worldOffset(self, dir):
"""Offsets this direction by a vector in world coordinates"""
if self._frame == None:
self._localCoordinates = vectorops.add(self._localCoordinates, dir)
else:
self._localCoordinates = vectorops.add(
so3.apply(so3.inv(self._frame.worldCoordinates()[0]), self._localCoordinates), dir
)
def matrix(self):
o = orientation_matrix(*self.ori)
Ry = so3.rotation([0.,1.,0.],self.rot[0])
Rx = so3.rotation([1.,0.,0.],self.rot[1])
Rz = so3.rotation([0.,0.,1.],self.rot[2])
R = so3.mul(Ry,so3.mul(Rx,Rz))
R = so3.mul(o,R);
raise (R,so3.apply(o,self.pos))
def to(self, newframe):
"""Returns a Direction representing the same direction in space, but
in a different reference frame"""
if newframe == None or newframe == 'world':
return self.toWorld()
newlocal = so3.apply(so3.inv(newframe.worldCoordinates()[0]),
self.worldCoordinates())
return Direction(newlocal, newframe)
def worldOffset(self, dir):
"""Offsets this direction by a vector in world coordinates"""
if self._frame == None:
self._localCoordinates = vectorops.add(self._localCoordinates, dir)
else:
self._localCoordinates = vectorops.add(
so3.apply(so3.inv(self._frame.worldCoordinates()[0]),
self._localCoordinates), dir)
def matrix(self):
"""Returns the camera transform."""
o = orientation_matrix(*self.ori)
Ry = so3.rotation([0., 1., 0.], self.rot[0])
Rx = so3.rotation([1., 0., 0.], self.rot[1])
Rz = so3.rotation([0., 0., 1.], self.rot[2])
R = so3.mul(Ry, so3.mul(Rx, Rz))
R = so3.mul(o, R)
raise (R, so3.apply(o, self.pos))
def matrix(self):
"""Returns the camera transform."""
o = orientation_matrix(*self.ori)
Ry = so3.rotation([0.,1.,0.],self.rot[0])
Rx = so3.rotation([1.,0.,0.],self.rot[1])
Rz = so3.rotation([0.,0.,1.],self.rot[2])
R = so3.mul(Ry,so3.mul(Rx,Rz))
R = so3.mul(o,R);
raise (R,so3.apply(o,self.pos))
def readHold(text):
"""Loads a Hold from a string"""
lines = parseLines(text)
if lines[0] != 'begin hold':
raise ValueError('Invalid hold begin text')
if lines[-1] != 'end':
raise ValueError('Invalid hold end text')
h = hold.Hold()
posLocal = None
posWorld = None
localPos0 = None
rotAxis = None
rotWorld = None
iktype = 0
for l in lines[1:-1]:
items = l.split()
if items[1] != '=':
raise ValueError("Invalid line format")
if items[0] == 'link':
h.link = int(items[2])
elif items[0] == 'contacts':
ind = 2
while ind < len(items):
h.contacts.append(readContactPoint(' '.join(items[ind:ind+7])))
ind += 7
elif items[0] == "position":
posLocal = [float(v) for v in items[2:5]]
posWorld = [float(v) for v in items[5:8]]
elif items[0] == "axis":
rotAxis = [float(v) for v in items[2:5]]
rotWorld = [float(v) for v in items[5:8]]
elif items[0] == "rotation":
rotWorld = [float(v) for v in items[2:5]]
elif items[0] == "ikparams":
localPos0 = [float(v) for v in items[2:5]]
rotWorld = [float(v) for v in items[5:8]]
iktype = 1
else:
raise ValueError("Invalid item "+items[0])
if iktype == 0:
h.ikConstraint = IKObjective()
if posLocal==None:
raise ValueError("Hold must have some point constraint")
if rotWorld==None:
h.ikConstraint.setFixedPoint(h.link,posLocal,posWorld)
elif rotAxis==None:
R = momentToMatrix(rotWorld)
t = vectorops.sub(posWorld,so3.apply(R,posLocal))
h.ikConstraint.setFixedTransform(h.link,R,t)
else:
raise NotImplementedError("Can't do axis rotation yet")
h.ikConstraint.setAxisRotation(h.link,posLocal,posWorld,localAxis,worldAxis)
else:
raise NotImplementedError("other ik specifications not done yet")
h.setupIKConstraint(localPos0,rotWorld)
return h
def matrix(self):
o = orientation_matrix(*self.ori)
Ry = so3.rotation([0.,1.,0.],self.rot[0])
Rx = so3.rotation([1.,0.,0.],self.rot[1])
Rz = so3.rotation([0.,0.,1.],self.rot[2])
R = so3.mul(Ry,so3.mul(Rx,Rz))
R = so3.mul(o,R);
t = so3.apply(R,self.tgt)
return (R,vectorops.madd(t,[0.,0.,1.],-self.dist))
def click_ray(self,x,y):
"""Returns a pair of 3-tuples indicating the ray source and direction
in world coordinates for a screen-coordinate point (x,y)"""
R,t = se3.inv(self.camera.matrix())
#from x and y compute ray direction
u = float(x-self.width/2)
v = float(self.height-y-self.height/2)
scale = math.tan(self.fov*math.pi/180.0)/self.width
d = (u*scale,v*scale,-1.0)
return (t,so3.apply(R,d))
def matrix(self):
"""Returns the camera transform."""
o = orientation_matrix(*self.ori)
Ry = so3.rotation([0., 1., 0.], self.rot[0])
Rx = so3.rotation([1., 0., 0.], self.rot[1])
Rz = so3.rotation([0., 0., 1.], self.rot[2])
R = so3.mul(Ry, so3.mul(Rx, Rz))
R = so3.mul(o, R)
t = so3.apply(R, self.tgt)
return (R, vectorops.madd(t, [0., 0., 1.], -self.dist))
def motionfunc(self,x,y,dx,dy):
if self.dragging:
if self.modifiers & GLUT_ACTIVE_CTRL:
R,t = self.camera.matrix()
delta = so3.apply(so3.inv(R),[float(dx)*self.camera.dist/self.width,-float(dy)*self.camera.dist/self.width,0])
self.camera.tgt = vectorops.add(self.camera.tgt,delta)
elif self.modifiers & GLUT_ACTIVE_SHIFT:
self.camera.dist *= math.exp(dy*0.01)
else:
self.camera.rot[2] += float(dx)*0.01
self.camera.rot[1] += float(dy)*0.01
self.refresh()
def motionfunc(self,x,y,dx,dy):
if self.dragging:
if self.modifiers & GLUT_ACTIVE_CTRL:
R,t = self.camera.matrix()
delta = so3.apply(so3.inv(R),[float(dx)*self.camera.dist/self.width,-float(dy)*self.camera.dist/self.width,0])
self.camera.tgt = vectorops.add(self.camera.tgt,delta)
elif self.modifiers & GLUT_ACTIVE_SHIFT:
self.camera.dist *= math.exp(dy*0.01)
else:
self.camera.rot[2] += float(dx)*0.01
self.camera.rot[1] += float(dy)*0.01
self.refresh()
def click_ray(self,x,y):
"""Returns a pair of 3-tuples indicating the ray source and direction
in world coordinates for a screen-coordinate point (x,y)"""
R,t = se3.inv(self.camera.matrix())
#from x and y compute ray direction
u = float(x-self.width/2)
v = float(self.height-y-self.height/2)
scale = math.tan(self.fov*math.pi/180.0)/self.height
#HACK: I don't know why this seems to work!
scale *= 0.925
d = (u*scale,v*scale,-1.0)
d = vectorops.div(d,vectorops.norm(d))
return (t,so3.apply(R,d))
def click_ray(self,x,y):
"""Returns a pair of 3-tuples indicating the ray source and direction
in world coordinates for a screen-coordinate point (x,y)"""
R,t = se3.inv(self.camera.matrix())
#from x and y compute ray direction
u = float(x-self.width/2)
v = float(self.height-y-self.height/2)
aspect = float(self.width)/float(self.height)
rfov = self.fov*math.pi/180.0
scale = 2.0*math.tan(rfov*0.5/aspect)*aspect
d = (u*scale,v*scale,-1.0)
d = vectorops.div(d,vectorops.norm(d))
return (t,so3.apply(R,d))
def click_ray(self,x,y):
"""Returns a pair of 3-tuples indicating the ray source and direction
in world coordinates for a screen-coordinate point (x,y)"""
R,t = se3.inv(self.camera.matrix())
#from x and y compute ray direction
u = float(x-self.width/2)
v = float(self.height-y-self.height/2)
aspect = float(self.width)/float(self.height)
rfov = self.fov*math.pi/180.0
scale = 2.0*math.tan(rfov*0.5/aspect)*aspect
d = (u*scale,v*scale,-1.0)
d = vectorops.div(d,vectorops.norm(d))
return (t,so3.apply(R,d))
def motionfunc(self,x,y):
dx = x - self.lastx
dy = y - self.lasty
if self.modifiers & GLUT_ACTIVE_CTRL:
R,t = self.camera.matrix()
delta = so3.apply(so3.inv(R),[float(dx)*self.camera.dist/self.width,-float(dy)*self.camera.dist/self.width,0])
self.camera.tgt = vectorops.add(self.camera.tgt,delta)
elif self.modifiers & GLUT_ACTIVE_SHIFT:
self.camera.dist *= math.exp(dy*0.01)
else:
self.camera.rot[2] += float(dx)*0.01
self.camera.rot[1] += float(dy)*0.01
self.lastx = x
self.lasty = y
glutPostRedisplay()
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)
def directionFromWorld(self,worldCoordinates=[0,0,0],frame='world'):
"""Alias for to(direction(worldCoordinates,'root'),frame)"""
f = self.frame(frame)
local = so3.apply(so3.inv(f._worldCoordinates[0]),worldCoordinates)
return Direction(local,f)
def directionFromWorld(self, worldCoordinates=[0, 0, 0], frame='world'):
"""Alias for to(direction(worldCoordinates,'root'),frame)"""
f = self.frame(frame)
local = so3.apply(so3.inv(f._worldCoordinates[0]), worldCoordinates)
return Direction(local, f)
def apply_rotation(T,point):
"""Applies only the rotation part of T"""
R = T[0]
return so3.apply(R,point)
def apply_rotation(T, point):
"""Applies only the rotation part of T"""
R = T[0]
return so3.apply(R, point)
def worldCoordinates(self):
if self._frame == None:
return self._localCoordinates[:]
return so3.apply(self._frame.worldCoordinates()[0],
self._localCoordinates)
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 fromJson(jsonobj,type='auto'):
"""Converts from a JSON object (e.g., from json.loads()) to a Klamp't
object of the appropriate type. If 'type' is not provided, this attempts
to infer the object type automatically."""
if type == 'auto':
if isinstance(jsonobj,(list,tuple)):
return jsonobj
elif isinstance(jsonobj,(bool,int,float,str,unicode)):
return jsonobj
elif isinstance(jsonobj,dict):
if 'type' in jsonobj:
type = jsonobj["type"]
elif 'times' in jsonobj and 'milestones' in jsonobj:
type = 'Trajectory'
elif 'x' in jsonobj and 'n' in jsonobj and 'kFriction' in jsonobj:
type = 'ContactPoint'
else:
raise RuntimeError("Unknown JSON object of type "+jsonobj.__class__.__name)
if type in ['Config','Configs','Vector','Matrix','Matrix3','RotationMatrix','Value','IntArray','StringArray']:
return jsonobj
elif type == 'RigidTransform':
return jsonobj
elif type == 'ContactPoint':
return ContactPoint(jsonobj['x'],jsonobj['n'],jsonobj['kFriction'])
elif type == 'Trajectory':
return Trajectory(jsonobj['times'],jsonobj['milestones'])
elif type == 'IKObjective':
link = jsonobj['link']
destlink = jsonobj['destLink'] if 'destLink' in jsonobj else -1
posConstraint = 'free'
rotConstraint = 'free'
localPosition = endPosition = None
direction = None
endRotation = None
localAxis = None
if 'posConstraint' in jsonobj:
posConstraint = jsonobj['posConstraint']
if 'rotConstraint' in jsonobj:
rotConstraint = jsonobj['rotConstraint']
if posConstraint == 'planar' or posConstraint == 'linear':
direction = jsonobj['direction']
if posConstraint != 'free':
localPosition = jsonobj['localPosition']
endPosition = jsonobj['endPosition']
if rotConstraint != 'free':
endRotation = jsonobj['endRotation']
if rotConstraint == 'axis' or rotConstraint == 'twoaxis':
localAxis = jsonobj['localAxis']
if posConstraint == 'free' and rotConstraint == 'free':
#empty
return IKObjective()
elif posConstraint != 'fixed':
raise NotImplementedError("Can't do non-fixed position constraints yet in Python API")
if rotConstraint == 'twoaxis':
raise NotImplementedError("twoaxis constraints are not implemented in Klampt")
if rotConstraint == 'free':
obj = IKObjective()
if destlink >= 0:
obj.setRelativePoint(link,destlink,localPosition,endPosition)
else:
obj.setFixedPoint(link,localPosition,endPosition)
return obj
elif rotConstraint == 'axis':
obj = IKObjective()
h = 0.1
lpts = [localPosition,vectorops.madd(localPosition,localAxis,h)]
wpts = [endPosition,vectorops.madd(endPosition,endRotation,h)]
if destlink >= 0:
obj.setRelativePoints(link,destlink,lpts,wpts)
else:
obj.setFixedPoints(link,lpts,wpts)
return obj
elif rotConstraint == 'fixed':
obj = IKObjective()
R = so3.from_moment(endRotation)
t = vectorops.sub(endPosition,so3.apply(R,localPosition))
obj.setFixedTransform(link,R,t)
return obj
else:
raise RuntimeError("Invalid IK rotation constraint "+rotConstraint)
elif type in readers:
return read(type,jsonobj["data"])
else:
raise RuntimeError("Unknown or unsupported type "+type)
def worldCoordinates(self):
if self._frame ==None:
return self._localCoordinates[:]
return so3.apply(self._frame.worldCoordinates()[0],self._localCoordinates)
def __init__(self):
Context.__init__(self)
self.Rtype = Type('V', 9)
self.ttype = Type('V', 3)
self.type = Type('L', 2, [self.Rtype, self.ttype])
T = Variable("T", self.type)
R = Variable("R", self.Rtype)
t = Variable("t", self.ttype)
self.make = self.declare(array(R, t), "make", ['R', 't'])
self.identity = self.declare(se3.identity, "identity")
self.homogeneous = self.declare(se3.homogeneous, "homogeneous")
self.homogeneous.addSimplifier(['se3.identity'], lambda T: eye(4))
Rinv = so3.inv(T[0])
self.inv = self.declare(array(Rinv, neg(so3.apply(Rinv, T[1]))), "inv",
['T'])
self.inv.autoSetJacobians()
self.inv.properties['inverse'] = weakref.proxy(self.inv)
self.inv.addSimplifier(['se3.identity'], lambda T: T)
self.mul = self.declare(se3.mul, "mul")
self.mul.setDeriv(0,
lambda T1, T2, dT1: self.mul(dT1, T2),
asExpr=True)
self.mul.setDeriv(1,
lambda T1, T2, dT2: self.mul(T1, dT2),
asExpr=True)
self.mul.addSimplifier(['se3.identity', None], (lambda T1, T2: T2),
pre=True)
self.mul.addSimplifier([None, 'se3.identity'], (lambda T1, T2: T1),
pre=True)
self.mul.properties['associative'] = True
pt = Variable('pt', self.ttype)
self.apply = self.declare(
so3.apply(T[0], pt) + T[1], "apply", ['T', 'pt'])
#self.apply.setDeriv(0,lambda T,pt,dT:array(so3.apply(dT[0],pt),dT[1]))
#self.apply.setDeriv(1,lambda T,pt,dx:so3.apply(T[0],dx))
self.apply.addSimplifier([None, 'zero'], lambda T, pt: T[1])
self.apply.addSimplifier(['se3.identity', None], lambda T, pt: pt)
self.apply.autoSetJacobians()
self.rotation = self.declare(T[0], "rotation", ['T'])
self.translation = self.declare(T[1], "translation", ['T'])
self.make.returnType = self.type
self.homogeneous.returnType = Type('M', (4, 4))
self.homogeneous.argTypes = [self.type]
self.homogeneous.setDeriv(
0,
lambda T, dT: array([[dT[0][0], dT[0][3], dT[0][6], dT[1][0]],
[dT[0][1], dT[0][4], dT[0][7], dT[1][1]],
[dT[0][2], dT[0][5], dT[0][8], dT[1][2]],
[0., 0., 0., 0.]]),
asExpr=True)
M = Variable("M", Type('M', (4, 4)))
self.from_homogeneous = self.declare(
array(
array(M[0, 0], M[1, 0], M[2, 0], M[0, 1], M[1, 1], M[2, 1],
M[0, 2], M[1, 2], M[2, 2]),
array(M[0, 3], M[1, 3], M[2, 3])), 'from_homogeneous', ['M'])
self.from_homogeneous.autoSetJacobians()
self.matrix = self.declare(self.homogeneous(T), "matrix", ['T'])
self.make.autoSetJacobians()
self.matrix.autoSetJacobians()
self.rotation.autoSetJacobians()
self.translation.autoSetJacobians()
self.identity.returnType = self.type
self.inv.returnType = self.type
self.inv.argTypes = [self.type]
self.mul.returnType = self.type
self.mul.argTypes = [self.type, self.type]
self.apply.returnType = self.ttype
self.apply.argTypes = [self.type, self.ttype]
self.rotation.returnType = self.Rtype
self.translation.returnType = self.ttype
def __init__(self):
Context.__init__(self)
self.type = Type('V', 9)
Rvar = Variable("R", self.type)
Rsymb = VariableExpression(Rvar)
R1 = Variable("R1", self.type)
R2 = Variable("R2", self.type)
V3type = Type('V', 3)
q = Variable('q', Type('V', 4))
pointvar = Variable("point", V3type)
pointsymb = VariableExpression(pointvar)
self.identity = self.declare(expr(so3.identity()), "identity", [])
self.identity.description = "The identity rotation"
self.matrix = self.declare(expr(so3.matrix(Rsymb)), "matrix", ["R"])
self.matrix.addSimplifier(['so3.identity'], (lambda R: eye(3)),
pre=True)
self.matrix.description = "Converts to a 3x3 matrix"
M = Variable("M", Type('M', (3, 3)))
self.from_matrix = self.declare(flatten(transpose(M)), "from_matrix",
['M'])
self.from_matrix.description = "Converts from a 3x3 matrix"
self.from_matrix.autoSetJacobians()
self.inv = self.declare(expr(so3.inv(Rsymb)), "inv", ["R"])
self.inv.description = "Inverts a rotation"
self.inv.autoSetJacobians()
self.inv.properties['inverse'] = weakref.proxy(self.inv)
self.inv.addSimplifier(['so3.identity'], lambda R: R)
self.mul = self.declare(so3.mul, "mul")
self.mul.description = "Inverts a rotation"
self.mul.setDeriv(0,
lambda R1, R2, dR1: self.mul(dR1, R2),
asExpr=True)
self.mul.setDeriv(1,
lambda R1, R2, dR2: self.mul(R1, dR2),
asExpr=True)
self.mul.addSimplifier(['so3.identity', None], (lambda R1, R2: R2),
pre=True)
self.mul.addSimplifier([None, 'so3.identity'], (lambda R1, R2: R1),
pre=True)
self.mul.properties['associative'] = True
self.apply = self.declare(expr(so3.apply(Rsymb, pointsymb)), "apply",
["R", "point"])
self.apply.addSimplifier(['so3.identity', None],
(lambda R, point: point),
pre=True)
self.apply.addSimplifier([None, 'zero'], (lambda R, point: point),
pre=True)
self.apply.autoSetJacobians()
self.rotation = self.declare(so3.rotation, "rotation")
self.from_rpy = self.declare(so3.from_rpy, "from_rpy")
self.rpy = self.declare(so3.rpy, "rpy")
self.from_quaternion = self.declare(
expr(so3.from_quaternion([q[0], q[1], q[2], q[3]])),
"from_quaternion", ["q"])
self.quaternion = self.declare(so3.quaternion, "quaternion")
self.from_rotation_vector = self.declare(so3.from_rotation_vector,
"from_rotation_vector")
self.rotation_vector = self.declare(so3.rotation_vector,
"rotation_vector")
self.axis = self.declare(unit(self.rotation_vector(Rvar)), "rotation",
["R"])
self.angle = self.declare(so3.angle, "angle")
self.error = self.declare(so3.error, "error")
self.distance = self.declare(self.angle(self.mul(self.inv(R1), R2)),
"distance", ['R1', 'R2'])
self.distance.properties['nonnegative'] = True
Rm = self.matrix(Rsymb)
self.eq_constraint = self.declare(dot(Rm.T, Rm), 'eq_constraint',
['R'])
self.quaternion_constraint = self.declare(
norm2(q) - 1, 'quaternion_constraint', ['q'])
self.identity.returnType = self.type
self.inv.returnType = self.type
self.inv.argTypes = [self.type]
self.mul.returnType = self.type
self.mul.argTypes = [self.type, self.type]
self.apply.returnType = V3type
self.apply.argTypes = [self.type, V3type]
self.rotation.returnType = self.type
self.rotation.argTypes = [V3type, Numeric]
self.rotation.setDeriv(1, lambda axis, angle: so3.cross_product(axis))
self.axis.returnType = V3type
self.axis.argTypes = [self.type]
self.angle.returnType = V3type
self.angle.argTypes = [self.type]
def angle_deriv(R, dR):
cosangle = (R[0] + R[4] + R[8] - 1) * 0.5
angle = arccos(cosangle)
#dangle / dR[0] = -1.0/sqrt(1-cosangle**2) * dcosangle/dR[0]
dacos = -1.0 / sqrt(1 - cosangle**2)
return expr([
0.5 * dacos * dR[0], 0, 0, 0, 0.5 * dacos * dR[4], 0, 0, 0,
0.5 * dacos * dR[8]
])
self.angle.setDeriv(0, angle_deriv, asExpr=True)
self.error.returnType = V3type
self.error.argTypes = [self.type, self.type]
self.distance.returnType = Numeric
self.distance.argTypes = [self.type, self.type]
self.distance.autoSetJacobians()
self.from_matrix.returnType = self.type
self.from_matrix.argTypes = [M.type]
self.from_rpy.returnType = self.type
self.from_rpy.argTypes = [V3type]
self.from_quaternion.returnType = self.type
self.from_quaternion.argTypes = [Type('V', 4)]
self.from_rotation_vector.returnType = self.type
self.from_rotation_vector.argTypes = [V3type]
self.matrix.returnType = self.from_matrix.argTypes[0]
self.matrix.argTypes = [self.from_matrix.returnType]
self.rpy.returnType = self.from_rpy.argTypes[0]
self.rpy.argTypes = [self.from_rpy.returnType]
self.quaternion.returnType = self.from_quaternion.argTypes[0]
self.quaternion.argTypes = [self.from_quaternion.returnType]
self.rotation_vector.returnType = self.from_rotation_vector.argTypes[0]
self.rotation_vector.argTypes = [self.from_rotation_vector.returnType]
def __init__(self):
Context.__init__(self)
self.type = Type('V',9)
Rvar = Variable("R",self.type)
Rsymb = VariableExpression(Rvar)
R1 = Variable("R1",self.type)
R2 = Variable("R2",self.type)
V3type = Type('V',3)
q = Variable('q',Type('V',4))
pointvar = Variable("point",V3type)
pointsymb = VariableExpression(pointvar)
self.identity = self.declare(expr(so3.identity()),"identity",[])
self.identity.description = "The identity rotation"
self.matrix = self.declare(expr(so3.matrix(Rsymb)),"matrix",["R"])
self.matrix.addSimplifier(['so3.identity'],(lambda R:eye(3)),pre=True)
self.matrix.description = "Converts to a 3x3 matrix"
M = Variable("M",Type('M',(3,3)))
self.from_matrix = self.declare(flatten(transpose(M)),"from_matrix",['M'])
self.from_matrix.description = "Converts from a 3x3 matrix"
self.from_matrix.autoSetJacobians()
self.inv = self.declare(expr(so3.inv(Rsymb)),"inv",["R"])
self.inv.description = "Inverts a rotation"
self.inv.autoSetJacobians()
self.inv.properties['inverse'] = weakref.proxy(self.inv)
self.inv.addSimplifier(['so3.identity'],lambda R:R)
self.mul = self.declare(so3.mul,"mul")
self.mul.description = "Inverts a rotation"
self.mul.setDeriv(0,lambda R1,R2,dR1:self.mul(dR1,R2),asExpr=True)
self.mul.setDeriv(1,lambda R1,R2,dR2:self.mul(R1,dR2),asExpr=True)
self.mul.addSimplifier(['so3.identity',None],(lambda R1,R2:R2),pre=True)
self.mul.addSimplifier([None,'so3.identity'],(lambda R1,R2:R1),pre=True)
self.mul.properties['associative'] = True
self.apply = self.declare(expr(so3.apply(Rsymb,pointsymb)),"apply",["R","point"])
self.apply.addSimplifier(['so3.identity',None],(lambda R,point:point),pre=True)
self.apply.addSimplifier([None,'zero'],(lambda R,point:point),pre=True)
self.apply.autoSetJacobians()
self.rotation = self.declare(so3.rotation,"rotation")
self.from_rpy = self.declare(so3.from_rpy,"from_rpy")
self.rpy = self.declare(so3.rpy,"rpy")
self.from_quaternion = self.declare(expr(so3.from_quaternion([q[0],q[1],q[2],q[3]])),"from_quaternion",["q"])
self.quaternion = self.declare(so3.quaternion,"quaternion")
self.from_rotation_vector = self.declare(so3.from_rotation_vector,"from_rotation_vector")
self.rotation_vector = self.declare(so3.rotation_vector,"rotation_vector")
self.axis = self.declare(unit(self.rotation_vector(Rvar)),"rotation",["R"])
self.angle = self.declare(so3.angle,"angle")
self.error = self.declare(so3.error,"error")
self.distance = self.declare(self.angle(self.mul(self.inv(R1),R2)),"distance",['R1','R2'])
self.distance.properties['nonnegative'] = True
Rm = self.matrix(Rsymb)
self.eq_constraint = self.declare(dot(Rm.T,Rm),'eq_constraint',['R'])
self.quaternion_constraint = self.declare(norm2(q)-1,'quaternion_constraint',['q'])
self.identity.returnType = self.type
self.inv.returnType = self.type
self.inv.argTypes = [self.type]
self.mul.returnType = self.type
self.mul.argTypes = [self.type,self.type]
self.apply.returnType = V3type
self.apply.argTypes = [self.type,V3type]
self.rotation.returnType = self.type
self.rotation.argTypes = [V3type,Numeric]
self.rotation.setDeriv(1,lambda axis,angle:so3.cross_product(axis))
self.axis.returnType = V3type
self.axis.argTypes = [self.type]
self.angle.returnType = V3type
self.angle.argTypes = [self.type]
def angle_deriv(R,dR):
cosangle = (R[0]+R[4]+R[8]-1)*0.5
angle = arccos(cosangle)
#dangle / dR[0] = -1.0/sqrt(1-cosangle**2) * dcosangle/dR[0]
dacos = -1.0/sqrt(1-cosangle**2)
return expr([0.5*dacos*dR[0],0,0,0,0.5*dacos*dR[4],0,0,0,0.5*dacos*dR[8]])
self.angle.setDeriv(0,angle_deriv,asExpr=True)
self.error.returnType = V3type
self.error.argTypes = [self.type,self.type]
self.distance.returnType = Numeric
self.distance.argTypes = [self.type,self.type]
self.distance.autoSetJacobians()
self.from_matrix.returnType = self.type
self.from_matrix.argTypes = [M.type]
self.from_rpy.returnType = self.type
self.from_rpy.argTypes = [V3type]
self.from_quaternion.returnType = self.type
self.from_quaternion.argTypes = [Type('V',4)]
self.from_rotation_vector.returnType = self.type
self.from_rotation_vector.argTypes = [V3type]
self.matrix.returnType = self.from_matrix.argTypes[0]
self.matrix.argTypes = [self.from_matrix.returnType]
self.rpy.returnType = self.from_rpy.argTypes[0]
self.rpy.argTypes = [self.from_rpy.returnType]
self.quaternion.returnType = self.from_quaternion.argTypes[0]
self.quaternion.argTypes = [self.from_quaternion.returnType]
self.rotation_vector.returnType = self.from_rotation_vector.argTypes[0]
self.rotation_vector.argTypes = [self.from_rotation_vector.returnType]
def readIKObjective(text):
"""Reads an IKObjective from text"""
items = text.split()
if len(items) < 4:
raise ValueError("Not enough items to unpack")
link = int(items[0])
destlink = int(items[1])
ind = 2
posType = None
posLocal = None
posWorld = None
posDirection = None
rotType = None
rotWorld = None
rotAxis = None
#read pos constraint
posType = items[ind]
ind += 1
if posType=='N':
#no constraint
pass
elif posType=='P' or posType=='L':
posLocal = items[ind:ind+3]
ind += 3
posWorld = items[ind:ind+3]
ind += 3
posDirection = items[ind:ind+3]
ind += 3
elif posType=='F':
posLocal = items[ind:ind+3]
ind += 3
posWorld = items[ind:ind+3]
ind += 3
else:
raise ValueError("Invalid pos type "+posType+", must be N,P,L or F")
rotType = items[ind]
ind += 1
if rotType=='N':
#no constraint
pass
elif rotType=='T' or rotType=='A':
rotAxis = items[ind:ind+3]
ind += 3
rotWorld = items[ind:ind+3]
ind += 3
elif rotType=='F':
rotWorld = items[ind:ind+3]
ind += 3
else:
raise ValueError("Invalid rot type "+rotType+", must be N,T,A or F")
if posType != 'F':
raise NotImplementedError("Python API can only do point and fixed IK goals")
obj = IKObjective()
if rotType == 'N':
#point constraint
obj.setRelativePoint(link,destlink,[float(v) for v in posLocal],[float(v) for v in posWorld])
elif rotType == 'F':
#fixed constraint
R = so3.from_moment([float(v) for v in rotWorld])
t = vectorops.sub([float(v) for v in posWorld],so3.apply(R,[float(v) for v in posLocal]))
obj.setRelativeTransform(link,destlink,R,t)
elif rotType == 'A':
#TODO: rotational axis constraints actually can be set in the python API
raise NotImplementedError("Axis rotations not yet implemented")
else:
raise NotImplementedError("Two-axis rotational constraints not supported")
return obj
