def updateFromWorld(self):
"""For any frames with associated world elements, updates the
transforms from the world elements."""
for (n, f) in self.frames.iteritems():
if f._data == None: continue
if hasattr(f._data, 'getTransform'):
worldCoordinates = f._data.getTransform()
if hasattr(f._data, 'getParent'):
p = f._data.getParent()
if p >= 0:
plink = f._data.robot().link(p)
parentCoordinates = plink.getTransform()
f._relativeCoordinates = se3.mul(
se3.inv(parentCoordinates), worldCoordinates)
else:
f._relativeCoordinates = worldCoordinates
else:
f._relativeCoordinates = worldCoordinates
f._worldCoordinates = worldCoordinates
#update downstream non-link items
for c in self.childLists[f._name]:
if c.data == None or not hasattr(c._data, 'getTransform'):
c._worldCoordinates = se3.mul(f._worldCoordinates,
c._relativeCoordinates)
self.updateDependentFrames(c)
if isinstance(f._data, tuple) and isinstance(
f._data[0], SimRobotController):
controller, index, itemtype = f._data
#TODO: update the frame from the controller data
for (n, g) in self.subgroups.iteritems():
g.updateFromWorld()
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 updateFromWorld(self):
"""For any frames with associated world elements, updates the
transforms from the world elements."""
for (n,f) in self.frames.iteritems():
if f._data == None: continue
if hasattr(f._data,'getTransform'):
worldCoordinates = f._data.getTransform()
if hasattr(f._data,'getParent'):
p = f._data.getParent()
if p >= 0:
plink = f._data.robot().link(p)
parentCoordinates = plink.getTransform()
f._relativeCoordinates = se3.mul(se3.inv(parentCoordinates),worldCoordinates)
else:
f._relativeCoordinates = worldCoordinates
else:
f._relativeCoordinates = worldCoordinates
f._worldCoordinates = worldCoordinates
#update downstream non-link items
for c in self.childLists[f._name]:
if c.data == None or not hasattr(c._data,'getTransform'):
c._worldCoordinates = se3.mul(f._worldCoordinates,c._relativeCoordinates)
self.updateDependentFrames(c)
if isinstance(f._data,tuple) and isinstance(f._data[0],SimRobotController):
controller,index,itemtype = f._data
#TODO: update the frame from the controller data
for (n,g) in self.subgroups.iteritems():
g.updateFromWorld()
def setFrameCoordinates(self,name,coordinates,parent='relative'):
"""Sets the coordinates of the frame, given as an se3 element.
The coordinates can be given either in 'relative' mode, where the
coordinates are the natural coordinates of the frame relative to
its parent, or in 'world' mode, where the coordinates are the
global world coordinates, or they can be given relative to any
other frame in this coordinate Group. If None, this defaults
to the root frame of this Group."""
f = self.frame(name)
if parent==None:
parent = 'root'
if isinstance(parent,str):
if parent=='relative':
parent = f._parent
elif parent=='world':
parent = None
else:
parent = self.frames[parent]
if parent:
worldCoordinates = se3.mul(parent._worldCoordinates,coordinates)
else:
worldCoordinates = coordinates
if parent == f._parent:
f._relativeCoordinates = coordinates
else:
f._relativeCoordinates = se3.mul(se3.inv(f._parent._worldCoordinates),worldCoordinates)
f._worldCoordinates = worldCoordinates
self.updateDependentFrames(f)
def setFrameCoordinates(self, name, coordinates, parent='relative'):
"""Sets the coordinates of the frame, given as an se3 element.
The coordinates can be given either in 'relative' mode, where the
coordinates are the natural coordinates of the frame relative to
its parent, or in 'world' mode, where the coordinates are the
global world coordinates, or they can be given relative to any
other frame in this coordinate Group. If None, this defaults
to the root frame of this Group."""
f = self.frame(name)
if parent == None:
parent = 'root'
if isinstance(parent, str):
if parent == 'relative':
parent = f._parent
elif parent == 'world':
parent = None
else:
parent = self.frames[parent]
if parent:
worldCoordinates = se3.mul(parent._worldCoordinates, coordinates)
else:
worldCoordinates = coordinates
if parent == f._parent:
f._relativeCoordinates = coordinates
else:
f._relativeCoordinates = se3.mul(
se3.inv(f._parent._worldCoordinates), worldCoordinates)
f._worldCoordinates = worldCoordinates
self.updateDependentFrames(f)
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 postTransform(self,R): """Postmultiplies every rotation in here by the se3 element R. In other words, if R rotates a local frame F to frame F', this method converts this SO3Trajectory from describing how F' rotates to how F rotates.""" for i,m in enumerate(self.milestones): self.milestones[i] = se3.mul(m,T)
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 coordinates(self):
"""Returns the SE(3) coordinates that transform elements from the
source to the destination Frame."""
if self._destination == None:
return self._source.worldCoodinates()
return se3.mul(se3.inv(self._destination.worldCoordinates()),
self._source.worldCoordinates())
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 postTransform(self,T): """Postmultiplies every transform in here by the se3 element T. In other words, if T transforms a local frame F to frame F', this method converts this SE3Trajectory from describing how F' moves to how F moves.""" for i,m in enumerate(self.milestones): Tm = (m[:9],m[9:]) self.milestones[i] = se3.mul(Tm,T)
def preTransform(self,T): """Premultiplies every transform in here by the se3 element T. In other words, if T transforms a local frame F to frame F', this method converts this SE3Trajectory from coordinates in F to coordinates in F'""" for i,m in enumerate(self.milestones): Tm = (m[:9],m[9:]) self.milestones[i] = se3.mul(T,Tm)
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 updateDependentFrames(self,frame):
"""Whenever Frame's world coordinates are updated, call this to update
the downstream frames. This will be called automatically via
setFrameCoordinates but not if you change a Frame's coordinates
manually."""
for c in self.childLists[frame._name]:
c._worldCoordinates = se3.mul(frame._worldCoordinates,c._relativeCoordinates)
self.updateDependentFrames(c)
def updateDependentFrames(self, frame):
"""Whenever Frame's world coordinates are updated, call this to update
the downstream frames. This will be called automatically via
setFrameCoordinates but not if you change a Frame's coordinates
manually."""
for c in self.childLists[frame._name]:
c._worldCoordinates = se3.mul(frame._worldCoordinates,
c._relativeCoordinates)
self.updateDependentFrames(c)
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 integrate(self,x,d):
w = [d[0],d[4],d[8]]
v = d[9:]
return se3.mul(x,(so3.from_moment(w),v))
def ik_objective(obj, target):
"""Returns an IK objective that attempts to fix the given
klampt.coordinates object 'obj' at given target object 'target'.
Arguments:
- obj: An instance of one of the
{Point,Direction,Transform,Frame} classes.
- target: If 'obj' is a Point, Direction, or Frame objects, this
must be an object of the same type of 'obj' denoting the target to
which 'obj' should be fixed. In other words, the local coordinates
of 'obj' relative to 'target's parent frame will be equal to 'target's
local coordinates.
If obj is a Transform object, this element is an se3 object.
Return value:
- An IK objective to be used with the klampt.ik module.
Since the klampt.ik module is not aware about custom frames, an
ancestor of the object must be attached to a RobotModelLink or a
RigidObjectModel, or else None will be returned. The same goes for target,
if provided.
TODO: support lists of objects to fix.
TODO: support Direction constraints.
"""
body = None
coords = None
ref = None
if isinstance(obj, Frame):
assert isinstance(
target, Frame), "ik_objective: target must be of same type as obj"
body = obj
ref = target.parent()
coords = target.relativeCoordinates()
elif isinstance(obj, Transform):
if ref != None:
print "ik_objective: Warning, ref argument passed with Transform object, ignoring"
body = obj.source()
ref = obj.destination()
coords = target
elif isinstance(obj, (Point, Direction)):
assert type(target) == type(
obj), "ik_objective: target must be of same type as obj"
body = obj.frame()
ref = target.frame()
coords = target.localCoordinates()
else:
raise ValueError(
"Argument to ik_objective must be an object from the coordinates module"
)
linkframe = _ancestor_with_link(body)
if linkframe == None:
print "Warning: object provided to ik_objective is not attached to a robot link or rigid object, returning None"
return None
linkbody = linkframe._data
#find the movable frame attached to ref
refframe = _ancestor_with_link(ref) if ref != None else None
refbody = (refframe._data if refframe != None else None)
if isinstance(obj, (Frame, Transform)):
#figure out the desired transform T[linkbody->refbody], given
#coords = T[obj->ref], T[obj->linkbody], T[ref->refbody]
#result = (T[ref->refbody] * coords * T[obj->linkbody]^-1)
if linkframe != body:
coords = se3.mul(coords, Transform(linkframe, body).coordinates())
if refframe != ref:
coords = se3.mul(Transform(ref, refframe).coordinates(), coords)
return ik.objective(linkbody, ref=refbody, R=coords[0], t=coords[1])
elif isinstance(obj, Point):
#figure out the local and world points
local = obj.to(linkframe).localCoordinates()
world = target.to(refframe).localCoordinates()
return ik.objective(linkbody, local=[local], world=[world])
elif isinstance(obj, Direction):
raise ValueError(
"Axis constraints are not yet supported in the klampt.ik module")
return None
def integrate(self, x, d):
w = [d[0], d[4], d[8]]
v = d[9:]
return se3.mul(x, (so3.from_moment(w), v))
def mousefunc(self,button,state,x,y):
if _VisualEditorBase.mousefunc(self,button,state,x,y):
self.value = se3.mul(se3.inv(self.frame),self.xformposer.get())
def coordinates(self):
"""Returns the SE(3) coordinates that transform elements from the
source to the destination Frame."""
if self._destination==None:
return self._source.worldCoodinates()
return se3.mul(se3.inv(self._destination.worldCoordinates()),self._source.worldCoordinates())
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 mousefunc(self, button, state, x, y):
if _VisualEditorBase.mousefunc(self, button, state, x, y):
self.value = se3.mul(se3.inv(self.frame), self.xformposer.get())
def ik_objective(obj,target):
"""Returns an IK objective that attempts to fix the given
klampt.coordinates object 'obj' at given target object 'target'.
Arguments:
- obj: An instance of one of the
{Point,Direction,Transform,Frame} classes.
- target: If 'obj' is a Point, Direction, or Frame objects, this
must be an object of the same type of 'obj' denoting the target to
which 'obj' should be fixed. In other words, the local coordinates
of 'obj' relative to 'target's parent frame will be equal to 'target's
local coordinates.
If obj is a Transform object, this element is an se3 object.
Return value:
- An IK objective to be used with the klampt.ik module.
Since the klampt.ik module is not aware about custom frames, an
ancestor of the object must be attached to a RobotModelLink or a
RigidObjectModel, or else None will be returned. The same goes for target,
if provided.
TODO: support lists of objects to fix.
TODO: support Direction constraints.
"""
body = None
coords = None
ref = None
if isinstance(obj,Frame):
assert isinstance(target,Frame),"ik_objective: target must be of same type as obj"
body = obj
ref = target.parent()
coords = target.relativeCoordinates()
elif isinstance(obj,Transform):
if ref != None: print "ik_objective: Warning, ref argument passed with Transform object, ignoring"
body = obj.source()
ref = obj.destination()
coords = target
elif isinstance(obj,(Point,Direction)):
assert type(target)==type(obj),"ik_objective: target must be of same type as obj"
body = obj.frame()
ref = target.frame()
coords = target.localCoordinates()
else:
raise ValueError("Argument to ik_objective must be an object from the coordinates module")
linkframe = _ancestor_with_link(body)
if linkframe == None:
print "Warning: object provided to ik_objective is not attached to a robot link or rigid object, returning None"
return None
linkbody = linkframe._data
#find the movable frame attached to ref
refframe = _ancestor_with_link(ref) if ref != None else None
refbody = (refframe._data if refframe!=None else None)
if isinstance(obj,(Frame,Transform)):
#figure out the desired transform T[linkbody->refbody], given
#coords = T[obj->ref], T[obj->linkbody], T[ref->refbody]
#result = (T[ref->refbody] * coords * T[obj->linkbody]^-1)
if linkframe != body: coords = se3.mul(coords,Transform(linkframe,body).coordinates())
if refframe != ref: coords = se3.mul(Transform(ref,refframe).coordinates(),coords)
return ik.objective(linkbody,ref=refbody,R=coords[0],t=coords[1])
elif isinstance(obj,Point):
#figure out the local and world points
local = obj.to(linkframe).localCoordinates()
world = target.to(refframe).localCoordinates()
return ik.objective(linkbody,local=[local],world=[world])
elif isinstance(obj,Direction):
raise ValueError("Axis constraints are not yet supported in the klampt.ik module")
return None
