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'"
        )
Example #2
0
 def __init__(self):
     Context.__init__(self)
     self.type = Type('V',9)
     Rvar = Variable("R",self.type)
     Rsymb = VariableExpression(Rvar)
     R1 = Variable("R1",self.type)
     R2 = Variable("R2",self.type)
     V3type = Type('V',3)
     q = Variable('q',Type('V',4))
     pointvar = Variable("point",V3type)
     pointsymb = VariableExpression(pointvar)
     self.identity = self.declare(expr(so3.identity()),"identity",[])
     self.identity.description = "The identity rotation"
     self.matrix = self.declare(expr(so3.matrix(Rsymb)),"matrix",["R"])
     self.matrix.addSimplifier(['so3.identity'],(lambda R:eye(3)),pre=True)
     self.matrix.description = "Converts to a 3x3 matrix"
     M = Variable("M",Type('M',(3,3)))
     self.from_matrix = self.declare(flatten(transpose(M)),"from_matrix",['M'])
     self.from_matrix.description = "Converts from a 3x3 matrix"
     self.from_matrix.autoSetJacobians()
     self.inv = self.declare(expr(so3.inv(Rsymb)),"inv",["R"])
     self.inv.description = "Inverts a rotation"
     self.inv.autoSetJacobians()
     self.inv.properties['inverse'] = weakref.proxy(self.inv)
     self.inv.addSimplifier(['so3.identity'],lambda R:R)
     self.mul = self.declare(so3.mul,"mul")
     self.mul.description = "Inverts a rotation"
     self.mul.setDeriv(0,lambda R1,R2,dR1:self.mul(dR1,R2),asExpr=True)
     self.mul.setDeriv(1,lambda R1,R2,dR2:self.mul(R1,dR2),asExpr=True)
     self.mul.addSimplifier(['so3.identity',None],(lambda R1,R2:R2),pre=True)
     self.mul.addSimplifier([None,'so3.identity'],(lambda R1,R2:R1),pre=True)
     self.mul.properties['associative'] = True
     self.apply = self.declare(expr(so3.apply(Rsymb,pointsymb)),"apply",["R","point"])
     self.apply.addSimplifier(['so3.identity',None],(lambda R,point:point),pre=True)
     self.apply.addSimplifier([None,'zero'],(lambda R,point:point),pre=True)
     self.apply.autoSetJacobians()
     self.rotation = self.declare(so3.rotation,"rotation")
     self.from_rpy = self.declare(so3.from_rpy,"from_rpy")
     self.rpy = self.declare(so3.rpy,"rpy")
     self.from_quaternion = self.declare(expr(so3.from_quaternion([q[0],q[1],q[2],q[3]])),"from_quaternion",["q"])
     self.quaternion = self.declare(so3.quaternion,"quaternion")
     self.from_rotation_vector = self.declare(so3.from_rotation_vector,"from_rotation_vector")
     self.rotation_vector = self.declare(so3.rotation_vector,"rotation_vector")
     self.axis = self.declare(unit(self.rotation_vector(Rvar)),"rotation",["R"])
     self.angle = self.declare(so3.angle,"angle")
     self.error = self.declare(so3.error,"error")
     self.distance = self.declare(self.angle(self.mul(self.inv(R1),R2)),"distance",['R1','R2'])
     self.distance.properties['nonnegative'] = True
     Rm = self.matrix(Rsymb)
     self.eq_constraint = self.declare(dot(Rm.T,Rm),'eq_constraint',['R'])
     self.quaternion_constraint = self.declare(norm2(q)-1,'quaternion_constraint',['q'])
     self.identity.returnType = self.type
     self.inv.returnType = self.type
     self.inv.argTypes = [self.type]
     self.mul.returnType = self.type
     self.mul.argTypes = [self.type,self.type]
     self.apply.returnType = V3type
     self.apply.argTypes = [self.type,V3type]
     self.rotation.returnType = self.type
     self.rotation.argTypes = [V3type,Numeric]
     self.rotation.setDeriv(1,lambda axis,angle:so3.cross_product(axis))
     self.axis.returnType = V3type
     self.axis.argTypes = [self.type]
     self.angle.returnType = V3type
     self.angle.argTypes = [self.type]
     def angle_deriv(R,dR):
         cosangle = (R[0]+R[4]+R[8]-1)*0.5
         angle = arccos(cosangle)
         #dangle / dR[0] = -1.0/sqrt(1-cosangle**2) * dcosangle/dR[0]
         dacos = -1.0/sqrt(1-cosangle**2)
         return expr([0.5*dacos*dR[0],0,0,0,0.5*dacos*dR[4],0,0,0,0.5*dacos*dR[8]])
     self.angle.setDeriv(0,angle_deriv,asExpr=True)
     self.error.returnType = V3type
     self.error.argTypes = [self.type,self.type]
     self.distance.returnType = Numeric
     self.distance.argTypes = [self.type,self.type]
     self.distance.autoSetJacobians()
     self.from_matrix.returnType = self.type
     self.from_matrix.argTypes = [M.type]
     self.from_rpy.returnType = self.type
     self.from_rpy.argTypes = [V3type]
     self.from_quaternion.returnType = self.type
     self.from_quaternion.argTypes = [Type('V',4)]
     self.from_rotation_vector.returnType = self.type
     self.from_rotation_vector.argTypes = [V3type]
     self.matrix.returnType = self.from_matrix.argTypes[0]
     self.matrix.argTypes = [self.from_matrix.returnType]
     self.rpy.returnType = self.from_rpy.argTypes[0]
     self.rpy.argTypes = [self.from_rpy.returnType]
     self.quaternion.returnType = self.from_quaternion.argTypes[0]
     self.quaternion.argTypes = [self.from_quaternion.returnType]
     self.rotation_vector.returnType = self.from_rotation_vector.argTypes[0]
     self.rotation_vector.argTypes = [self.from_rotation_vector.returnType]
Example #3
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)
Example #4
0
    def __init__(self):
        Context.__init__(self)
        self.type = Type('V', 9)
        Rvar = Variable("R", self.type)
        Rsymb = VariableExpression(Rvar)
        R1 = Variable("R1", self.type)
        R2 = Variable("R2", self.type)
        V3type = Type('V', 3)
        q = Variable('q', Type('V', 4))
        pointvar = Variable("point", V3type)
        pointsymb = VariableExpression(pointvar)
        self.identity = self.declare(expr(so3.identity()), "identity", [])
        self.identity.description = "The identity rotation"
        self.matrix = self.declare(expr(so3.matrix(Rsymb)), "matrix", ["R"])
        self.matrix.addSimplifier(['so3.identity'], (lambda R: eye(3)),
                                  pre=True)
        self.matrix.description = "Converts to a 3x3 matrix"
        M = Variable("M", Type('M', (3, 3)))
        self.from_matrix = self.declare(flatten(transpose(M)), "from_matrix",
                                        ['M'])
        self.from_matrix.description = "Converts from a 3x3 matrix"
        self.from_matrix.autoSetJacobians()
        self.inv = self.declare(expr(so3.inv(Rsymb)), "inv", ["R"])
        self.inv.description = "Inverts a rotation"
        self.inv.autoSetJacobians()
        self.inv.properties['inverse'] = weakref.proxy(self.inv)
        self.inv.addSimplifier(['so3.identity'], lambda R: R)
        self.mul = self.declare(so3.mul, "mul")
        self.mul.description = "Inverts a rotation"
        self.mul.setDeriv(0,
                          lambda R1, R2, dR1: self.mul(dR1, R2),
                          asExpr=True)
        self.mul.setDeriv(1,
                          lambda R1, R2, dR2: self.mul(R1, dR2),
                          asExpr=True)
        self.mul.addSimplifier(['so3.identity', None], (lambda R1, R2: R2),
                               pre=True)
        self.mul.addSimplifier([None, 'so3.identity'], (lambda R1, R2: R1),
                               pre=True)
        self.mul.properties['associative'] = True
        self.apply = self.declare(expr(so3.apply(Rsymb, pointsymb)), "apply",
                                  ["R", "point"])
        self.apply.addSimplifier(['so3.identity', None],
                                 (lambda R, point: point),
                                 pre=True)
        self.apply.addSimplifier([None, 'zero'], (lambda R, point: point),
                                 pre=True)
        self.apply.autoSetJacobians()
        self.rotation = self.declare(so3.rotation, "rotation")
        self.from_rpy = self.declare(so3.from_rpy, "from_rpy")
        self.rpy = self.declare(so3.rpy, "rpy")
        self.from_quaternion = self.declare(
            expr(so3.from_quaternion([q[0], q[1], q[2], q[3]])),
            "from_quaternion", ["q"])
        self.quaternion = self.declare(so3.quaternion, "quaternion")
        self.from_rotation_vector = self.declare(so3.from_rotation_vector,
                                                 "from_rotation_vector")
        self.rotation_vector = self.declare(so3.rotation_vector,
                                            "rotation_vector")
        self.axis = self.declare(unit(self.rotation_vector(Rvar)), "rotation",
                                 ["R"])
        self.angle = self.declare(so3.angle, "angle")
        self.error = self.declare(so3.error, "error")
        self.distance = self.declare(self.angle(self.mul(self.inv(R1), R2)),
                                     "distance", ['R1', 'R2'])
        self.distance.properties['nonnegative'] = True
        Rm = self.matrix(Rsymb)
        self.eq_constraint = self.declare(dot(Rm.T, Rm), 'eq_constraint',
                                          ['R'])
        self.quaternion_constraint = self.declare(
            norm2(q) - 1, 'quaternion_constraint', ['q'])
        self.identity.returnType = self.type
        self.inv.returnType = self.type
        self.inv.argTypes = [self.type]
        self.mul.returnType = self.type
        self.mul.argTypes = [self.type, self.type]
        self.apply.returnType = V3type
        self.apply.argTypes = [self.type, V3type]
        self.rotation.returnType = self.type
        self.rotation.argTypes = [V3type, Numeric]
        self.rotation.setDeriv(1, lambda axis, angle: so3.cross_product(axis))
        self.axis.returnType = V3type
        self.axis.argTypes = [self.type]
        self.angle.returnType = V3type
        self.angle.argTypes = [self.type]

        def angle_deriv(R, dR):
            cosangle = (R[0] + R[4] + R[8] - 1) * 0.5
            angle = arccos(cosangle)
            #dangle / dR[0] = -1.0/sqrt(1-cosangle**2) * dcosangle/dR[0]
            dacos = -1.0 / sqrt(1 - cosangle**2)
            return expr([
                0.5 * dacos * dR[0], 0, 0, 0, 0.5 * dacos * dR[4], 0, 0, 0,
                0.5 * dacos * dR[8]
            ])

        self.angle.setDeriv(0, angle_deriv, asExpr=True)
        self.error.returnType = V3type
        self.error.argTypes = [self.type, self.type]
        self.distance.returnType = Numeric
        self.distance.argTypes = [self.type, self.type]
        self.distance.autoSetJacobians()
        self.from_matrix.returnType = self.type
        self.from_matrix.argTypes = [M.type]
        self.from_rpy.returnType = self.type
        self.from_rpy.argTypes = [V3type]
        self.from_quaternion.returnType = self.type
        self.from_quaternion.argTypes = [Type('V', 4)]
        self.from_rotation_vector.returnType = self.type
        self.from_rotation_vector.argTypes = [V3type]
        self.matrix.returnType = self.from_matrix.argTypes[0]
        self.matrix.argTypes = [self.from_matrix.returnType]
        self.rpy.returnType = self.from_rpy.argTypes[0]
        self.rpy.argTypes = [self.from_rpy.returnType]
        self.quaternion.returnType = self.from_quaternion.argTypes[0]
        self.quaternion.argTypes = [self.from_quaternion.returnType]
        self.rotation_vector.returnType = self.from_rotation_vector.argTypes[0]
        self.rotation_vector.argTypes = [self.from_rotation_vector.returnType]
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'")
Example #6
0
def from_translation(t):
    """Returns a transformation T that translates points by t"""
    return (so3.identity(), t[:])
Example #7
0
def from_translation(t):
    """Returns a transformation T that translates points by t"""
    return (so3.identity(),t[:])