def icp(object,scene):
"""Computes a rotation and translation of the object to the scene
using the ICP algorithm. Returns the transform (R,t) in Klamp't se3
format.
"""
#TODO: Sample both maps to make the closest point computations faster
objectTrans = object
R = [[1,0,0],[0,1,0],[0,0,1]]
t = [0,0,0]
for iters in range(20):
print "beginning iter",iters
transform_points(objectTrans,R,t)
Rdelta,tdelta = icp_step(scene, objectTrans)
r1 = transform_point([R[0][0],R[1][0],R[2][0]],Rdelta,[0,0,0])
r2 = transform_point([R[0][1],R[1][1],R[2][1]],Rdelta,[0,0,0])
r2 = transform_point([R[0][2],R[1][2],R[2][2]],Rdelta,[0,0,0])
R[0][0],R[1][0] = r1
R[0][1],R[1][1] = r2
t = transform_point(t,Rdelta,tdelta)
# Compute the minimum distance between the points
# Reject the outliers, for example, using a threshold
# Compute the R and t matrices. The procedure can be similar as the one for the
# 2D images.
return se3.identity()
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 addFrame(self,
name,
worldCoordinates=None,
parent=None,
relativeCoordinates=None):
"""Adds a new named Frame, possibly with a parent. 'parent' may either be a string
identifying another named Frame in this Group, or it can be a Frame object. (Warning:
unknown behavior may result from specifying a Frame not in this Group).
Either worldCoordinates or relativeCoordinates must be given. If worldCoordinates is given,
then the frame's initial relative transform is determined by the current coordinates of the
parent. If all parameters are left as default, the frame is placed directly at the origin
of the parent"""
if name in self.frames:
raise ValueError("Frame " + name + " already exists")
if parent == None:
parent = 'root'
if isinstance(parent, str):
parent = self.frames[parent]
if worldCoordinates == None and relativeCoordinates == None:
relativeCoordinates = se3.identity()
self.frames[name] = Frame(name,
worldCoordinates=worldCoordinates,
parent=parent,
relativeCoordinates=relativeCoordinates)
self.childLists[parent._name].append(self.frames[name])
return self.frames[name]
def __init__(self,
name,
worldCoordinates=se3.identity(),
parent=None,
relativeCoordinates=None):
self._name = name
self._parent = parent
self._worldCoordinates = worldCoordinates
self._data = None
if relativeCoordinates == None:
if worldCoordinates == None:
raise ValueError(
"One of relativeCoordinates or worldCoordinates must be provided"
)
if parent == None:
self._relativeCoordinates = worldCoordinates
else:
self._relativeCoordinates = se3.mul(
se3.inv(parent.worldCoordinates()), worldCoordinates)
else:
self._relativeCoordinates = relativeCoordinates
if worldCoordinates == None:
if parent == None:
self._worldcoordinates = relativeCoordinates
else:
self._worldCoordinates = se3.mul(parent.worldCoordinates(),
relativeCoordinates)
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 __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.xformposer = TransformPoser()
self.xformposer.set(*se3.mul(self.frame,value))
self.xformposer.enableRotation(True)
self.xformposer.enableTranslation(True)
self.addWidget(self.xformposer)
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.xformposer = TransformPoser()
self.xformposer.set(*se3.mul(self.frame, value))
self.xformposer.enableRotation(True)
self.xformposer.enableTranslation(True)
self.addWidget(self.xformposer)
def setWorldModel(self,worldModel):
"""Sets this group to contain all entities of a world model"""
for i in xrange(worldModel.numRobots()):
rgroup = self.addGroup(worldModel.robot(0).getName())
rgroup.setRobotModel(worldModel.robot(0))
for i in xrange(worldModel.numRigidObjects()):
f = self.addFrame(worldModel.rigidObject(0).getName(),worldCoordinates=worldModel.rigidObject(0).getTransform())
f._data = worldModel.rigidObject(0)
for i in xrange(worldModel.numTerrains()):
f = self.addFrame(worldModel.terrain(0).getName(),worldCoordinates=se3.identity())
f._data = worldModel.terrain(0)
return
def setWorldModel(self, worldModel):
"""Sets this group to contain all entities of a world model"""
for i in xrange(worldModel.numRobots()):
rgroup = self.addGroup(worldModel.robot(0).getName())
rgroup.setRobotModel(worldModel.robot(0))
for i in xrange(worldModel.numRigidObjects()):
f = self.addFrame(
worldModel.rigidObject(0).getName(),
worldCoordinates=worldModel.rigidObject(0).getTransform())
f._data = worldModel.rigidObject(0)
for i in xrange(worldModel.numTerrains()):
f = self.addFrame(worldModel.terrain(0).getName(),
worldCoordinates=se3.identity())
f._data = worldModel.terrain(0)
return
def __init__(self, name, worldCoordinates=se3.identity(), parent=None, relativeCoordinates=None):
self._name = name
self._parent = parent
self._worldCoordinates = worldCoordinates
self._data = None
if relativeCoordinates == None:
if worldCoordinates == None:
raise ValueError("One of relativeCoordinates or worldCoordinates must be provided")
if parent == None:
self._relativeCoordinates = worldCoordinates
else:
self._relativeCoordinates = se3.mul(se3.inv(parent.worldCoordinates()), worldCoordinates)
else:
self._relativeCoordinates = relativeCoordinates
if worldCoordinates == None:
if parent == None:
self._worldcoordinates = relativeCoordinates
else:
self._worldCoordinates = se3.mul(parent.worldCoordinates(), relativeCoordinates)
def addFrame(self,name,worldCoordinates=None,parent=None,relativeCoordinates=None):
"""Adds a new named Frame, possibly with a parent. 'parent' may either be a string
identifying another named Frame in this Group, or it can be a Frame object. (Warning:
unknown behavior may result from specifying a Frame not in this Group).
Either worldCoordinates or relativeCoordinates must be given. If worldCoordinates is given,
then the frame's initial relative transform is determined by the current coordinates of the
parent. If all parameters are left as default, the frame is placed directly at the origin
of the parent"""
if name in self.frames:
raise ValueError("Frame "+name+" already exists")
if parent==None:
parent = 'root'
if isinstance(parent,str):
parent = self.frames[parent]
if worldCoordinates == None and relativeCoordinates == None:
relativeCoordinates = se3.identity()
self.frames[name] = Frame(name,worldCoordinates=worldCoordinates,parent=parent,relativeCoordinates=relativeCoordinates)
self.childLists[parent._name].append(self.frames[name])
return self.frames[name]
def drawRaw():
gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01))
def edit(name,value,type='auto',description=None,editor='visual',world=None,frame=None):
"""Launches an editor for the given value. Returns a pair (save,result)
where save indicates what the user wanted to do with the edited value
and result is the edited value."""
if name == None and type=='auto':
raise RuntimeError("Cannot do an anonymous edit without the 'type' argument specified")
if name == None:
name = 'Anonymous'
if type == 'auto':
type = nameToType(name)
if not _PyQtAvailable and editor=='visual':
print "PyQt is not available, defaulting to console editor"
editor = 'console'
if isinstance(world,str):
#a single argument, e.g., a robot file
global _editTemporaryWorlds
if world not in _editTemporaryWorlds:
_editTemporaryWorlds[world] = WorldModel()
if not _editTemporaryWorlds[world].readFile(world):
raise RuntimeError("Error loading world file "+world)
world = _editTemporaryWorlds[world]
if isinstance(frame,str):
try:
oframe = world.rigidObject(frame)
frame = oframe
except RuntimeError:
try:
oframe = world.robot(0).getLink(frame)
frame = oframe
except RuntimeError:
try:
oframe = world.terrain(frame)
frame = oframe
except RuntimeError:
raise RuntimeError('Named frame "'+frame+'" is not a valid frame')
if value==None:
if type == 'Config':
if world==None:
raise RuntimeError("Cannot visually edit a Config resource without a world")
value = world.robot(0).getConfig()
elif type == 'Configs':
raise RuntimeError("Cannot visually edit a Configs resource without a world")
value = [world.robot(0).getConfig()]
elif type == 'IKGoal':
value = IKObjective()
elif type == 'Vector3' or type == 'Point':
value = [0,0,0]
elif type == 'Rotation':
value = so3.identity()
elif type == 'RigidTransform':
value = se3.identity()
else:
raise RuntimeError("Don't know how to edit objects of type "+type)
if editor == 'console':
return console_edit(name,value,type,description,world,frame)
elif editor == 'visual':
if type == 'Config':
return _launch(_ConfigVisualEditor(name,value,description,world))
elif type == 'Configs':
return _launch(_ConfigsVisualEditor(name,value,description,world))
elif type == 'Vector3' or type == 'Point':
if isinstance(frame,(RigidObjectModel,RobotModelLink)):
frame = frame.getTransform()
return _launch(_PointVisualEditor(name,value,description,world,frame))
elif type == 'Rotation':
if isinstance(frame,(RigidObjectModel,RobotModelLink)):
frame = frame.getTransform()
return _launch(_RotationVisualEditor(name,value,description,world,frame))
elif type == 'RigidTransform':
if isinstance(frame,RigidObjectModel):
return _launch(_ObjectTransformVisualEditor(name,value,description,world,frame))
if isinstance(frame,RobotModelLink):
frame = frame.getTransform()
return _launch(_RigidTransformVisualEditor(name,value,description,world,frame))
else:
raise RuntimeError("Don't know how to edit objects of type "+type)
else:
raise ValueError("Invalid value for argument 'editor', must be either 'visual' or 'console'")
def edit(name,
value,
type='auto',
description=None,
editor='visual',
world=None,
frame=None):
"""Launches an editor for the given value. Returns a pair (save,result)
where save indicates what the user wanted to do with the edited value
and result is the edited value."""
if name == None and type == 'auto':
raise RuntimeError(
"Cannot do an anonymous edit without the 'type' argument specified"
)
if name == None:
name = 'Anonymous'
if type == 'auto':
type = nameToType(name)
if not _PyQtAvailable and editor == 'visual':
print "PyQt is not available, defaulting to console editor"
editor = 'console'
if isinstance(world, str):
#a single argument, e.g., a robot file
global _editTemporaryWorlds
if world not in _editTemporaryWorlds:
_editTemporaryWorlds[world] = WorldModel()
if not _editTemporaryWorlds[world].readFile(world):
raise RuntimeError("Error loading world file " + world)
world = _editTemporaryWorlds[world]
if isinstance(frame, str):
try:
oframe = world.rigidObject(frame)
frame = oframe
except RuntimeError:
try:
oframe = world.robot(0).getLink(frame)
frame = oframe
except RuntimeError:
try:
oframe = world.terrain(frame)
frame = oframe
except RuntimeError:
raise RuntimeError('Named frame "' + frame +
'" is not a valid frame')
if value == None:
if type == 'Config':
if world == None:
raise RuntimeError(
"Cannot visually edit a Config resource without a world")
value = world.robot(0).getConfig()
elif type == 'Configs':
raise RuntimeError(
"Cannot visually edit a Configs resource without a world")
value = [world.robot(0).getConfig()]
elif type == 'IKGoal':
value = IKObjective()
elif type == 'Vector3' or type == 'Point':
value = [0, 0, 0]
elif type == 'Rotation':
value = so3.identity()
elif type == 'RigidTransform':
value = se3.identity()
else:
raise RuntimeError("Don't know how to edit objects of type " +
type)
if editor == 'console':
return console_edit(name, value, type, description, world, frame)
elif editor == 'visual':
if type == 'Config':
return _launch(_ConfigVisualEditor(name, value, description,
world))
elif type == 'Configs':
return _launch(
_ConfigsVisualEditor(name, value, description, world))
elif type == 'Vector3' or type == 'Point':
if isinstance(frame, (RigidObjectModel, RobotModelLink)):
frame = frame.getTransform()
return _launch(
_PointVisualEditor(name, value, description, world, frame))
elif type == 'Rotation':
if isinstance(frame, (RigidObjectModel, RobotModelLink)):
frame = frame.getTransform()
return _launch(
_RotationVisualEditor(name, value, description, world, frame))
elif type == 'RigidTransform':
if isinstance(frame, RigidObjectModel):
return _launch(
_ObjectTransformVisualEditor(name, value, description,
world, frame))
if isinstance(frame, RobotModelLink):
frame = frame.getTransform()
return _launch(
_RigidTransformVisualEditor(name, value, description, world,
frame))
else:
raise RuntimeError("Don't know how to edit objects of type " +
type)
else:
raise ValueError(
"Invalid value for argument 'editor', must be either 'visual' or 'console'"
)
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)
