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,
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 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 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 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 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 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 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 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 click_ray(self,x,y):
"""Returns a pair of 3-tuples indicating the ray source and direction
in world coordinates for a screen-coordinate point (x,y)"""
R,t = se3.inv(self.camera.matrix())
#from x and y compute ray direction
u = float(x-self.width/2)
v = float(self.height-y-self.height/2)
scale = math.tan(self.fov*math.pi/180.0)/self.width
d = (u*scale,v*scale,-1.0)
return (t,so3.apply(R,d))
def viewport(self):
"""Gets a Viewport instance corresponding to the current view.
This is used to interface with the Widget classes"""
vp = Viewport()
vp.x,vp.y,vp.w,vp.h = 0,0,self.width,self.height
vp.n,vp.f = self.clippingplanes
vp.perspective = True
aspect = float(self.width)/float(self.height)
rfov = self.fov*math.pi/180.0
vp.scale = 1.0/(2.0*math.tan(rfov*0.5/aspect)*aspect)
vp.setRigidTransform(*se3.inv(self.camera.matrix()))
return vp
def viewport(self):
"""Gets a Viewport instance corresponding to the current view.
This is used to interface with the Widget classes"""
vp = Viewport()
vp.x,vp.y,vp.w,vp.h = 0,0,self.width,self.height
vp.n,vp.f = self.clippingplanes
vp.perspective = True
aspect = float(self.width)/float(self.height)
rfov = self.fov*math.pi/180.0
vp.scale = 1.0/(2.0*math.tan(rfov*0.5/aspect)*aspect)
vp.setRigidTransform(*se3.inv(self.camera.matrix()))
return vp
def click_ray(self,x,y):
"""Returns a pair of 3-tuples indicating the ray source and direction
in world coordinates for a screen-coordinate point (x,y)"""
R,t = se3.inv(self.camera.matrix())
#from x and y compute ray direction
u = float(x-self.width/2)
v = float(self.height-y-self.height/2)
aspect = float(self.width)/float(self.height)
rfov = self.fov*math.pi/180.0
scale = 2.0*math.tan(rfov*0.5/aspect)*aspect
d = (u*scale,v*scale,-1.0)
d = vectorops.div(d,vectorops.norm(d))
return (t,so3.apply(R,d))
def click_ray(self,x,y):
"""Returns a pair of 3-tuples indicating the ray source and direction
in world coordinates for a screen-coordinate point (x,y)"""
R,t = se3.inv(self.camera.matrix())
#from x and y compute ray direction
u = float(x-self.width/2)
v = float(self.height-y-self.height/2)
aspect = float(self.width)/float(self.height)
rfov = self.fov*math.pi/180.0
scale = 2.0*math.tan(rfov*0.5/aspect)*aspect
d = (u*scale,v*scale,-1.0)
d = vectorops.div(d,vectorops.norm(d))
return (t,so3.apply(R,d))
def click_ray(self,x,y):
"""Returns a pair of 3-tuples indicating the ray source and direction
in world coordinates for a screen-coordinate point (x,y)"""
R,t = se3.inv(self.camera.matrix())
#from x and y compute ray direction
u = float(x-self.width/2)
v = float(self.height-y-self.height/2)
scale = math.tan(self.fov*math.pi/180.0)/self.height
#HACK: I don't know why this seems to work!
scale *= 0.925
d = (u*scale,v*scale,-1.0)
d = vectorops.div(d,vectorops.norm(d))
return (t,so3.apply(R,d))
def __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 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 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 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 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 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 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 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())
