ROS探索总结(二十六)——MoveIt编程

  • 内容
  • 评论
  • 相关

  在之前的基础学习中,我们已经对moveit有了一个基本的认识,在实际的应用中,GUI提供的功能毕竟有限,很多实现还是需要我们在代码中完成,moveitmove_group也提供了丰富的C++ API,不仅可以帮助我们使用代码完成GUI可以实现的功能,还可以加入更多丰富的功能。我们继续使用《Mastering ROS for robotics Programming》中的源码作为学习对象。

clip_image002

 

一、创建功能包

首先,我们先创建一个新的功能包,来放置我们的代码:

  1. $ catkin_create_pkg seven_dof_arm_test catkin cmake_modules interactive_markers moveit_core moveit_ros_perception moveit_ros_planning_interface pluginlib roscpp std_msgs

  也可以直接使用《Mastering ROS for robotics Programming》中的seven_dof_arm_test功能包。

二、随机轨迹

  通过rvizplanning插件的功能,我们可以为机器人产生一个随机的目标位置,让机器人完成运动规划并移动到目标点。使用代码同样可以实现相同的功能,我们首先从这个较为简单的例程入手,对Moveit C++ API有一个初步的认识。

  二话不说,先上代码(源码文件 test_random.cpp可以在源码包中找到):

  1. //首先要包含API的头文件
  2. #include <moveit/move_group_interface/move_group.h>
  3. int main(int argc, char **argv)
  4. {
  5.   ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName);
  6.   // 创建一个异步的自旋线程(spinning thread)
  7.   ros::AsyncSpinner spinner(1);
  8.   spinner.start();
  9.   // 连接move_group节点中的机械臂实例组,这里的组名arm是我们之前在setup assistant中设置的
  10.   move_group_interface::MoveGroup group("arm");
  11.   // 随机产生一个目标位置
  12.   group.setRandomTarget();
  13.   // 开始运动规划,并且让机械臂移动到目标位置
  14.   group.move();
  15.   ros::waitForShutdown();
  16. }

   已经在代码中加入了重点代码的解释,move_group_interface::MoveGroup用来声明一个机械臂的示例,后边都是针对该实例进行控制。

   除了moveit,可能很多人对ROS单节点中的多线程API接触的比较少。一般我们使用的自旋API都是spin()或者spinonce(),但是有些情况下会有问题,比如说我们有两个回调函数,第一个回调函数会延时5秒,那么当我们开始spin()时,回调函数会顺序执行,第二个回调函数会因为第一个回调函数的延时,在5秒之后才开始执行,这个当然是我们无法接受的,所如果采用多线程的spin(),就不会存在这个问题了。关于ROS多线程的问题,可以参考wiki等资料:

http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning

http://www.cnblogs.com/feixiao5566/p/5288206.html

   然后修改CMakeLists文件,编译代码,执行下边的命令:

  1. $ roslaunch seven_dof_arm_config demo.launch
  2. $ rosrun seven_dof_arm_test test_random_node

   稍等一下,我们就可以在rviz中看到机械臂的动作了。

clip_image004

 

三、自定义目标位置并完成规划

接下来我们继续学习如何使用API自定义一个目标位置并让机器人运动过去。源码是 test_custom.cpp,这里我删掉了部分冗余的代码,进行了部分修改:

  1. // 包含miveit的API头文件
  2. #include <moveit/move_group_interface/move_group.h>
  3.  
  4. int main(int argc, char **argv)
  5. {
  6.   ros::init(argc, argv, "move_group_interface_tutorial");
  7.   ros::NodeHandle node_handle; 
  8.   ros::AsyncSpinner spinner(1);
  9.   spinner.start();
  10.  
  11.   moveit::planning_interface::MoveGroup group("arm");
  12.  
  13.   // 设置机器人终端的目标位置
  14.   geometry_msgs::Pose target_pose1;
  15.   target_pose1.orientation.w = 0.726282;
  16.   target_pose1.orientation.x= 4.04423e-07;
  17.   target_pose1.orientation.y = -0.687396;
  18.   target_pose1.orientation.z = 4.81813e-07;
  19.  
  20.   target_pose1.position.x = 0.0261186;
  21.   target_pose1.position.y = 4.50972e-07;
  22.   target_pose1.position.z = 0.573659;
  23.   group.setPoseTarget(target_pose1);
  24.  
  25.  
  26.   // 进行运动规划,计算机器人移动到目标的运动轨迹,此时只是计算出轨迹,并不会控制机械臂运动
  27.   moveit::planning_interface::MoveGroup::Plan my_plan;
  28.   bool success = group.plan(my_plan);
  29.  
  30.   ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");   
  31.  
  32.   //让机械臂按照规划的轨迹开始运动。
  33.   //if(success)
  34.   //    group.execute(my_plan);
  35.  
  36.   ros::shutdown(); 
  37.   return 0;
  38. }

     对比生成随机目标的源码,基本上只有只是加入了设置终端目标位置的部分代码,此外这里规划路径使用的是plan(),这个对应rvizplanningplan按键,只会规划路径,可以在界面中看到规划的路径,但是并不会让机器人开始运动,如果要让机器人运动,需要使用execute(my_plan),对应execute按键。当然,我们也可以使用一个move()来代替paln()execute()。具体的API说明,可以参考官方的文档:

http://docs.ros.org/jade/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1MoveGroup.html

  1. $ roslaunch seven_dof_arm_config demo.launch
  2. $ rosrun seven_dof_arm_test test_custom_node

     

clip_image006

 

四、碰撞检测

moveit可以帮助我们完成机器人的自身碰撞检测和环境障碍物碰撞检测,rivzGUI中,我们可以通过 Planning Scene插件来导入障碍物并且对障碍物进行编辑,现在我们在前边学习内容的基础上,通过代码加入一个障碍物,看下会对运动规划有什么影响。

  1. // 包含API的头文件
  2. #include <moveit/move_group_interface/move_group.h>
  3. #include <moveit/planning_scene_interface/planning_scene_interface.h>
  4. #include <moveit_msgs/AttachedCollisionObject.h>
  5. #include <moveit_msgs/CollisionObject.h>
  6.  
  7. int main(int argc, char **argv)
  8. {
  9.     ros::init(argc, argv, "add_collision_objct");
  10.     ros::NodeHandle nh;
  11.     ros::AsyncSpinner spin(1);
  12.     spin.start();
  13.  
  14.     // 创建运动规划的情景,等待创建完成
  15.     moveit::planning_interface::PlanningSceneInterface current_scene;
  16.     sleep(5.0);
  17.  
  18.     // 声明一个障碍物的实例,并且为其设置一个id,方便对其进行操作,该实例会发布到当前的情景实例中
  19.     moveit_msgs::CollisionObject cylinder;
  20.     cylinder.id = "seven_dof_arm_cylinder";
  21.  
  22.     // 设置障碍物的外形、尺寸等属性   
  23.     shape_msgs::SolidPrimitive primitive;
  24.     primitive.type = primitive.CYLINDER;
  25.     primitive.dimensions.resize(3);
  26.     primitive.dimensions[0] = 0.6;
  27.     primitive.dimensions[1] = 0.2;
  28.  
  29.     // 设置障碍物的位置
  30.     geometry_msgs::Pose pose;
  31.     pose.orientation.w = 1.0;
  32.     pose.position.x =  0.0;
  33.     pose.position.y = -0.4;
  34.     pose.position.z =  0.4;
  35.  
  36.     // 将障碍物的属性、位置加入到障碍物的实例中
  37.     cylinder.primitives.push_back(primitive);
  38.     cylinder.primitive_poses.push_back(pose);
  39.     cylinder.operation = cylinder.ADD;
  40.  
  41.     // 创建一个障碍物的列表,把之前创建的障碍物实例加入其中
  42.     std::vector<moveit_msgs::CollisionObject> collision_objects;
  43.     collision_objects.push_back(cylinder);
  44.  
  45.     // 所有障碍物加入列表后(这里只有一个障碍物),再把障碍物加入到当前的情景中,如果要删除障碍物,使用removeCollisionObjects(collision_objects)
  46.     current_scene.addCollisionObjects(collision_objects);
  47.  
  48.     ros::shutdown();
  49.  
  50.     return 0;
  51. }

我们再来编译运行一下,看看会发生什么。

  1. $ roslaunch seven_dof_arm_config demo.launch
  2. $ rosrun seven_dof_arm_test add_collision_objct

   稍等五秒钟,有没有看到,现在界面中多了一个圆柱体。

clip_image008

scene objects中可以对障碍物的属性进行再次调整。

        clip_image010

   障碍物加入之后,再运动机械人的时候,就会检测机器人是否会与障碍物发生碰撞,比如我们用鼠标拖动机器人的终端,让机器人和障碍物发生碰撞:

clip_image012

有没有看到机械臂的部分links变成了红色,这就说明这些links和障碍物发生了碰撞,如果此时进行运动规划,会提示错误的。

clip_image014

上面的代码只是在场景中加入了障碍物,碰撞检测由moveit的插件完成,那么我们如何通过代码来检测是否发生了碰撞呢?可以学习源码包中的 check_collision.cpp

  1. #include <ros/ros.h>
  2. // 包含moveit的API
  3. #include <moveit/robot_model_loader/robot_model_loader.h>
  4. #include <moveit/planning_scene/planning_scene.h>
  5. #include <moveit/kinematic_constraints/utils.h>
  6. #include <eigen_conversions/eigen_msg.h>
  7.  
  8. int main(int argc, char **argv)
  9. {
  10.   ros::init (argc, argv, "arm_kinematics");
  11.   ros::AsyncSpinner spinner(1);
  12.   spinner.start();
  13.  
  14.   // 加载机器人的运动学模型到情景实例中
  15.   robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  16.   robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
  17.   planning_scene::PlanningScene planning_scene(kinematic_model);
  18.  
  19.   // 自身碰撞检测
  20.   // 首先需要创建一个碰撞检测的请求对象和响应对象,然后调用碰撞检测的API checkSelfCollision,检测结果会放到collision_result中
  21.   collision_detection::CollisionRequest collision_request;
  22.   collision_detection::CollisionResult collision_result;
  23.   planning_scene.checkSelfCollision(collision_request, collision_result);
  24.   ROS_INFO_STREAM("1. Self collision Test: "<< (collision_result.collision ? "in" : "not in")
  25.                   << " self collision");
  26.  
  27.   // 修改机器人的状态
  28.   // 我们可以使用场景实例的getCurrentStateNonConst()获取当前机器人的状态,然后修改机器人的状态到一个随机的位置,
  29.   // 清零collision_result的结果后再次检测机器人是否发生滋生碰撞
  30.   robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst();
  31.   current_state.setToRandomPositions();
  32.   collision_result.clear();
  33.   planning_scene.checkSelfCollision(collision_request, collision_result);
  34.   ROS_INFO_STREAM("2. Self collision Test(Change the state): "<< (collision_result.collision ? "in" : "not in"));
  35.  
  36.   // 我们也可以指定查询一个group是否和其他部分发生碰撞,只需要在collision_request中修改group_name属性
  37.   collision_request.group_name = "arm";
  38.   current_state.setToRandomPositions();
  39.   collision_result.clear();
  40.   planning_scene.checkSelfCollision(collision_request, collision_result);
  41.   ROS_INFO_STREAM("3. Self collision Test(In a group): "<< (collision_result.collision ? "in" : "not in"));
  42.  
  43.   // 获取碰撞关系
  44.   // 首先,我们先让机器人发生自身碰撞 
  45.   std::vector<double> joint_values;
  46.   const robot_model::JointModelGroup* joint_model_group =
  47.   current_state.getJointModelGroup("arm");
  48.   current_state.copyJointGroupPositions(joint_model_group, joint_values);
  49.   joint_values[2] = 1.57; //原来的代码这里是joint_values[0],并不会导致碰撞,我改成了joint_values[2],在该状态下机器人会发生碰撞
  50.   current_state.setJointGroupPositions(joint_model_group, joint_values);
  51.   ROS_INFO_STREAM("4. Collision points "
  52.                   << (current_state.satisfiesBounds(joint_model_group) ? "valid" : "not valid"));
  53.  
  54.   // 然后我们再来检测机器人是否发生了自身碰撞,已经发生碰撞的是哪两个部分
  55.   collision_request.contacts = true;
  56.   collision_request.max_contacts = 1000;
  57.   collision_result.clear();
  58.   planning_scene.checkSelfCollision(collision_request, collision_result);
  59.   ROS_INFO_STREAM("5. Self collision Test: "<< (collision_result.collision ? "in" : "not in")
  60.                   << " self collision");
  61.  
  62.   collision_detection::CollisionResult::ContactMap::const_iterator it;
  63.   for(it = collision_result.contacts.begin();
  64.       it != collision_result.contacts.end();
  65.       ++it)
  66.   {
  67.     ROS_INFO("6 . Contact between: %s and %s",
  68.              it->first.first.c_str(),
  69.              it->first.second.c_str());
  70.   }
  71.  
  72.   // 修改允许碰撞矩阵(Allowed Collision Matrix,acm)
  73.   // 我们可以通过修改acm来指定机器人是否检测自身碰撞和与障碍物的碰撞,在不检测的状态下,即使发生碰撞,检测结果也会显示未发生碰撞
  74.   collision_detection::AllowedCollisionMatrix acm = planning_scene.getAllowedCollisionMatrix();
  75.   robot_state::RobotState copied_state = planning_scene.getCurrentState();
  76.   collision_detection::CollisionResult::ContactMap::const_iterator it2;
  77.   for(it2 = collision_result.contacts.begin();
  78.       it2 != collision_result.contacts.end();
  79.       ++it2)
  80.   {
  81.     acm.setEntry(it2->first.first, it2->first.second, true);
  82.   }
  83.   collision_result.clear();
  84.   planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm);
  85.  
  86.   ROS_INFO_STREAM("6. Self collision Test after modified ACM: "<< (collision_result.collision ? "in" : "not in")
  87.                   << " self collision");
  88.  
  89.   // 完整的碰撞检测
  90.   // 同时检测自身碰撞和与障碍物的碰撞
  91.   collision_result.clear();
  92.   planning_scene.checkCollision(collision_request, collision_result, copied_state, acm);
  93.  
  94.   ROS_INFO_STREAM("6. Full collision Test: "<< (collision_result.collision ? "in" : "not in")
  95.                   << " collision");
  96.  
  97.   ros::shutdown();
  98.   return 0;
  99. }

编译运行:

  1. $ roslaunch seven_dof_arm_config demo.launch
  2. $ roslaunch seven_dof_arm_test check_collision

     可以看到碰撞检测的结果:

clip_image016

 

  更多MoveIt API的使用例程还可以参考下边的链接:

http://docs.ros.org/indigo/api/pr2_moveit_tutorials/html/planning/src/doc/move_group_interface_tutorial.html

 

  还有MoveIt interfaceClass文档:

http://docs.ros.org/indigo/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1MoveGroup.html


原创文章,转载请注明: 转载自古月居

本文链接地址: ROS探索总结(二十六)——MoveIt编程

微信 OR 支付宝 扫描二维码
为本文作者 打个赏
pay_weixinpay_weixin

评论

14条评论
  1. Gravatar 头像

    Shawn 回复

    在test_custom.cpp例子中,源码多了几行代码,我没看懂,
    moveit::planning_interface::PlanningSceneInterface
    planning_scene_interface;
    ros::Publisher display_publisher =
    node_handle.advertise("/move_group/
    display_planned_path", 1, true);
    moveit_msgs::DisplayTrajectory display_trajectory;

    进行了初始化,却没有在后面的代码中用上。

    您也提到这部分代码是冗余的,删除了。

    原作者留下这部分代码的用意是什么?

    • 古月

      古月 回复

      @Shawn 这个应该是用于rviz显示路径的,没有用到

  2. Gravatar 头像

    海漩涡 回复

    月哥,你好
    我在弄随机轨迹那个例子时,我的程序总是卡在move这个函数不往下走了,但是机器人是动了的。

    操作多次,结果都一样。请问下你当时的效果也是这样的吗

    • 古月

      古月 回复

      @海漩涡 我运行的时候是没有问题的,你试一下《Mastering ROS for robotics Programming》里边提供的源码,或者在rviz中用target设置一下目标位置。

  3. Gravatar 头像

    海漩涡 回复

    月哥,我最近在研究moveit,使用它对机器人的手臂进行控制。
    在博客的评论里问了你很多问题了,感觉这样交流不是很方面

    望能够提供QQ或邮箱,日后能更好的交流。
    我的QQ469249801

  4. Gravatar 头像

    海漩涡 回复

    月哥,请教下你有没有用moveit的compute_cartesian_path接口来计算一系列waypoints的轨迹

    我在使用此接口时规划的结果是 30% 45% 80% 等都有。

    我想知道怎么正确的使用此接口?

    • 古月

      古月 回复

      @海漩涡 你好,这个接口我很早以前试过,好像不太好用,就是按照文档上的说明使用的

  5. Gravatar 头像

    章金锋 回复

    古月,您好,不知道怎么能联系上您,所以只能给您留言了,我是上海哲谦应用科技的,说大了就是看你的文章在学习,直接点就是想找您合作,方便的话,您能把你的联系方式发到我的邮箱吗?zzhang0617@163.com 希望您能看到我的这条留言,也希望您能给我发个联系邮件,我们具体谈,谢谢!

  6. Gravatar 头像

    增达信购 回复

    我对你博客的爱,你永远不会明白!

  7. Gravatar 头像

    tanxiaohai 回复

    月哥你好,看了你几篇文章了学到了很多,非常感谢

    想请教你个问题:
    我在网上下载的urdf通过setup assistant进行配置完后,想用artibox模拟器来进行运动规划的测试。
    我现在遇到的问题是我能跑起:
    /arbotix
    /right_gripper_controller
    /robot_state_publisher

    这三个节点,我想用rviz来显示我的机器人,之后再用moveIt来控制。但是我确在配置文件中找不到rviz的配置文件。

    请问下怎样才能获得与urdf对应的rviz配置文件???

    • 古月

      古月 回复

      @tanxiaohai 你好,用setup assistant生成的rviz配置文件在configb包的launch文件夹下,moveit.rviz

  8. Gravatar 头像

    王聪 回复

    想问一下能在kinetic上安装moveit吗?官方好像还没有,但是ubuntu装的16版本

    • 古月

      古月 回复

      @王聪 你好,我没试过kinetic版本,但是我看到moveit有这个版本的源码,应该是可以安装的,用tab试一下

发表评论

电子邮件地址不会被公开。 必填项已用*标注