【现代机器人学】学习笔记十三：配套代码解析
最近一直忙于工作，每天都在写一些业务代码。而目前工程中的技术栈并没有使用旋量这一套机器人理论系统，因此时间长了自己都忘记了。
于是决定把这本书配套的代码内容也过一遍，查漏补缺，把这本书的笔记内容完结一下。
代码来源于github：https://github.com/NxRLab/ModernRobotics
其中Python部分，相关的函数一共有47个，其实也不是很多。
读者朋友们可以配合我的专栏地址一起学习：
【现代机器人学】学习笔记
下面开始：
刚体变换部分：
NearZero：判断是否接近0
def NearZero(z):
"""Determines whether a scalar is small enough to be treated as zero
:param z: A scalar input to check
:return: True if z is close to zero, false otherwise
Example Input:
z = -1e-7
Output:
True
"""
return abs(z) < 1e-6这个函数是判断输入的标量是否接近于0，比较简单。
当然，如果输入是向量，则会返回一个装有true或false的列表，指明各项是否接近0。
Normalize：向量归一化
def Normalize(V):
"""Normalizes a vector
:param V: A vector
:return: A unit vector pointing in the same direction as z
Example Input:
V = np.array([1, 2, 3])
Output:
np.array([0.26726124, 0.53452248, 0.80178373])
"""
return V / np.linalg.norm(V)这个函数的作用是对向量进行归一化，即方向不变，除以模长。而np.linalg.norm()用于求范数，linalg本意为linear(线性) + algebra(代数)，norm则表示范数。默认是2范数，即元素之和开平方。
RotInv：旋转矩阵求逆
def RotInv(R):
"""Inverts a rotation matrix
:param R: A rotation matrix
:return: The inverse of R
Example Input:
R = np.array([[0, 0, 1],
[1, 0, 0],
[0, 1, 0]])
Output:
np.array([[0, 1, 0],
[0, 0, 1],
[1, 0, 0]])
"""
return np.array(R).T求旋转矩阵的逆矩阵。我们知道旋转矩阵的特性是，其逆矩阵与转置矩阵相同。因此求逆速度可以大幅度加快。
VecToso3：向量到so3
def VecToso3(omg):
"""Converts a 3-vector to an so(3) representation
:param omg: A 3-vector
:return: The skew symmetric representation of omg
Example Input:
omg = np.array([1, 2, 3])
Output:
np.array([[ 0, -3, 2],
[ 3, 0, -1],
[-2, 1, 0]])
"""
return np.array([[0, -omg[2], omg[1]],
[omg[2], 0, -omg[0]],
[-omg[1], omg[0], 0]])这个函数对应公式：
即把一个向量转化为so3的矩阵表示。我们知道旋转矩阵的表示则是：
即通过轴角可转化为旋转矩阵。这个之后介绍。
与此相反，
so3ToVec：so3到向量
def so3ToVec(so3mat):
"""Converts an so(3) representation to a 3-vector
:param so3mat: A 3x3 skew-symmetric matrix
:return: The 3-vector corresponding to so3mat
Example Input:
so3mat = np.array([[ 0, -3, 2],
[ 3, 0, -1],
[-2, 1, 0]])
Output:
np.array([1, 2, 3])
"""
return np.array([so3mat[2][1], so3mat[0][2], so3mat[1][0]])这个函数的作用和VecToso3刚好相反，是把[w]转化为向量表示。
AxisAng3：旋转三维矢量到轴角
def AxisAng3(expc3):
"""Converts a 3-vector of exponential coordinates for rotation into
axis-angle form
:param expc3: A 3-vector of exponential coordinates for rotation
:return omghat: A unit rotation axis
:return theta: The corresponding rotation angle
Example Input:
expc3 = np.array([1, 2, 3])
Output:
(np.array([0.26726124, 0.53452248, 0.80178373]), 3.7416573867739413)
"""
return (Normalize(expc3), np.linalg.norm(expc3))这是一个工具函数，我们可以看到：其作用是：将用于旋转的指数坐标的3维矢量转换为轴角度形式。即返回的第一个数是归一化的方向，第二维是角度。
MatrixExp3：矩阵指数积，so3到旋转矩阵
def MatrixExp3(so3mat):
"""Computes the matrix exponential of a matrix in so(3)
:param so3mat: A 3x3 skew-symmetric matrix
:return: The matrix exponential of so3mat
Example Input:
so3mat = np.array([[ 0, -3, 2],
[ 3, 0, -1],
[-2, 1, 0]])
Output:
np.array([[-0.69492056, 0.71352099, 0.08929286],
[-0.19200697, -0.30378504, 0.93319235],
[ 0.69297817, 0.6313497 , 0.34810748]])
"""
omgtheta = so3ToVec(so3mat)
if NearZero(np.linalg.norm(omgtheta)):
return np.eye(3)
else:
theta = AxisAng3(omgtheta)[1]
omgmat = so3mat / theta
return np.eye(3) + np.sin(theta) * omgmat \
+ (1 - np.cos(theta)) * np.dot(omgmat, omgmat)
从这个函数开始，代码量开始增大，它的作用是：
输入一个反对称矩阵，把它转化为一个旋转矩阵。即我上面在VecToso3部分提到的公式：
即比较著名的罗德里格斯公式。（这个公式友好多种表达形式）
我们可以看到在实现中，我们首先调用了前面提到的so3ToVec，把反对称矩阵中的向量提出来。
在这里我们需要对它做一个安全性的校验：如果它的模长接近于0，即直接返回单位矩阵。否则要计算它的角度。
omgmat = so3mat / theta
我们可以看到有一步这样的操作：首先这个操作是要把[w]给变成标准的归一化反对称矩阵，然后再套用上面的公式来计算旋转矩阵。当角度是0的时候就不能除以了，因此做了一个if 和 else的检查判断。
MatrixLog3：矩阵对数，旋转矩阵到so3
def MatrixLog3(R):
"""Computes the matrix logarithm of a rotation matrix
:param R: A 3x3 rotation matrix
:return: The matrix logarithm of R
Example Input:
R = np.array([[0, 0, 1],
[1, 0, 0],
[0, 1, 0]])
Output:
np.array([[ 0, -1.20919958, 1.20919958],
[ 1.20919958, 0, -1.20919958],
[-1.20919958, 1.20919958, 0]])
"""
acosinput = (np.trace(R) - 1) / 2.0
if acosinput >= 1:
return np.zeros((3, 3))
elif acosinput <= -1:
if not NearZero(1 + R[2][2]):
omg = (1.0 / np.sqrt(2 * (1 + R[2][2]))) \
* np.array([R[0][2], R[1][2], 1 + R[2][2]])
elif not NearZero(1 + R[1][1]):
omg = (1.0 / np.sqrt(2 * (1 + R[1][1]))) \
* np.array([R[0][1], 1 + R[1][1], R[2][1]])
else:
omg = (1.0 / np.sqrt(2 * (1 + R[0][0]))) \
* np.array([1 + R[0][0], R[1][0], R[2][0]])
return VecToso3(np.pi * omg)
else:
theta = np.arccos(acosinput)
return theta / 2.0 / np.sin(theta) * (R - np.array(R).T)这个函数看起来比较复杂了，那么它是要干啥呢？
它会把一个输入的旋转矩阵，转化为so3的矩阵形式。
我们可以分析下代码是如何实现的：
我们首先计算了
在这里代码首先判断：
if acosinput >= 1:
return np.zeros((3, 3))
那这就说明R是一个单位阵（对角线元素都大于等于3了） ，那就返回一个零矩阵即可，模长为0，方向任意。
然后再判断：
elif acosinput <= -1:
即为上图的b的形式：
if not NearZero(1 + R[2][2]):
omg = (1.0 / np.sqrt(2 * (1 + R[2][2]))) \
* np.array([R[0][2], R[1][2], 1 + R[2][2]])
对应
注意这里公式的下标是从1开始，而代码的下标是从0开始。
elif not NearZero(1 + R[1][1]):
omg = (1.0 / np.sqrt(2 * (1 + R[1][1]))) \
* np.array([R[0][1], 1 + R[1][1], R[2][1]])
else:
omg = (1.0 / np.sqrt(2 * (1 + R[0][0]))) \
* np.array([1 + R[0][0], R[1][0], R[2][0]])
注意以上三种算出的都是w向量。根据函数要求，应该返回一个矩阵形式的内容，因此还需要调用一个VecToso3来实现：
return VecToso3(np.pi * omg)
需要谨记的是：
这种情况下，只求一个w是不对的， 因为根据函数的意思是，要把R送入，得到so3，所以里面要有角度相关的变量。这种情况下，theta为pi。因此传入VecToso3的实参是np.pi * omg而不是一个单纯的omg。
最后就是常规情况：
else:
theta = np.arccos(acosinput)
return theta / 2.0 / np.sin(theta) * (R - np.array(R).T)
先求出theta，然后直接根据函数内容得到so3进行返回就可以了。
RpToTrans：旋转平移构造齐次矩阵
def RpToTrans(R, p):
"""Converts a rotation matrix and a position vector into homogeneous
transformation matrix
:param R: A 3x3 rotation matrix
:param p: A 3-vector
:return: A homogeneous transformation matrix corresponding to the inputs
Example Input:
R = np.array([[1, 0, 0],
[0, 0, -1],
[0, 1, 0]])
p = np.array([1, 2, 5])
Output:
np.array([[1, 0, 0, 1],
[0, 0, -1, 2],
[0, 1, 0, 5],
[0, 0, 0, 1]])
"""
return np.r_[np.c_[R, p], [[0, 0, 0, 1]]]这个函数本身没有什么好说，但是发现它的实现方式有些好玩，用到了np.r_和np.c_：
numpy.r_: 将slice对象沿第一轴进行连接（上下拼接）
我们可以看到，在这里，np.r_的括号里分成了两份：
一份是np.c_[R, p],
一份是 [[0, 0, 0, 1]]
那么这俩二维数组，沿第一轴进行连接，即沿着x轴进行拼接。
numpy.c：将slice对象沿第二轴进行连接（左右拼接）
np.c_[R, p]即把R和p左右拼起来。
TransToRp：齐次矩阵拆分旋转平移
def TransToRp(T):
"""Converts a homogeneous transformation matrix into a rotation matrix
and position vector
:param T: A homogeneous transformation matrix
:return R: The corresponding rotation matrix,
:return p: The corresponding position vector.
Example Input:
T = np.array([[1, 0, 0, 0],
[0, 0, -1, 0],
[0, 1, 0, 3],
[0, 0, 0, 1]])
Output:
(np.array([[1, 0, 0],
[0, 0, -1],
[0, 1, 0]]),
np.array([0, 0, 3]))
"""
T = np.array(T)
return T[0: 3, 0: 3], T[0: 3, 3]用到了切片，入门知识，没什么好说的。
TransInv：齐次矩阵的逆矩阵
def TransInv(T):
"""Inverts a homogeneous transformation matrix
:param T: A homogeneous transformation matrix
:return: The inverse of T
Uses the structure of transformation matrices to avoid taking a matrix
inverse, for efficiency.
Example input:
T = np.array([[1, 0, 0, 0],
[0, 0, -1, 0],
[0, 1, 0, 3],
[0, 0, 0, 1]])
Output:
np.array([[1, 0, 0, 0],
[0, 0, 1, -3],
[0, -1, 0, 0],
[0, 0, 0, 1]])
"""
R, p = TransToRp(T)
Rt = np.array(R).T
return np.r_[np.c_[Rt, -np.dot(Rt, p)], [[0, 0, 0, 1]]]
实际在机械臂控制当中，一般都有速度要求，即你的求解必须在机械臂的控制频率内计算得到。例如你的机械臂是1000hz控制频率，那就意味着每个控制周期必须在1ms内计算完成，否则机械臂就会丢包导致不稳定。因此快速计算齐次矩阵的逆矩阵则是非常必要的。
看到代码中又用到了np.r_和np.c_，我们可以辅助记忆，r代表row，即按行拼接。c代表column，代表按列拼接。
VecTose3：向量到se3
def VecTose3(V):
"""Converts a spatial velocity vector into a 4x4 matrix in se3
:param V: A 6-vector representing a spatial velocity
:return: The 4x4 se3 representation of V
Example Input:
V = np.array([1, 2, 3, 4, 5, 6])
Output:
np.array([[ 0, -3, 2, 4],
[ 3, 0, -1, 5],
[-2, 1, 0, 6],
[ 0, 0, 0, 0]])
"""
return np.r_[np.c_[VecToso3([V[0], V[1], V[2]]), [V[3], V[4], V[5]]],
np.zeros((1, 4))]我们知道在旋量理论体系下，是w在前，v在后的：
根据这一套实现可以把旋量的向量转化为se3
se3ToVec：se3到向量
def se3ToVec(se3mat):
""" Converts an se3 matrix into a spatial velocity vector
:param se3mat: A 4x4 matrix in se3
:return: The spatial velocity 6-vector corresponding to se3mat
Example Input:
se3mat = np.array([[ 0, -3, 2, 4],
[ 3, 0, -1, 5],
[-2, 1, 0, 6],
[ 0, 0, 0, 0]])
Output:
np.array([1, 2, 3, 4, 5, 6])
"""
return np.r_[[se3mat[2][1], se3mat[0][2], se3mat[1][0]],
[se3mat[0][3], se3mat[1][3], se3mat[2][3]]]这个就是从[V]中把向量提取出来，和VecTose3为反过程。
Adjoint：计算齐次矩阵的伴随矩阵
def Adjoint(T):
"""Computes the adjoint representation of a homogeneous transformation
matrix
:param T: A homogeneous transformation matrix
:return: The 6x6 adjoint representation [AdT] of T
Example Input:
T = np.array([[1, 0, 0, 0],
[0, 0, -1, 0],
[0, 1, 0, 3],
[0, 0, 0, 1]])
Output:
np.array([[1, 0, 0, 0, 0, 0],
[0, 0, -1, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 3, 1, 0, 0],
[3, 0, 0, 0, 0, -1],
[0, 0, 0, 0, 1, 0]])
"""
R, p = TransToRp(T)
return np.r_[np.c_[R, np.zeros((3, 3))],
np.c_[np.dot(VecToso3(p), R), R]]这个函数即计算伴随矩阵：
我们可以看到现在开始使用到前面所造的轮子了。
既然写到这里了，我们回顾一下，这个伴随矩阵是干嘛用的？
伴随矩阵就是实现两个旋量之间的坐标变换用的。
ScrewToAxis：螺旋轴qsh到标准螺旋轴：
def ScrewToAxis(q, s, h):
"""Takes a parametric description of a screw axis and converts it to a
normalized screw axis
:param q: A point lying on the screw axis
:param s: A unit vector in the direction of the screw axis
:param h: The pitch of the screw axis
:return: A normalized screw axis described by the inputs
Example Input:
q = np.array([3, 0, 0])
s = np.array([0, 0, 1])
h = 2
Output:
np.array([0, 0, 1, 0, -3, 2])
"""
return np.r_[s, np.cross(q, s) + np.dot(h, s)]
我们这里可以回顾一下：
【现代机器人学】学习笔记二：刚体运动
s表示螺旋轴的朝向，\dot{\theta}则表示绕轴转动的角速度大小。（注意这里是dot，上面是有一点的，表示角度的导数，角速度）。
h表示节距，即线速度/角速度。
q为轴上任意一点，用于配合s定位这根轴。
那么代码里的S和这里的旋量v，其实代表了S是V的一个正则化。
即满足条件：
AxisAng6：旋量到螺旋轴的正则化过程
def AxisAng6(expc6):
"""Converts a 6-vector of exponential coordinates into screw axis-angle
form
:param expc6: A 6-vector of exponential coordinates for rigid-body motion
S*theta
:return S: The corresponding normalized screw axis
:return theta: The distance traveled along/about S
Example Input:
expc6 = np.array([1, 0, 0, 1, 2, 3])
Output:
(np.array([1.0, 0.0, 0.0, 1.0, 2.0, 3.0]), 1.0)
"""
theta = np.linalg.norm([expc6[0], expc6[1], expc6[2]])
if NearZero(theta):
theta = np.linalg.norm([expc6[3], expc6[4], expc6[5]])
return (np.array(expc6 / theta), theta)我们注意到代码中，先把前三个数字做了二范数，即判断角速度的模长是否接近0。然后才做后续的步骤：
这个函数的描述是，对6维指数坐标转化为螺旋轴表示形式：
其实这是描述了一个旋量到螺旋轴的正则化过程。
MatrixExp6：se3到齐次矩阵
def MatrixExp6(se3mat):
"""Computes the matrix exponential of an se3 representation of
exponential coordinates
:param se3mat: A matrix in se3
:return: The matrix exponential of se3mat
Example Input:
se3mat = np.array([[0, 0, 0, 0],
[0, 0, -1.57079632, 2.35619449],
[0, 1.57079632, 0, 2.35619449],
[0, 0, 0, 0]])
Output:
np.array([[1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, -1.0, 0.0],
[0.0, 1.0, 0.0, 3.0],
[ 0, 0, 0, 1]])
"""
se3mat = np.array(se3mat)
omgtheta = so3ToVec(se3mat[0: 3, 0: 3])
if NearZero(np.linalg.norm(omgtheta)):
return np.r_[np.c_[np.eye(3), se3mat[0: 3, 3]], [[0, 0, 0, 1]]]
else:
theta = AxisAng3(omgtheta)[1]
omgmat = se3mat[0: 3, 0: 3] / theta
return np.r_[np.c_[MatrixExp3(se3mat[0: 3, 0: 3]),
np.dot(np.eye(3) * theta \
+ (1 - np.cos(theta)) * omgmat \
+ (theta - np.sin(theta)) \
* np.dot(omgmat,omgmat),
se3mat[0: 3, 3]) / theta],
[[0, 0, 0, 1]]]送入一个se3：
求解下面的内容：
我们首先要判断角速度是不是0，如果是的话，那角度就是0，即省去很多计算：
if NearZero(np.linalg.norm(omgtheta)):
return np.r_[np.c_[np.eye(3), se3mat[0: 3, 3]], [[0, 0, 0, 1]]]
否则的话我们得先老老实实求出so3和对应角度：
theta = AxisAng3(omgtheta)[1]
omgmat = se3mat[0: 3, 0: 3] / theta
然后套用上面的公式来求齐次矩阵：
return np.r_[np.c_[MatrixExp3(se3mat[0: 3, 0: 3]),
np.dot(np.eye(3) * theta \
+ (1 - np.cos(theta)) * omgmat \
+ (theta - np.sin(theta)) \
* np.dot(omgmat,omgmat),
se3mat[0: 3, 3]) / theta],
[[0, 0, 0, 1]]]
我们观察到，左上角：
MatrixExp3(se3mat[0: 3, 0: 3]) 调用MatrixExp3把so3转换为旋转矩阵
然后右上角
np.dot(np.eye(3) * theta \
+ (1 - np.cos(theta)) * omgmat \
+ (theta - np.sin(theta)) \
* np.dot(omgmat,omgmat),
se3mat[0: 3, 3]) / theta
代表两项，由
np.eye(3) * theta \
+ (1 - np.cos(theta)) * omgmat \
+ (theta - np.sin(theta)) \
* np.dot(omgmat,omgmat)
和 se3mat[0: 3, 3]) / theta 进行点乘。
前者代表：
后者代表v，注意，在这个公式当中，w的模长为1，或w=0，v的模长为1。这也就是为什么se3mat[0: 3, 3]) / theta 代表v。
那么这里的theta是通过调用AxisAng3得到的，注意我们这里的这个case，已经是w不为0的case了。
MatrixLog6：其次矩阵到se3
def MatrixLog6(T):
"""Computes the matrix logarithm of a homogeneous transformation matrix
:param R: A matrix in SE3
:return: The matrix logarithm of R
Example Input:
T = np.array([[1, 0, 0, 0],
[0, 0, -1, 0],
[0, 1, 0, 3],
[0, 0, 0, 1]])
Output:
np.array([[0, 0, 0, 0]
[0, 0, -1.57079633, 2.35619449]
[0, 1.57079633, 0, 2.35619449]
[0, 0, 0, 0]])
"""
R, p = TransToRp(T)
omgmat = MatrixLog3(R)
if np.array_equal(omgmat, np.zeros((3, 3))):
return np.r_[np.c_[np.zeros((3, 3)),
[T[0][3], T[1][3], T[2][3]]],
[[0, 0, 0, 0]]]
else:
theta = np.arccos((np.trace(R) - 1) / 2.0)
return np.r_[np.c_[omgmat,
np.dot(np.eye(3) - omgmat / 2.0 \
+ (1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) \
* np.dot(omgmat,omgmat) / theta,[T[0][3],
T[1][3],
T[2][3]])],
[[0, 0, 0, 0]]]我们可以看到这个函数的注释是作者偷懒了，他拷贝来的，return还是MatrixLog3的内容。。
言归正传：
这个实现看起来比较复杂，我们拆开来看一下：
R, p = TransToRp(T)
omgmat = MatrixLog3(R)
是从齐次矩阵得到的旋转和平移，然后把旋转直接用之前的函数MatrixLog3求了一个so3出来。
随后，要判断这个旋转矩阵是不是单位阵：
if np.array_equal(omgmat, np.zeros((3, 3))):
return np.r_[np.c_[np.zeros((3, 3)),
[T[0][3], T[1][3], T[2][3]]],
[[0, 0, 0, 0]]]
他的实现是判断so3是不是各项都为0，当然我们自己实现的时候也可以判断轴角的角度是不是0，或者三个轴是不是都为0，这个实现可以多种。
如果是的话，那得到的[v]中的[w]就都是0，v的话直接取平移部分的值。
这里要注意：
这里得到的w和v，其实是螺旋轴[S]里的表示。
我们这里一定要分清楚：
se3指的是
中的红色部分，因此是有theta项的。
同理前面的MatrixLog3函数的返回值中，也是有一项是theta项的。（接下来也要注意这句话！！）
所以接下来后面注意，返回值得到的[S]以后，也要和theta相乘才是真正的se3的结果！
接下来我们再看当旋转不为0的情况：
theta = np.arccos((np.trace(R) - 1) / 2.0)
return np.r_[np.c_[omgmat,
np.dot(np.eye(3) - omgmat / 2.0 \
+ (1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) \
* np.dot(omgmat,omgmat) / theta,[T[0][3],
T[1][3],
T[2][3]])],
[[0, 0, 0, 0]]]
omgmat代表[w]theta， 然后在列上做一个拼接，和v*theta拼接到一起。
v怎么得到呢？
v*theta 即
np.dot(np.eye(3) - omgmat / 2.0 \
+ (1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) \
* np.dot(omgmat,omgmat) / theta,[T[0][3],
T[1][3],
T[2][3]])],
其中，p则是
[T[0][3], T[1][3], T[2][3]])],
而
第一项，np.eye(3)代表1/theta * I * theta， 我们知道theta是一个标量，可以直接乘进去
第二项，因为我们的omgmat是从MatrixLog3(R)得到的，因此omgmat这一项代表了[w]* theta，所以 omgmat / 2.0代表了 1/2 *[w] *theta
我们再看第三项，
这一项再乘以theta对应：
(1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) * np.dot(omgmat,omgmat) / theta
这是怎么得到的呢？
![(\frac{1}{\theta} -\frac{1}{2}cot\frac{\theta}{2})[w]^2 \theta = (\frac{1}{\theta} -\frac{1}{2}\frac{1}{tan\frac{\theta}{2}})[w]^2 \theta](http://img.e-com-net.com/image/info8/4d76b6591f9a46fc9d48c0a5b5f3bd5a.png){kind=small}
注意，omgmat实际上是[w]theta，因此np.dot(omgmat,omgmat)实际上变成了[w]^2 \theta^2，所以要多除以一个theta！
今天时间不早了，先写到这里，后续继续更新~