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 make_outline(self):
left = []
right = []
for i in xrange(len(self.path)):
s1l = None
s1r = None
s2l = None
s2r = None
if i > 0:
d = vectorops.unit(
vectorops.sub(self.path[i], self.path[i - 1]))
ofs = (self.gutter * d[1], -self.gutter * d[0])
s1l = (d, ofs)
s1r = (d, (-ofs[0], -ofs[1]))
if i + 1 < len(self.path):
d = vectorops.unit(
vectorops.sub(self.path[i + 1], self.path[i]))
ofs = (self.gutter * d[1], -self.gutter * d[0])
s2l = (d, ofs)
s2r = (d, (-ofs[0], -ofs[1]))
if i == 0:
left.append(tuple(vectorops.add(self.path[i], s2l[1])))
right.append(tuple(vectorops.add(self.path[i], s2r[1])))
elif i + 1 == len(self.path):
left.append(tuple(vectorops.add(self.path[i], s1l[1])))
right.append(tuple(vectorops.add(self.path[i], s1r[1])))
else:
#figure out intersections
if vectorops.dot(s1r[0], s2r[1]) < 0:
#inner junction, find line intersection
right.append(
tuple(
vectorops.add(
self.path[i],
ray_ray_intersection(s1r[1], s1r[0], s2r[1],
s2r[0]))))
else:
#outer junction
right.append(tuple(vectorops.add(self.path[i], s1r[1])))
right.append(tuple(vectorops.add(self.path[i], s2r[1])))
if vectorops.dot(s1l[0], s2l[1]) < 0:
#inner junction, find line intersection
left.append(
tuple(
vectorops.add(
self.path[i],
ray_ray_intersection(s1l[1], s1l[0], s2l[1],
s2l[0]))))
else:
#outer junction
left.append(tuple(vectorops.add(self.path[i], s1l[1])))
left.append(tuple(vectorops.add(self.path[i], s2l[1])))
self.outline = left + list(reversed(right))
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 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 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 error(T1,T2):
"""Returns a 6D "difference vector" that describes how far T1 is from T2.
More precisely, this is the Lie derivative (w,v)."""
(R1,t1)=T1
(R2,t2)=T2
#concatenate lists
return so3.error(R1,R2) + vectorops.sub(t1,t2)
def error(T1, T2):
"""Returns a 6D "difference vector" that describes how far T1 is from T2.
More precisely, this is the Lie derivative (w,v)."""
(R1, t1) = T1
(R2, t2) = T2
#concatenate lists
return so3.error(R1, R2) + vectorops.sub(t1, t2)
def is_rotation(R,tol=1e-5):
"""Returns true if R is a rotation matrix, i.e. is orthogonal to the given tolerance and has + determinant"""
RRt = mul(R,inv(R))
err = vectorops.sub(RRt,identity())
if any(abs(v) > tol for v in err):
return False
if det(R) < 0:
return False
return True
def eval_path(self, param):
index = self.findSegment(param)
if index < 0: return self.path[0]
if index + 1 >= len(self.path_times): return self.path[-1]
u = (param - self.path_times[index]) / (self.path_times[index + 1] -
self.path_times[index])
return vectorops.madd(
self.path[index],
vectorops.sub(self.path[index + 1], self.path[index]), u)
def is_rotation(R, tol=1e-5):
"""Returns true if R is a rotation matrix, i.e. is orthogonal to the given tolerance and has + determinant"""
RRt = mul(R, inv(R))
err = vectorops.sub(RRt, identity())
if any(abs(v) > tol for v in err):
return False
if det(R) < 0:
return False
return True
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 eval_path(self, param):
index = int(math.floor(param))
u = param - index
if index < 0:
return self.path[0]
if index + 1 >= len(self.path):
return self.path[-1]
return vectorops.madd(
self.path[index],
vectorops.sub(self.path[index + 1], self.path[index]), u)
def ray_ray_intersection(s1, d1, s2, d2, eps=1e-6):
#solve the coordinates x = s1+t*d1 = s2-u*d2 for some t, u
#[d1,d2]*[t,u]^T = [s2-s1]
det = d1[0] * d2[1] - d1[1] * d2[0]
if abs(det) < eps: return None
#inv mat is 1/det [d2[1] -d2[0]]
# [-d1[1] d1[0]]
b = vectorops.sub(s2, s1)
t = (d2[1] * b[0] - d2[0] * b[1]) / det
u = (-d1[1] * b[0] + d1[0] * b[1]) / det
return vectorops.madd(s1, d1, t)
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 click_world(self, x, y):
"""Helper: returns a list of world objects sorted in order of
increasing distance."""
#get the viewport ray
(s, d) = self.click_ray(x, y)
#run the collision tests
collided = []
for g in self.collider.geomList:
(hit, pt) = g[1].rayCast(s, d)
if hit:
dist = vectorops.dot(vectorops.sub(pt, s), d)
collided.append((dist, g[0]))
if len(collided) == 0:
return
id = collided[0][1].getID()
if id in self.value:
self.value.remove(id)
else:
self.value.append(id)
self.lastClicked = id
self.refresh()
def click_robot(self,x,y):
"""Helper: returns a list of robot objects sorted in order of
increasing distance."""
#get the viewport ray
(s,d) = self.click_ray(x,y)
#run the collision tests
collided = []
for l in range(self.robot.numLinks()):
(hit,pt) = self.robot.link(l).geometry().rayCast(s,d)
if hit:
dist = vectorops.dot(vectorops.sub(pt,s),d)
collided.append((dist,l))
if len(collided) == 0:
return
id = collided[0][1]
if id in self.value:
self.value.remove(id)
else:
self.value.append(id)
self.lastClicked = id
self.refresh()
def click_world(self,x,y):
"""Helper: returns a list of world objects sorted in order of
increasing distance."""
#get the viewport ray
(s,d) = self.click_ray(x,y)
#run the collision tests
collided = []
for g in self.collider.geomList:
(hit,pt) = g[1].rayCast(s,d)
if hit:
dist = vectorops.dot(vectorops.sub(pt,s),d)
collided.append((dist,g[0]))
if len(collided) == 0:
return
id = collided[0][1].getID()
if id in self.value:
self.value.remove(id)
else:
self.value.append(id)
self.lastClicked = id
self.refresh()
def click_robot(self, x, y):
"""Helper: returns a list of robot objects sorted in order of
increasing distance."""
#get the viewport ray
(s, d) = self.click_ray(x, y)
#run the collision tests
collided = []
for l in range(self.robot.numLinks()):
(hit, pt) = self.robot.link(l).geometry().rayCast(s, d)
if hit:
dist = vectorops.dot(vectorops.sub(pt, s), d)
collided.append((dist, l))
if len(collided) == 0:
return
id = collided[0][1]
if id in self.value:
self.value.remove(id)
else:
self.value.append(id)
self.lastClicked = id
self.refresh()
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 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 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 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
def difference(self,a,b): """Can override this to implement non-Cartesian spaces""" return vectorops.sub(a,b)
def difference(self,a,b,u): """Subclasses can override this to implement non-Cartesian spaces. Returns the derivative along the geodesic from b to a.""" return vectorops.sub(a,b)
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 difference(self,a,b):
"""For Lie groups, returns a difference vector that, when integrated
would get to a from b. In Cartesian spaces it is a-b."""
return vectorops.sub(a,b)
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 difference(self, a, b):
"""For Lie groups, returns a difference vector that, when integrated
would get to a from b. In Cartesian spaces it is a-b."""
return vectorops.sub(a, b)
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
