"""Operations for rigid rotations in Klampt. All rotations are
represented by a 9-list specifying the entries of the rotation matrix
in column major form.
In other words, given a 3x3 matrix::
[a11,a12,a13]
[a21,a22,a23]
[a31,a32,a33],
Klamp't represents the matrix as a list [a11,a21,a31,a12,a22,a32,a13,a23,a33].
The reasons for this representation are 1) simplicity, and 2) a more
convenient interface with C code.
"""
import math
from . import vectorops
from typing import Tuple,Callable
from ..model.typing import Rotation,Matrix3,Vector3
def __str__(R : Rotation) -> str:
"""Converts a rotation to a string."""
return '\n'.join([' '.join([str(ri) for ri in r]) for r in matrix(R)])
[docs]def identity() -> Rotation:
"""Returns the identity rotation."""
return [1.,0.,0.,0.,1.,0.,0.,0.,1.]
[docs]def inv(R : Rotation) -> Rotation:
"""Inverts the rotation."""
Rinv = [R[0],R[3],R[6],R[1],R[4],R[7],R[2],R[5],R[8]]
return Rinv
[docs]def apply(R : Rotation, point : Vector3) -> Vector3:
"""Applies the rotation to a point."""
return (R[0]*point[0]+R[3]*point[1]+R[6]*point[2],
R[1]*point[0]+R[4]*point[1]+R[7]*point[2],
R[2]*point[0]+R[5]*point[1]+R[8]*point[2])
[docs]def matrix(R : Rotation) -> Matrix3:
"""Returns the 3x3 rotation matrix corresponding to R."""
return [[R[0],R[3],R[6]],
[R[1],R[4],R[7]],
[R[2],R[5],R[8]]]
[docs]def from_matrix(mat : Matrix3) -> Rotation:
"""Returns a rotation R corresponding to the 3x3 rotation matrix mat."""
R = [mat[0][0],mat[1][0],mat[2][0],mat[0][1],mat[1][1],mat[2][1],mat[0][2],mat[1][2],mat[2][2]]
return R
[docs]def ndarray(R : Rotation) -> "ndarray":
"""Returns the 3x3 numpy rotation matrix corresponding to R."""
import numpy
return numpy.array(matrix(R))
[docs]def from_ndarray(mat : "ndarray") -> Rotation:
"""Returns a rotation R corresponding to the 3x3 rotation matrix mat."""
return mat.T.flatten().tolist()
[docs]def mul(R1 : Rotation, R2 : Rotation) -> Rotation:
"""Multiplies two rotations."""
if len(R1) != 9: raise ValueError("R1 is not a rotation matrix")
if len(R2) != 9: raise ValueError("R2 is not a rotation matrix (did you mean to use apply())?")
m1=matrix(R1)
m2T=matrix(inv(R2))
mres = matrix(identity())
for i in range(3):
for j in range(3):
mres[i][j] = vectorops.dot(m1[i],m2T[j])
R = from_matrix(mres)
return R
[docs]def trace(R : Rotation) -> float:
"""Computes the trace of the rotation matrix."""
return R[0]+R[4]+R[8]
[docs]def angle(R : Rotation) -> float:
"""Returns absolute deviation of R from identity"""
ctheta = (trace(R) - 1.0)*0.5
return math.acos(max(min(ctheta,1.0),-1.0))
[docs]def rpy(R : Rotation) -> Vector3:
"""Converts a rotation matrix to a roll,pitch,yaw angle triple.
The result is given in radians."""
sign = lambda x: 1 if x > 0 else (-1 if x < 0 else 0)
m = matrix(R)
_sb = min(1.0, max(m[2][0],-1.0))
b = -math.asin(_sb) # m(2,0)=-sb
cb = math.cos(b)
if abs(cb) > 1e-7:
ca = m[0][0]/cb #m(0,0)=ca*cb
ca = min(1.0,max(ca,-1.0))
if sign(m[1][0]) == sign(cb): #m(1,0)=sa*cb
a = math.acos(ca);
else:
a = 2*math.pi - math.acos(ca)
cc = m[2][2] / cb #m(2,2)=cb*cc
cc = min(1.0,max(cc,-1.0))
if sign(m[2][1]) == sign(cb): #m(2,1)=cb*sc
c = math.acos(cc)
else:
c = math.pi*2 - math.acos(cc)
else:
#b is close to 90 degrees, i.e. cb=0
#this reduces the degrees of freedom, so we can set c=0
c = 0
#m(0,1)=-sa
_sa = min(1.0, max(m[0][1],-1.0))
a = -math.asin(_sa);
if sign(math.cos(a)) != sign(m[1][1]): #m(1,1)=ca
a = math.pi - a;
return c,b,a
[docs]def from_rpy(rollpitchyaw : Vector3) -> Rotation:
"""Converts from roll,pitch,yaw angle triple to a rotation
matrix. The triple is given in radians. The x axis is "roll",
y is "pitch", and z is "yaw".
"""
roll,pitch,yaw = rollpitchyaw
Rx,Ry,Rz = from_axis_angle(((1,0,0),roll)),from_axis_angle(((0,1,0),pitch)),from_axis_angle(((0,0,1),yaw))
return mul(Rz,mul(Ry,Rx))
[docs]def rotation_vector(R : Rotation) -> Vector3:
"""Returns the rotation vector w (exponential map) representation of R such
that e^[w] = R. Equivalent to axis-angle representation with
w/||w||=axis, ||w||=angle."""
theta = angle(R)
if abs(theta-math.pi)<0.5:
#for values close to pi this alternate technique has better numerical
#performance
c = math.cos(theta)
x2=(R[0]-c)/(1.0 - c)
y2=(R[4]-c)/(1.0 - c)
z2=(R[8]-c)/(1.0 - c)
if x2 < 0:
assert(x2>-1e-5)
x2=0
if y2 < 0:
assert(y2>-1e-5)
y2=0
if z2 < 0:
assert(z2>-1e-5)
z2=0
x = theta*math.sqrt(x2)
y = theta*math.sqrt(y2)
z = theta*math.sqrt(z2)
if abs(theta-math.pi) < 1e-5:
#determined up to sign changes, we know r12=2xy,r13=2xz,r23=2yz
xy=R[3]
xz=R[6]
yz=R[7]
if(x > y):
if(x > z):
#x is largest
if(xy < 0): y=-y
if(xz < 0): z=-z
else:
#z is largest
if(yz < 0): y=-y
if(xz < 0): x=-x
else:
if(y > z):
#y is largest
if(xy < 0): x=-x
if(yz < 0): z=-z
else:
#z is largest
if(yz < 0): y=-y
if(xz < 0): x=-x
else:
#alternate technique: use sign of anti-cross product
eps = theta-math.pi
if eps*(R[3+2]-R[6+1]) > 0:
x = -x
if eps*(R[6+0]-R[0+2]) > 0:
y = -y
if eps*(R[0+1]-R[3+0]) > 0:
z = -z
return [x,y,z]
#normal
scale = 1
if abs(theta) > 1e-5:
scale = theta/math.sin(theta)
return vectorops.mul(deskew(R),scale)
[docs]def axis_angle(R : Rotation) -> Tuple:
"""Returns the (axis,angle) pair representing R"""
m = rotation_vector(R)
return (vectorops.unit(m),vectorops.norm(m))
[docs]def from_axis_angle(aa : Tuple) -> Rotation:
"""Converts an axis-angle representation (axis,angle) to a 3D rotation
matrix."""
return rotation(aa[0],aa[1])
[docs]def from_rotation_vector(w : Vector3) -> Rotation:
"""Converts a rotation vector representation w to a 3D rotation matrix."""
length = vectorops.norm(w)
if length < 1e-7: return identity()
return rotation(vectorops.mul(w,1.0/length),length)
#aliases for rotation_vector and from_rotation_vector
moment = rotation_vector
from_moment = from_rotation_vector
[docs]def from_quaternion(q : Tuple) -> Rotation:
"""Given a unit quaternion (w,x,y,z), produce the corresponding rotation
matrix."""
w,x,y,z = q
x2 = x + x; y2 = y + y; z2 = z + z;
xx = x * x2; xy = x * y2; xz = x * z2;
yy = y * y2; yz = y * z2; zz = z * z2;
wx = w * x2; wy = w * y2; wz = w * z2;
a11 = 1.0 - (yy + zz)
a12 = xy - wz
a13 = xz + wy
a21 = xy + wz
a22 = 1.0 - (xx + zz)
a23 = yz - wx
a31 = xz - wy
a32 = yz + wx
a33 = 1.0 - (xx + yy)
return [a11,a21,a31,a12,a22,a32,a13,a23,a33]
[docs]def quaternion(R : Rotation) -> Tuple:
"""Given a Klamp't rotation representation, produces the corresponding
unit quaternion (w,x,y,z)."""
tr = trace(R) + 1.0;
a11,a21,a31,a12,a22,a32,a13,a23,a33 = R
#If the trace is nonzero, it's a nondegenerate rotation
if tr > 1e-5:
s = math.sqrt(tr)
w = s * 0.5
s = 0.5 / s
x = (a32 - a23) * s
y = (a13 - a31) * s
z = (a21 - a12) * s
return vectorops.unit((w,x,y,z))
else:
#degenerate it's a rotation of 180 degrees
nxt = [1, 2, 0]
#check for largest diagonal entry
i = 0
if a22 > a11: i = 1
if a33 > max(a11,a22): i = 2
j = nxt[i]
k = nxt[j]
M = matrix(R)
q = [0.0]*4
s = math.sqrt((M[i][i] - (M[j][j] + M[k][k])) + 1.0);
q[i] = s * 0.5
if abs(s)<1e-7:
raise ValueError("Could not solve for quaternion... Invalid rotation matrix?")
else:
s = 0.5 / s;
q[3] = (M[k][j] - M[j][k]) * s;
q[j] = (M[i][j] + M[j][i]) * s;
q[k] = (M[i][k] + M[k][i]) * s;
w,x,y,z = q[3],q[0],q[1],q[2]
return vectorops.unit([w,x,y,z])
[docs]def distance(R1 : Rotation, R2 : Rotation) -> float:
"""Returns the absolute angle one would need to rotate in order to get
from R1 to R2"""
R = mul(R1,inv(R2))
return angle(R)
[docs]def error(R1 : Rotation, R2 : Rotation) -> float:
"""Returns a 3D "difference vector" that describes how far R1 is from R2.
More precisely, this is the (local) Lie derivative, which is the rotation
vector representation of R1*R2^T.
Fun fact: the error w=error(R1,R2) is related to the derivative of
interpolate(R2,R1,u) at u=0 by
d/du interpolate(R2,R1,0) = mul(cross_product(w),R2).
You can also recover R1 from w via R1 = mul(from_moment(w),R2).
"""
R = mul(R1,inv(R2))
return moment(R)
[docs]def cross_product(w : Vector3) -> Rotation:
"""Returns the cross product matrix associated with w.
The matrix [w]R is the derivative of the matrix R as it rotates about
the axis w/||w|| with angular velocity ||w||.
"""
return [0.,w[2],-w[1], -w[2],0.,w[0], w[1],-w[0],0.]
[docs]def diag(R : Rotation) -> Vector3:
"""Returns the diagonal of the 3x3 matrix reprsenting the so3 element R."""
return [R[0],R[4],R[8]]
[docs]def deskew(R : Rotation) -> Vector3:
"""If R is a (flattened) cross-product matrix of the 3-vector w, this will
return w. Otherwise, it will return a representation w of (R-R^T)/2 (off
diagonals of R) such that (R-R^T)/2 = cross_product(w). """
return [0.5*(R[5]-R[7]),0.5*(R[6]-R[2]),0.5*(R[1]-R[3])]
[docs]def rotation(axis : Vector3, angle: float) -> Rotation:
"""Given a unit axis and an angle in radians, returns the rotation
matrix."""
cm = math.cos(angle)
sm = math.sin(angle)
#m = s[r]-c[r][r]+rrt = s[r]-c(rrt-I)+rrt = cI + rrt(1-c) + s[r]
R = vectorops.mul(cross_product(axis),sm)
for i in range(3):
for j in range(3):
R[i*3+j] += axis[i]*axis[j]*(1.-cm)
R[0] += cm
R[4] += cm
R[8] += cm
return R
[docs]def canonical(v : Vector3) -> Rotation:
"""Given a unit vector v, finds R that defines a basis [x,y,z] such that
x = v and y and z are orthogonal"""
if abs(vectorops.normSquared(v) - 1.0) > 1e-4:
raise RuntimeError("Nonunit vector supplied to canonical()")
assert(len(v)==3)
if abs(v[0]-1.0) < 1e-5:
return identity()
elif abs(v[0]+1.0) < 1e-5:
#flip of basis
R = identity()
R[0] = -1.0
R[4] = -1.0
return R
R = list(v) + [0.]*6
x,y,z = v
scale = 1.0/(1.0+x);
R[3]= -y;
R[4]= x + scale*z*z;
R[5]= -scale*y*z;
R[6]= -z;
R[7]= -scale*y*z;
R[8]= x + scale*y*y;
return R
[docs]def align(a : Vector3, b : Vector3) -> Rotation:
"""Returns a minimal-angle rotation that aligns the vector a to align with
the vector b. Both a and b must be nonzero."""
an = vectorops.norm(a)
bn = vectorops.norm(b)
if abs(an) < 1e-5 or abs(bn) < 1e-5:
return identity()
a = vectorops.mul(a,1.0/an)
b = vectorops.mul(b,1.0/bn)
v = vectorops.cross(a,b)
c = vectorops.dot(a,b)
if abs(c+1)<1e-5: #rotation of pi
v = vectorops.cross(a,[0,0,1])
vn = vectorops.norm(v)
if vn < 1e-5:
v = vectorops.cross(a,[0,1,0])
vn = vectorops.norm(v)
return rotation(vectorops.mul(v,1.0/vn),math.pi)
vhat = cross_product(v)
vhat2 = mul(vhat,vhat)
return vectorops.madd(vectorops.add(identity(),vhat),vhat2,1.0/(1.0+c))
[docs]def interpolate(R1 : Rotation, R2 : Rotation, u : float) -> Rotation:
"""Interpolate linearly between the two rotations R1 and R2. """
R = mul(inv(R1),R2)
m = moment(R)
angle = vectorops.norm(m)
if angle==0: return R1
axis = vectorops.div(m,angle)
return mul(R1,rotation(axis,angle*u))
[docs]def interpolator(R1 : Rotation, R2 : Rotation) -> Callable:
"""Returns a function of one parameter u that interpolates linearly
between the two rotations R1 and R2. After f(u) is constructed, calling
f(u) is about 2x faster than calling interpolate(R1,R2,u)."""
R = mul(inv(R1),R2)
m = moment(R)
angle = vectorops.norm(m)
if angle==0:
axis = [1,0,0]
else:
axis = vectorops.div(m,angle)
def f(u,R1=R1,axis=axis,angle=angle):
return mul(R1,rotation(axis,angle*u))
return f
[docs]def det(R : Rotation) -> float:
"""Returns the determinant of the 3x3 matrix R"""
m = matrix(R)
return m[0][0]*m[1][1]*m[2][2]+m[0][1]*m[1][2]*m[2][0]+m[0][2]*m[1][0]*m[2][1]-m[0][0]*m[1][2]*m[2][1]-m[0][1]*m[1][0]*m[2][2]-m[0][2]*m[1][1]*m[2][0]
[docs]def is_rotation(R : Rotation, tol=1e-5) -> bool:
"""Returns true if R is a rotation matrix, i.e. is orthogonal to the given tolerance and has + determinant"""
RRt = mul(R,inv(R))
err = vectorops.sub(RRt,identity())
if any(abs(v) > tol for v in err):
return False
if det(R) < 0:
return False
return True
[docs]def sample() -> Rotation:
"""Returns a uniformly distributed rotation matrix."""
import random
q = [random.gauss(0,1),random.gauss(0,1),random.gauss(0,1),random.gauss(0,1)]
q = vectorops.unit(q)
theta = math.acos(q[3])*2.0
if abs(theta) < 1e-8:
m = [0,0,0]
else:
m = vectorops.mul(vectorops.unit(q[0:3]),theta)
return from_moment(m)