def worldPosJacobian_localPos(robot,link,localPos): if not isinstance(link,RobotModelLink): link = robot.link(link) assert link.index >= 0 return so3.matrix(link.getTransform()[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 worldPosJacobian_localPos(robot, link, localPos): if not isinstance(link, RobotModelLink): link = robot.link(link) assert link.index >= 0 return so3.matrix(link.getTransform()[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 writeSo3(x): """Writes an so3 element, i.e., rotation matrix, to string in the same format as written to by Klampt C++ bindings (row major).""" assert len(x)==9,"Argument must be an so3 element" return '\t'.join([' '.join([str(mij) for mij in mi ]) for mi in so3.matrix(x)])
def readMatrix3(text): """Reads a 3x3 matrix from a string""" return so3.matrix(readSo3(text))
def rotation(T): """Returns the 3x3 rotation matrix corresponding to T's rotation""" (R, t) = T return so3.matrix(R)
def rotation(T): """Returns the 3x3 rotation matrix corresponding to T's rotation""" (R,t) = T return so3.matrix(R)
def euler_from_expmap(axis_angle_parameters, axes="sxyz"): axis_angle = (axis_angle_parameters[0:3], axis_angle_parameters[3]) R = so3.from_axis_angle(axis_angle) matrix = so3.matrix(R) x, y, z = transformations.euler_from_matrix(matrix, axes) return x, y, z