def _so3_rotation(axis, angle): """Symbolic version of so3.rotation""" cm = cos(angle) sm = sin(angle) #m = s[r]-c[r][r]+rrt = s[r]-c(rrt-I)+rrt = cI + rrt(1-c) + s[r] cp = so3.cross_product(axis) R = mul(cp, sm) R2 = [0] * 9 for i in xrange(3): for j in xrange(3): R2[i * 3 + j] = axis[i] * axis[j] * (1. - cm) R2[0] += cm R2[4] += cm R2[8] += cm return R + expr(R2)
def _so3_rotation(axis,angle): """Symbolic version of so3.rotation""" cm = cos(angle) sm = sin(angle) #m = s[r]-c[r][r]+rrt = s[r]-c(rrt-I)+rrt = cI + rrt(1-c) + s[r] cp = so3.cross_product(axis) R = mul(cp,sm) R2 = [0]*9 for i in xrange(3): for j in xrange(3): R2[i*3+j] = axis[i]*axis[j]*(1.-cm) R2[0] += cm R2[4] += cm R2[8] += cm return R + expr(R2)
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 difference(self,a,b): w = se3.error(a,b) return so3.cross_product(w[:3])+w[3:]
def difference(self,a,b): w = so3.error(a,b) return so3.cross_product(w)
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 difference(self, a, b): w = se3.error(a, b) return so3.cross_product(w[:3]) + w[3:]
def difference(self, a, b): w = so3.error(a, b) return so3.cross_product(w)