标签：工业机器人第二章单元测试、刚体的位姿表示刚体的位置和姿态，位置用p矩阵表示，姿态用R矩阵表示