Pybullet中矩阵变换相关API介绍

前言

  最近看Pybullet官方手册，发现关于几个矩阵变换的函数解释的不够详尽，虽然心里明白他的用途，但用起来没那么放心，故在此总结一番

invertTransform

  官方手册只提到输入参数以及输出参数，看不到底层源码，我就自己写了个测试代码，可以确定该函数求取的是齐次变换矩阵的逆矩阵：
$${\left[\begin{array}{cc}R& v\\ 0& 1\end{array}\right]}^{-1}=\left[\begin{array}{cc}{R}^{-1}& -R^{-1}v\\ 0& 1\end{array}\right]\text{\hspace{1em}(1)}$$

  其中position表示齐次变换的$v$，而旋转矩阵$R$是由第二个参数中的四元数转换而来的，转换规则自行查阅资料，这里只用知道四元数$q=[x,y,z,w]$转换为轴角表达如下：
  设旋转轴为$r=[i,j,k]$，旋转角度为$\theta$，则表示绕轴$r$旋转$\theta$角的四元数$q$为:$[x=i\cdot sin(\frac{\theta }{2}),y=j\cdot sin(\frac{\theta }{2}),z=k\cdot sin(\frac{\theta }{2}),w=cos(\frac{\theta }{2})]$，测试代码如下：

import pybullet as p
import numpy as np

# v:求逆前齐次变换的位置变换v
# q:求逆前表示旋转变换的四元数
v= [2, 1, 0]
q= [0, 0.5 ** 0.5, 0, 0.5 ** 0.5]  # 绕y轴旋转90度
# v_:求逆后齐次变换的位置变换v_
# q_:求逆前表示旋转变换的四元数
v_, q_ = p.invertTransform(v, q)  # 求逆
print(v_)  # (0.0, -1.0, -2.0)

R = p.getMatrixFromQuaternion(q)  # 获取求逆前的旋转矩阵
R_ = p.getMatrixFromQuaternion(q_)  # 获取求逆后的旋转矩阵

np_R = np.array([list(R[0: 3]), list(R[3: 6]), list(R[6: 9])],dtype=float)
np_R_ = np.array([list(R_[0: 3]), list(R_[3: 6]), list(R_[6: 9])],dtype=float)
inv_R = np.linalg.inv(np_R)  # 求逆前的旋转矩阵手动求逆

print(inv_R)  # 与np_R_比较相同
print(-(inv_R @ v))  # 与v_比较相同
print(np_R_)

multiplyTransforms

  经过实际测试发现这其实就是由两个（四元数-位置变换组）组成的齐次变换矩阵的矩阵乘法，具体哪个齐次矩阵乘在左侧，也不清楚，测试如下：
  从下面代码测试发现应该由positionA-orientationA组成的齐次矩阵左乘positionB-orientationB组成的齐次矩阵，亦即先进行A齐次变换，再到A变换的基础上进行B齐次变换:

import pybullet as p
import numpy as np

def Transform(v, q):
    R = p.getMatrixFromQuaternion(q)  # 获取旋转矩阵
    # 拼凑齐次矩阵
    slices = [list(R[0: 3]), list(R[3: 6]), list(R[6: 9])]
    for i in range(3):
        slices[i].append(v[i])
    np_R = np.array(slices)
    bottom = np.array([0, 0, 0, 1])
    T = np.vstack((np_R, bottom))  # 在底部补齐
    # print(T)
    return T


v1 = [3, 2, 1]
q1 = [0, 0.5 ** 0.5, 0, 0.5 ** 0.5]  # 绕y轴旋转90度

v2 = [1, 2, 3]
q2 = [0, 0, 0.5 ** 0.5, 0.5 ** 0.5]  # 绕z轴旋转90度

v, q = p.multiplyTransforms(v1, q1, v2, q2)
T1 = Transform(v1, q1)
T2 = Transform(v2, q2)

T_test1 = T1 @ T2
T_test2 = T2 @ T1
print(f"T1在左侧结果:{T_test1}")
print(f"T2在左侧结果:{T_test2}")

T_after = Transform(v, q)
print(f"multiplyTransforms函数求取的结果:{T_after}")
#T_fter和T_test1结果相同

  本文到这就结束了，因为其他相关的API官网解释的很清楚了，大家看官网资料吧，有问题留在评论区，大家一起探讨呀，或者还有其他需要我进一步探索的，欢迎留言