Ejemplo n.º 1
0
Archivo: ik.py Proyecto: arocchi/Klampt
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")
Ejemplo n.º 2
0
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"
        )
Ejemplo n.º 3
0
 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())
Ejemplo n.º 4
0
 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)
Ejemplo n.º 5
0
	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])
Ejemplo n.º 6
0
 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)
Ejemplo n.º 7
0
 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)
Ejemplo n.º 8
0
 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)
Ejemplo n.º 9
0
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()
Ejemplo n.º 10
0
 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)
Ejemplo n.º 11
0
 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)
Ejemplo n.º 12
0
    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)
Ejemplo n.º 13
0
 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)
Ejemplo n.º 14
0
 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)
Ejemplo n.º 15
0
 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())
Ejemplo n.º 16
0
 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())
Ejemplo n.º 17
0
 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())