刚体位姿(即位置和姿态)，用刚体的方位矩阵和参考坐标的原点位置（）。