Exemple #1
0
 def to(self,newframe):
     """Returns a Direction representing the same direction in space, but
     in a different reference frame"""
     if newframe == None or newframe=='world':
         return self.toWorld()
     newlocal = so3.apply(so3.inv(newframe.worldCoordinates()[0]),self.worldCoordinates())
     return Direction(newlocal,newframe)
Exemple #2
0
def readSe3(text):
    """Reads an se3 element, i.e., rigid transformation, to string in the
    same format as written to by Klampt C++ bindings (row major R, followed by
    t)."""
    items = text.split()
    if len(items) != 12: raise ValueError("Invalid element of SE3, must have 12 elements")
    return (so3.inv([float(v) for v in items[:9]]),[float(v) for v in items[9:]])
Exemple #3
0
 def rotationCoordinates(self):
     """Returns the SO(3) coordinates that rotate elements from the source
     to the destination Frame"""
     if self._destination == None:
         return self._source.worldRotation()
     return so3.mul(so3.inv(self._destination.worldRotation()),
                    self._source.worldRotation())
Exemple #4
0
def inv(T):
    """Returns the inverse of the transformation."""
    (R,t) = T
    Rinv = so3.inv(R)
    tinv = [-Rinv[0]*t[0]-Rinv[3]*t[1]-Rinv[6]*t[2],
            -Rinv[1]*t[0]-Rinv[4]*t[1]-Rinv[7]*t[2],
            -Rinv[2]*t[0]-Rinv[5]*t[1]-Rinv[8]*t[2]]
    return (Rinv,tinv)
Exemple #5
0
 def to(self, newframe):
     """Returns a Direction representing the same direction in space, but
     in a different reference frame"""
     if newframe == None or newframe == 'world':
         return self.toWorld()
     newlocal = so3.apply(so3.inv(newframe.worldCoordinates()[0]),
                          self.worldCoordinates())
     return Direction(newlocal, newframe)
Exemple #6
0
 def worldOffset(self, dir):
     """Offsets this direction by a vector in world coordinates"""
     if self._frame == None:
         self._localCoordinates = vectorops.add(self._localCoordinates, dir)
     else:
         self._localCoordinates = vectorops.add(
             so3.apply(so3.inv(self._frame.worldCoordinates()[0]),
                       self._localCoordinates), dir)
Exemple #7
0
def inv(T):
    """Returns the inverse of the transformation."""
    (R,t) = T
    Rinv = so3.inv(R)
    tinv = [-Rinv[0]*t[0]-Rinv[3]*t[1]-Rinv[6]*t[2],
            -Rinv[1]*t[0]-Rinv[4]*t[1]-Rinv[7]*t[2],
            -Rinv[2]*t[0]-Rinv[5]*t[1]-Rinv[8]*t[2]]
    return (Rinv,tinv)
Exemple #8
0
 def worldOffset(self, dir):
     """Offsets this direction by a vector in world coordinates"""
     if self._frame == None:
         self._localCoordinates = vectorops.add(self._localCoordinates, dir)
     else:
         self._localCoordinates = vectorops.add(
             so3.apply(so3.inv(self._frame.worldCoordinates()[0]), self._localCoordinates), dir
         )
Exemple #9
0
 def __init__(self):
     Context.__init__(self)
     self.Rtype = Type('V',9)
     self.ttype = Type('V',3)
     self.type = Type('L',2,[self.Rtype,self.ttype])
     T = Variable("T",self.type)
     R = Variable("R",self.Rtype)
     t = Variable("t",self.ttype)
     self.make = self.declare(array(R,t),"make",['R','t'])
     self.identity = self.declare(se3.identity,"identity")
     self.homogeneous = self.declare(se3.homogeneous,"homogeneous")
     self.homogeneous.addSimplifier(['se3.identity'],lambda T:eye(4))
     Rinv = so3.inv(T[0])
     self.inv = self.declare(array(Rinv,neg(so3.apply(Rinv,T[1]))),"inv",['T'])
     self.inv.autoSetJacobians()
     self.inv.properties['inverse'] = weakref.proxy(self.inv)
     self.inv.addSimplifier(['se3.identity'],lambda T:T)
     self.mul = self.declare(se3.mul,"mul")
     self.mul.setDeriv(0,lambda T1,T2,dT1:self.mul(dT1,T2),asExpr=True)
     self.mul.setDeriv(1,lambda T1,T2,dT2:self.mul(T1,dT2),asExpr=True)
     self.mul.addSimplifier(['se3.identity',None],(lambda T1,T2:T2),pre=True)
     self.mul.addSimplifier([None,'se3.identity'],(lambda T1,T2:T1),pre=True)
     self.mul.properties['associative'] = True
     pt = Variable('pt',self.ttype)
     self.apply = self.declare(so3.apply(T[0],pt)+T[1],"apply",['T','pt'])
     #self.apply.setDeriv(0,lambda T,pt,dT:array(so3.apply(dT[0],pt),dT[1]))
     #self.apply.setDeriv(1,lambda T,pt,dx:so3.apply(T[0],dx))
     self.apply.addSimplifier([None,'zero'],lambda T,pt:T[1])
     self.apply.addSimplifier(['se3.identity',None],lambda T,pt:pt)
     self.apply.autoSetJacobians()
     self.rotation = self.declare(T[0],"rotation",['T'])
     self.translation = self.declare(T[1],"translation",['T'])
     self.make.returnType = self.type
     self.homogeneous.returnType = Type('M',(4,4))
     self.homogeneous.argTypes = [self.type]
     self.homogeneous.setDeriv(0,lambda T,dT:array([[dT[0][0],dT[0][3],dT[0][6],dT[1][0]],
         [dT[0][1],dT[0][4],dT[0][7],dT[1][1]],
         [dT[0][2],dT[0][5],dT[0][8],dT[1][2]],
         [0.,0.,0.,0.]]),asExpr=True)
     M = Variable("M",Type('M',(4,4)))
     self.from_homogeneous = self.declare(array(array(M[0,0],M[1,0],M[2,0],M[0,1],M[1,1],M[2,1],M[0,2],M[1,2],M[2,2]),array(M[0,3],M[1,3],M[2,3])),'from_homogeneous',['M'])
     self.from_homogeneous.autoSetJacobians()
     self.matrix = self.declare(self.homogeneous(T),"matrix",['T'])
     self.make.autoSetJacobians()
     self.matrix.autoSetJacobians()
     self.rotation.autoSetJacobians()
     self.translation.autoSetJacobians()
     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 = self.ttype
     self.apply.argTypes = [self.type,self.ttype]
     self.rotation.returnType = self.Rtype
     self.translation.returnType = self.ttype
Exemple #10
0
 def motionfunc(self,x,y,dx,dy):
     if self.dragging:
         if self.modifiers & GLUT_ACTIVE_CTRL:
             R,t = self.camera.matrix()
             delta = so3.apply(so3.inv(R),[float(dx)*self.camera.dist/self.width,-float(dy)*self.camera.dist/self.width,0])
             self.camera.tgt = vectorops.add(self.camera.tgt,delta)
         elif self.modifiers & GLUT_ACTIVE_SHIFT:
             self.camera.dist *= math.exp(dy*0.01)
         else:
             self.camera.rot[2] += float(dx)*0.01
             self.camera.rot[1] += float(dy)*0.01
     self.refresh()
Exemple #11
0
 def motionfunc(self,x,y,dx,dy):
     if self.dragging:
         if self.modifiers & GLUT_ACTIVE_CTRL:
             R,t = self.camera.matrix()
             delta = so3.apply(so3.inv(R),[float(dx)*self.camera.dist/self.width,-float(dy)*self.camera.dist/self.width,0])
             self.camera.tgt = vectorops.add(self.camera.tgt,delta)
         elif self.modifiers & GLUT_ACTIVE_SHIFT:
             self.camera.dist *= math.exp(dy*0.01)
         else:
             self.camera.rot[2] += float(dx)*0.01
             self.camera.rot[1] += float(dy)*0.01
     self.refresh()
def orientation_matrix(axis1, axis2, axis3):
    """Returns the matrix that maps world axes 1,2,3 to the
    camera's coordinate system (left,down,forward) (assuming no camera motion).
    
    Each axis can be either a 3-tuple or any element of
    ['x','y','z','-x','-y','-z']"""
    if isinstance(axis1, str):
        axis1 = basis_vectors[axis1]
    if isinstance(axis2, str):
        axis2 = basis_vectors[axis2]
    if isinstance(axis3, str):
        axis3 = basis_vectors[axis3]
    return so3.inv(so3.from_matrix([axis1, axis2, axis3]))
def orientation_matrix(axis1,axis2,axis3):
    """Returns the matrix that maps world axes 1,2,3 to the
    camera's coordinate system (left,down,forward) (assuming no camera motion).
    
    Each axis can be either a 3-tuple or any element of
    ['x','y','z','-x','-y','-z']"""
    if isinstance(axis1,str):
        axis1 = basis_vectors[axis1]
    if isinstance(axis2,str):
        axis2 = basis_vectors[axis2]
    if isinstance(axis3,str):
        axis3 = basis_vectors[axis3]
    return so3.inv(so3.from_matrix([axis1,axis2,axis3]))
Exemple #14
0
 def motionfunc(self,x,y):
     dx = x - self.lastx
     dy = y - self.lasty
     if self.modifiers & GLUT_ACTIVE_CTRL:
         R,t = self.camera.matrix()
         delta = so3.apply(so3.inv(R),[float(dx)*self.camera.dist/self.width,-float(dy)*self.camera.dist/self.width,0])
         self.camera.tgt = vectorops.add(self.camera.tgt,delta)
     elif self.modifiers & GLUT_ACTIVE_SHIFT:
         self.camera.dist *= math.exp(dy*0.01)
     else:
         self.camera.rot[2] += float(dx)*0.01
         self.camera.rot[1] += float(dy)*0.01        
     self.lastx = x
     self.lasty = y
     glutPostRedisplay()
Exemple #15
0
 def __init__(self):
     Context.__init__(self)
     self.Rtype = Type('V', 9)
     self.ttype = Type('V', 3)
     self.type = Type('L', 2, [self.Rtype, self.ttype])
     T = Variable("T", self.type)
     R = Variable("R", self.Rtype)
     t = Variable("t", self.ttype)
     self.make = self.declare(array(R, t), "make", ['R', 't'])
     self.identity = self.declare(se3.identity, "identity")
     self.homogeneous = self.declare(se3.homogeneous, "homogeneous")
     self.homogeneous.addSimplifier(['se3.identity'], lambda T: eye(4))
     Rinv = so3.inv(T[0])
     self.inv = self.declare(array(Rinv, neg(so3.apply(Rinv, T[1]))), "inv",
                             ['T'])
     self.inv.autoSetJacobians()
     self.inv.properties['inverse'] = weakref.proxy(self.inv)
     self.inv.addSimplifier(['se3.identity'], lambda T: T)
     self.mul = self.declare(se3.mul, "mul")
     self.mul.setDeriv(0,
                       lambda T1, T2, dT1: self.mul(dT1, T2),
                       asExpr=True)
     self.mul.setDeriv(1,
                       lambda T1, T2, dT2: self.mul(T1, dT2),
                       asExpr=True)
     self.mul.addSimplifier(['se3.identity', None], (lambda T1, T2: T2),
                            pre=True)
     self.mul.addSimplifier([None, 'se3.identity'], (lambda T1, T2: T1),
                            pre=True)
     self.mul.properties['associative'] = True
     pt = Variable('pt', self.ttype)
     self.apply = self.declare(
         so3.apply(T[0], pt) + T[1], "apply", ['T', 'pt'])
     #self.apply.setDeriv(0,lambda T,pt,dT:array(so3.apply(dT[0],pt),dT[1]))
     #self.apply.setDeriv(1,lambda T,pt,dx:so3.apply(T[0],dx))
     self.apply.addSimplifier([None, 'zero'], lambda T, pt: T[1])
     self.apply.addSimplifier(['se3.identity', None], lambda T, pt: pt)
     self.apply.autoSetJacobians()
     self.rotation = self.declare(T[0], "rotation", ['T'])
     self.translation = self.declare(T[1], "translation", ['T'])
     self.make.returnType = self.type
     self.homogeneous.returnType = Type('M', (4, 4))
     self.homogeneous.argTypes = [self.type]
     self.homogeneous.setDeriv(
         0,
         lambda T, dT: array([[dT[0][0], dT[0][3], dT[0][6], dT[1][0]],
                              [dT[0][1], dT[0][4], dT[0][7], dT[1][1]],
                              [dT[0][2], dT[0][5], dT[0][8], dT[1][2]],
                              [0., 0., 0., 0.]]),
         asExpr=True)
     M = Variable("M", Type('M', (4, 4)))
     self.from_homogeneous = self.declare(
         array(
             array(M[0, 0], M[1, 0], M[2, 0], M[0, 1], M[1, 1], M[2, 1],
                   M[0, 2], M[1, 2], M[2, 2]),
             array(M[0, 3], M[1, 3], M[2, 3])), 'from_homogeneous', ['M'])
     self.from_homogeneous.autoSetJacobians()
     self.matrix = self.declare(self.homogeneous(T), "matrix", ['T'])
     self.make.autoSetJacobians()
     self.matrix.autoSetJacobians()
     self.rotation.autoSetJacobians()
     self.translation.autoSetJacobians()
     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 = self.ttype
     self.apply.argTypes = [self.type, self.ttype]
     self.rotation.returnType = self.Rtype
     self.translation.returnType = self.ttype
Exemple #16
0
def readSo3(text):
    """Reads an so3 element, i.e., rotation matrix, from string in the same
    format as written to by Klampt C++ bindings (row major)."""
    items = text.split()
    if len(items) != 9: raise ValueError("Invalid element of SO3, must have 9 elements")
    return so3.inv([float(v) for v in items])
Exemple #17
0
 def directionFromWorld(self, worldCoordinates=[0, 0, 0], frame='world'):
     """Alias for to(direction(worldCoordinates,'root'),frame)"""
     f = self.frame(frame)
     local = so3.apply(so3.inv(f._worldCoordinates[0]), worldCoordinates)
     return Direction(local, f)
Exemple #18
0
 def rotationCoordinates(self):
     """Returns the SO(3) coordinates that rotate elements from the source
     to the destination Frame"""
     if self._destination==None:
         return self._source.worldRotation()
     return so3.mul(so3.inv(self._destination.worldRotation()),self._source.worldRotation())
Exemple #19
0
 def directionFromWorld(self,worldCoordinates=[0,0,0],frame='world'):
     """Alias for to(direction(worldCoordinates,'root'),frame)"""
     f = self.frame(frame)
     local = so3.apply(so3.inv(f._worldCoordinates[0]),worldCoordinates)
     return Direction(local,f)
Exemple #20
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]
Exemple #21
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]