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 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())