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
