Esempio n. 1
0
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()
Esempio n. 2
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)
Esempio n. 3
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)
Esempio n. 4
0
    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]
Esempio n. 5
0
 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)
Esempio n. 6
0
File: ik.py Progetto: 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")
Esempio n. 7
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"
        )
Esempio n. 8
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.xformposer = TransformPoser()
     self.xformposer.set(*se3.mul(self.frame,value))
     self.xformposer.enableRotation(True)
     self.xformposer.enableTranslation(True)
     self.addWidget(self.xformposer)
Esempio n. 9
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.xformposer = TransformPoser()
     self.xformposer.set(*se3.mul(self.frame, value))
     self.xformposer.enableRotation(True)
     self.xformposer.enableTranslation(True)
     self.addWidget(self.xformposer)
Esempio n. 10
0
 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
Esempio n. 11
0
 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
Esempio n. 12
0
 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)
Esempio n. 13
0
    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]
Esempio n. 14
0
 def drawRaw():
     gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01))
Esempio n. 15
0
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'")
Esempio n. 16
0
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'"
        )
Esempio n. 17
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)