引言

在死板人节制中，对于扭转的示意上，有好几种不同的示意方法，此日咱们来逐一讲授，并颠末pybullet考证他们之间的关联。

不同示意方法

扭转的示意方法，能够大抵分为RPY角，欧拉角，四元数和轴角。

RPY角

首先评释字母：

R：Roll横滚

P：pitch俯仰

Y：Yaw偏航

从字母的次序就能够懂得扭转的挨次：首先绕着x轴转，再绕着y轴转，着末绕着z轴转。不过须要留意的是RPY角中的扭转都是绕着最开端的坐标系扭转的，这是与欧拉角不同的场合。

欧拉角

欧拉角和RPY角很像，不过其扭转都是绕着扭转后的坐标系转的，况且扭转的挨次是先z-y-x。好比说绕着在绕着y转时：

先绕着开端的坐标系转z，再绕着扭转后的坐标系转y。

颠末云云扭转后，欧拉角扭转后的姿势和RPY角扭转后的姿势是相同的。

咱们以欧拉角来剖析，本来欧拉角存在一些题目：

1、死锁题目：

死锁，说的是当第二次扭转为90度的时刻（广泛来说，第二次扭转后坐标系产生重合），第三次扭转和第一次扭转的扭转方位相同，从整个来看便是遗失了一个自在度的节制。没法实行球面光滑插值。

上图便是产生在死锁环境下，且坐标系仍旧对齐原坐标但方位改变的环境。

能够发掘，由于死板臂的轴向为y轴，因而今世码中的zyx欧拉角转完后，死板臂轴向仍旧稳固；而盆模子的姿势并不但表如今y轴一个方位上，因而由平着转为竖直：

init_position=[0.,0,0.22]init_posture=p.getQuaternionFromEuler([-math.pi/2,0,-math.pi/2.])a=p.getQuaternionFromEuler([1,math.pi/2,1])rr=np.array(p.getMatrixFromQuaternion(a)).reshape(3,3)print(rr)init_posture=self.quaternion_rotation(init_posture,a)p.loadURDF("tray/tray.urdf",basePosition=[0,0.9,0.25],baseOrientation=a)jointposes=p.calculateInverseKinematics(self.UR5,6,init_position,init_posture,maxNumIterations=)forjinrange(6):p.resetJointState(self.UR5,j+1,jointposes[j])p.stepSimulation()

2、示意不惟一

由于角度的周期性改变，致使在示意的形势上也不惟一。且死锁题目的存在更增长知道的数量。固然这一点能够颠末束缚界限在[-,]处理。

3、插值后死锁诱发震撼

当插值后的值适值为90度相近时，会浮现类似姿势的欧拉角差异很大的环境（第二条）。

轴角

轴角形势的扭转，知道起来能够简单一些。假设说欧拉角是绕着坐标系举行三次次序举行的扭转，那末轴角实行的便是将这三次扭转后获得复合扭转，找对其对应的扭转轴和转角。由于轴角形势的扭转只举行一次，因而能够防止欧拉角带来的死锁题目。

其示意形势为一个用于示意吻合扭转中转轴的单元三维空间矢量，加一个标量的转角。

在举行插值的时刻，由于单元的三维空间矢量和标量的转角并不能归一化管教，因而会形成插值不波动。

四元数

四元数是另一种扭转的示意方法，用于扭转的四元数，每个份量的界限都在（-1，1）。其将单元化统一到四维空间中，能够便利得举行光滑插值，并防止死锁题目。

在举行线性插值，个别能够用这个花式举行示意：

为了保证单元性，须要对其举行归一化：

不过咱们理论冀望的是四元数中的转角线性改变：

因而须要球面插值：

不同示意之间的更动

在这边，紧要讲解两种更动，四元数相乘与扭转矩阵；另一种是轴角到四元数的更动。

四元数---扭转矩阵

四元数相乘的公式以下：

代码实行：

个中的np.cross实行了向量叉乘。

咱们测试了两个四元数相乘后获得终于更动为扭转矩阵的值，以及扭转矩阵直接相乘的值。发掘在哄骗中，哄骗四元数右乘和矩阵右乘实行的成绩类似。

init_posture=p.getQuaternionFromEuler([-math.pi/2,0,-math.pi/2.])r=np.array(p.getMatrixFromQuaternion(init_posture)).reshape(3,3)a=p.getQuaternionFromEuler([1,math.pi/2,1])rr=np.array(p.getMatrixFromQuaternion(a)).reshape(3,3)print(np.dot(r,rr))init_posture=self.quaternion_rotation(init_posture,a)print(np.array(p.getMatrixFromQuaternion(init_posture)).reshape(3,3))

轴角---四元数

哄骗pybullet中自带的接口函数就能够：

...defaxial_angle_2_quaternion(self,axis,alpha):q=p.getQuaternionFromAxisAngle(axis,alpha)print(np.array(p.getMatrixFromQuaternion(q)).reshape(3,3))returnq......U.axial_angle_2_quaternion([1,0,0],math.pi/2)

能够看到，终于仍旧切确的。

《在MoveIt中集成自界说活动布局算法》

用了这么久ROSMoveIt，你果真知道它么？你大概据说过ROS的插件机制，不过怎么颠末代码实行一个插件？假设你也有这些疑难，迎接到达这边咱们一同切磋。本门课程将为你显露MoveItpipeline的奇妙面纱，知道MoveIt活动布局的底层奇妙。

（扫描

转载请注明:http://www.aideyishus.com/lktp/854.html