def fixed_objective(link,ref=None,local=None,world=None):
"""Convenience function for fixing the given link at the current position
in space. If local and world are not provided, the entire link is
constrained. If only local is provided, these points are fixed
to their current positions in space. If only world is provided,
the points on the link with the given world position are constrained in
place."""
refcoords = ref.getTransform() if ref is not None else se3.identity()
Tw = link.getTransform()
Trel = se3.mul(se3.inv(refcoords),Tw)
if local is not None and not hasattr(local[0],'__iter__'):
#just a single point, make it a list of points
local = [local]
if world is not None and not hasattr(world[0],'__iter__'):
#just a single point, make it a list of points
world = [world]
if local is None and world is None:
#fixed rotation/position objective
return objective(link,ref,R=Trel[0],t=Trel[1])
elif local is None:
#fixed point, given by world coordinates
Trelinv = se3.inv(Trel)
local = [se3.apply(trelinv,p) for p in world]
return objective(link,ref,local=local,world=world)
elif world is None:
#fixed point, given by local coordinates
world = [se3.apply(Trel,p) for p in local]
return objective(link,ref,local=local,world=world)
else:
raise ValueError("ik.fixed_objective does not accept both local and world keyword arguments")
def fixed_objective(link, ref=None, local=None, world=None):
"""Convenience function for fixing the given link at the current position
in space. If local and world are not provided, the entire link is
constrained. If only local is provided, these points are fixed
to their current positions in space. If only world is provided,
the points on the link with the given world position are constrained in
place."""
refcoords = ref.getTransform() if ref is not None else se3.identity()
Tw = link.getTransform()
Trel = se3.mul(se3.inv(refcoords), Tw)
if local is not None and not hasattr(local[0], '__iter__'):
#just a single point, make it a list of points
local = [local]
if world is not None and not hasattr(world[0], '__iter__'):
#just a single point, make it a list of points
world = [world]
if local is None and world is None:
#fixed rotation/position objective
return objective(link, ref, R=Trel[0], t=Trel[1])
elif local is None:
#fixed point, given by world coordinates
Trelinv = se3.inv(Trel)
local = [se3.apply(trelinv, p) for p in world]
return objective(link, ref, local=local, world=world)
elif world is None:
#fixed point, given by local coordinates
world = [se3.apply(Trel, p) for p in local]
return objective(link, ref, local=local, world=world)
else:
raise ValueError(
"ik.fixed_objective does not accept both local and world keyword arguments"
)
def translationCoordinates(self):
"""Returns the coordinates of the origin of this frame in R^3, relative
to its destination"""
if self._destination == None:
return self._source.worldOrigin()
return se3.apply(se3.inv(self._destination.worldCoordinates()),
self._source.worldOrigin())
def to(self,newframe):
"""Returns a Point representing the same point in space, but
in a different reference frame"""
if newframe == None or newframe=='world':
return self.toWorld()
newlocal = se3.apply(se3.inv(newframe.worldCoordinates()),self.worldCoordinates())
return Point(newlocal,newframe)
def getPositionTrajectory(self,localPt=None): """Returns a Trajectory describing the movement of the given local point localPt (or the origin, if none is provided).""" if localPt is None: return Trajectory(times,[m[9:] for m in self.milestones]) else: return Trajectory(times,[se3.apply((m[:9],m[9:]),localPt) for m in self.milestones])
def __init__(self,name,value,description,world,frame=None):
_VisualEditorBase.__init__(self,name,value,description,world)
self.frame = se3.identity() if frame==None else frame
self.pointposer = PointPoser()
self.pointposer.set(se3.apply(self.frame,value))
self.pointposer.setFrame(self.frame[0])
self.addWidget(self.pointposer)
def __init__(self, name, value, description, world, frame=None):
_VisualEditorBase.__init__(self, name, value, description, world)
self.frame = se3.identity() if frame == None else frame
self.pointposer = PointPoser()
self.pointposer.set(se3.apply(self.frame, value))
self.pointposer.setFrame(self.frame[0])
self.addWidget(self.pointposer)
def to(self, newframe):
"""Returns a Point representing the same point in space, but
in a different reference frame"""
if newframe == None or newframe == 'world':
return self.toWorld()
newlocal = se3.apply(se3.inv(newframe.worldCoordinates()),
self.worldCoordinates())
return Point(newlocal, newframe)
def main():
"""
Main loop. Run ICP on the given model and scene file, display the results.
"""
model_full = get_reconstructed_model(model_file,True)
scene_full = get_raw_depth(depth_file)
#run the ICP
R,t = icp(model_full['positions'],scene_full)
#apply the ICP results to the model
transformed_points = [se3.apply((R,t),p) for p in model_full['positions']]
model_full['positions'] = transformed_points
#visualize the results
opengl_plot = OpenGLPlot(model_full, scene_full)
opengl_plot.initialize_main_loop()
def pointFromWorld(self, worldCoordinates=[0, 0, 0], frame='root'):
"""Alias for to(point(worldCoordinates,'root'),frame)"""
f = self.frame(frame)
local = se3.apply(se3.inv(f._worldCoordinates), worldCoordinates)
return Point(local, f)
def worldCoordinates(self):
"""Returns the coordinates of this point in the world Frame"""
if self._frame == None:
return self._localCoordinates[:]
return se3.apply(self._frame.worldCoordinates(),
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 pointFromWorld(self,worldCoordinates=[0,0,0],frame='root'):
"""Alias for to(point(worldCoordinates,'root'),frame)"""
f = self.frame(frame)
local = se3.apply(se3.inv(f._worldCoordinates),worldCoordinates)
return Point(local,f)
def worldCoordinates(self):
"""Returns the coordinates of this point in the world Frame"""
if self._frame ==None:
return self._localCoordinates[:]
return se3.apply(self._frame.worldCoordinates(),self._localCoordinates)
def mousefunc(self,button,state,x,y):
if _VisualEditorBase.mousefunc(self,button,state,x,y):
self.value = se3.apply(se3.inv(self.frame),self.pointposer.get())
def mousefunc(self, button, state, x, y):
if _VisualEditorBase.mousefunc(self, button, state, x, y):
self.value = se3.apply(se3.inv(self.frame), self.pointposer.get())
def translationCoordinates(self):
"""Returns the coordinates of the origin of this frame in R^3, relative
to its destination"""
if self._destination==None:
return self._source.worldOrigin()
return se3.apply(se3.inv(self._destination.worldCoordinates()),self._source.worldOrigin())