基于ROS与moveit的机械臂零件抓取探索笔记（1）

四元数与欧拉角的转变

在pick_and_place程序中遇到了一个问题，执行place任务时物块的方向总是不对，所以怀疑是orientation出了问题。在moveit中orientation试用四元数表示的，因此借此机会学习四元数。

欧拉角

Z轴(航偏角，yaw，Ψ)
Y轴(俯仰角，pitch，θ)
X轴(滚转角，roll，Φ)
优点是只需要三个参数，表示简单
缺点是对于同一个方向，可以有不同的欧拉角表示方法，不同的顺序会影响最后的结果。

四元数

三维空间的任意旋转，都可以用绕三维空间的某个轴旋转过某个角度来表示，即所谓的Axis-Angle表示方法。这种表示方法里，Axis可用一个三维向量(x,y,z)来表示，θ可以用一个角度值来表示，直观来讲，一个四维向量(θ,x,y,z)就可以表示出三维空间任意的旋转。注意，这里的三维向量(x,y,z)只是用来表示axis的方向朝向，因此更紧凑的表示方式是用一个单位向量来表示方向axis，而用该三维向量的长度来表示角度值θ。

这就是四元数的表达，表示(x,y,z)旋转θ角度后的四元数。当θ=0时，四元数为（1，0，0，0）。

程序上的修改

在函数make_place中修改了pose.orientation之后，问题仍然没有解决，物块还是东倒西歪的。冥思苦想之后，又理了一遍程序的逻辑，发现问题不在这里，在set_place_pose函数里面已经指定了pose的值，make_place函数只不过是做了一点小小的修改。因此在set_place_pose函数将orientation改为（1，0，0，0），发现无法求解。心里想可能是过约束的原因，遂把z删掉，程序恢复正常。

更新：这一方法是错误的。 昨天只是碰巧搞好，这一问题仍然有待解决。

Moveit选择OMPL库中的算法

这是本周遇到的最大的坑，相关的中文资料没有，写下来帮助各位朋友。在rviz的motion planner中的connect页可以非常轻松的选择不同的规划库里的算法，可是，如果通过编程的方式实现规划，该如何选择ompl库中的算法呢？
经过辛苦的查询后，发现一位大哥的moveit_config/config/ompl_planning.yaml 文件和我的样式不一样，他的文件里有的default_planner_config一项，而我的没有，所以想到能不能自己加一项，尝试了一下，非常顺利地成功了。
如果所示，即可改变默认算法。
顺疼摸瓜的百度了一下，通过api接口的方式也可以。在python中为set_planner_id，c++中为setPlannerid，有待之后查找。