刚体的位姿表示刚体的位置和姿态，位置用p矩阵表示，姿态用R矩阵表示