MoveIt! Tutorials, MoveIt! 教程——demo

MoveIt! Tutorials，gihub地址：https://github.com/ros-planning/moveit_tutorials/blob/kinetic-devel/index.rst

Beginner

MoveIt! RViz Plugin Tutorial

ROS Visualizer (RViz)

下载pr2_moveit_config包，然后编译

git clone https://github.com/davetcoleman/pr2_moveit_config.git

启动demo文件配置插件

roslaunch pr2_moveit_config demo.launch

启动时发现没有pr2_description包，然后下载安装

sudo apt-get install ros-kinetic-pr2-description

可以打开rviz，

demo.launch里的内容为（好像是自动生成的）：

<launch>
  
  <arg name="db" default="false" />
  
  <arg name="db_path" default="$(find pr2_moveit_config)/default_warehouse_mongo_db" />

  
  <arg name="debug" default="false" />

  
  <include file="$(find pr2_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  include>

  
    <node pkg="tf" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 odom_combined base_footprint 100" />


  
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="false"/>
    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]rosparam>
  node>

  
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

  
  <include file="$(find pr2_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="true"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  include>

  
  <include file="$(find pr2_moveit_config)/launch/moveit_rviz.launch">
    <arg name="config" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  include>

  
  <include file="$(find pr2_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
  include>

launch>

第一次打开需要添加Motion Planning插件，

在“Displays” 子窗口下的“Global Options” 标签中选择固定坐标系为odom_combined，
Planning Scene Topic设为“planning_scene”。
在Planning Request中，修改Planning Group为“right_arm”.
设置Trajectory Topic为“/move_group/display_planned_path”.
Interactive Marker Size设成0.2~0.5，总之不要是零。

下面开始交互
如下图添加interact工具。可以看到，绿色手臂对应的是起始位姿，黄色手臂对应的是目标位姿。


将目标点移动到另一个手臂附近时会发生碰撞，碰撞部位为红色。此时进行规划会失败。

在面板中打开“MotionPlanning - Slider”可以观察plan的每一步

Move Group Interface Tutorial

MoveIt!的用户接口是MoveGroup类。它提供了用户经常使用的各种函数，包括设置关节或者目标位置、建立运动规划、移动机器人和增加障碍物。这一接口通过ROS话题（topics）、服务（service）和动作（action）等进行通信。
据说有个YouTube的视频，本来想以后给传到YouKu上，但其实就是下面这样

建立工作空间

rosdep update
sudo apt-get update
sudo apt-get dist-upgrade
sudo apt-get install python-wstool python-catkin-tools clang-format-3.8

在/src下运行

wstool init .
wstool merge https://raw.githubusercontent.com/ros-planning/moveit/kinetic-devel/moveit.rosinstall
wstool update
rosdep install -y --from-paths . --ignore-src --rosdistro kinetic
cd ..
catkin config --extend /opt/ros/kinetic --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin_make

下载源码

git clone https://github.com/ros-planning/moveit_tutorials.git
git clone https://github.com/PR2/pr2_common.git -b kinetic-devel
git clone https://github.com/davetcoleman/pr2_moveit_config.git

安装相关依赖，然后编译

rosdep install --from-paths . --ignore-src --rosdistro kinetic
cd ~/ws_moveit
catkin config --extend /opt/ros/kinetic --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin_make

启动rviz

roslaunch pr2_moveit_config demo.launch

在一个新的终端里启动demo

roslaunch moveit_tutorials move_group_interface_tutorial.launch

启动后的页面是这个样子的

单机Next按钮或者按N，可以看到下一步的演示。
1. The robot moves its right arm to the pose goal to its right front.

2. The robot moves its right arm to the joint goal at its right side.

3. The robot moves its right arm back to a new pose goal while maintaining the end-effector level.

4. The robot moves its right arm along the desired cartesian path (a triangle up+forward, left, down+back).

5. A box object is added into the environment to the right of the right arm. The robot moves its right arm to the pose goal, avoiding collision with the box.

6. The object is attached to the wrist (its color will change to purple/orange/green).
The object is detached from the wrist (its color will change back to green).
The object is removed from the environment.

7. The robot moves both arms to two different pose goals at the same time.

Demo的解释

刚才的启动文件在这个路径下pr2_tutorials/planning，其中的文件内容为

<launch>
  <node 
    name="move_group_interface_tutorial" 
    pkg="moveit_tutorials" 
    type="move_group_interface_tutorial" 
    respawn="false"    //复位属性，如果为真的话，当节点停止的时候，roslaunch会重新启动该节点。
    output="screen">   //将标准输出显示在屏幕上而不是记录到日志文档。
  node>
launch>

可以看到它启动的是一个move_group_interface_tutorial类型的节点。

/* Author: Sachin Chitta, Dave Coleman */

#include 
#include 

#include 
#include 

#include 
#include 

#include 

int main(int argc, char **argv)
{
  ros::init(argc, argv, "move_group_interface_tutorial");//初始化节点，并设置节点名称
  ros::NodeHandle node_handle;//进程的处理程序，它允许我们与环境交互
  ros::AsyncSpinner spinner(1);//ROS多线程订阅消息，这里开了一个线程
  spinner.start();//线程启动

  // BEGIN_TUTORIAL
  //
  // Setup
  // ^^^^^
  /*MoveIt！ 对一组称为 "planning groups"的关节进行操作，并将其存储在名为JointModelGroup的对象中。在整个MoveIt！中 "planning group"和"joint model group"这些术语可以互换使用。*/
  static const std::string PLANNING_GROUP = "right_arm";

  /*MoveGroup类可以很容易的用你想规划和控制的planning group来进行设置 */
  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

  // moveit:planning_interface:`PlanningSceneInterface`
  // 这个类用于在我们的虚拟环境中增加/移除碰撞检测障碍物
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  // 原始指针经常被用来指代计划组以提高性能。
  const robot_state::JointModelGroup *joint_model_group =
    move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  // Visualization
  // ^^^^^^^^^^^^^
  //MoveItVisualTools包在rviz中提供了许多障碍物、机器人和轨迹的可视化的能力，
  //还有像脚本的一步一步的自我检查的调试工具
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools("odom_combined");
  visual_tools.deleteAllMarkers();

  // Remote control 远程控制是一种自查工具，允许用户通过Rviz中的按钮和键盘快捷键来浏览高级脚本
  visual_tools.loadRemoteControl();

  // Rviz 提供了许多marker的类型, 这里我们主要用到了 text, cylinders, and spheres
  Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
  text_pose.translation().z() = 1.75; // above head of PR2，文字的位置放在了PR2的头上
  visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);

  // Batch publishing，批量发布用于减少发送到Rviz的消息数量，以实现大型可视化
  visual_tools.trigger();

  // Getting Basic Information
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  //我们可以打印这个机器人的参考坐标系的名称。
  ROS_INFO_NAMED("tutorial", "Reference frame: %s", move_group.getPlanningFrame().c_str());

  // 我们也可以打印这个组的末端执行器连杆的名称。
  ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

  // Planning to a Pose goal
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // 我们可以为这个group制定一个动作，使其成为末端所需的姿势。
  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 1.0;
  target_pose1.position.x = 0.28;
  target_pose1.position.y = -0.7;
  target_pose1.position.z = 1.0;
  move_group.setPoseTarget(target_pose1);

  // 现在，我们打电话给planner来计算计划并将其可视化。 请注意，我们只是在计划，而不是要求move_group实际移动机器人。
  // 哈哈哈哈，打电话……想起了吐槽大会
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");

  // Visualizing plans
  // ^^^^^^^^^^^^^^^^^
  // 我们也可以把这个计划在Rviz中用一条带有标记的线来进行可视化。
  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
  visual_tools.publishAxisLabeled(target_pose1, "pose1");
  visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("next step");

  // Moving to a pose goal
  // ^^^^^^^^^^^^^^^^^^^^^
  //除了我们现在使用move()函数之外，移动到一个姿势目标与上面的步骤类似。 
  //请注意，我们之前设置的姿态目标仍然是活跃的，所以机器人将尝试移动到该目标。 
  //我们不会在本教程中使用该函数，因为它是一个阻塞函数，需要一个控制器处于活动状态，并在执行轨迹时报告成功。
  /* 在实际控制机器人时取消下面这一行的注释 */
  /* move_group.move() */

  // Planning to a joint-space goal
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //关节空间的运动规划。这将取代我们上面设置的姿态目标。
  // 首先，我们将创建一个引用当前机器人状态的指针。 
  // RobotState是包含所有当前位置/速度/加速度数据的对象。
  moveit::core::RobotStatePtr current_state = move_group.getCurrentState();

  // 接下来获取组的当前关节值集合。
  std::vector<double> joint_group_positions;
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

  // 现在，让我们修改其中一个关节，计划到新的关节空间目标并将计划可视化。
  joint_group_positions[0] = -1.0;  // 弧度
  move_group.setJointValueTarget(joint_group_positions);

  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");

  // Visualize the plan in Rviz
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("next step");

  // Planning with Path Constraints
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //可以轻松地为机器人上的连杆指定路径约束。 
  //让我们为我们的组指定路径约束和姿势目标。 
  //首先定义路径约束。
  moveit_msgs::OrientationConstraint ocm;
  ocm.link_name = "r_wrist_roll_link";
  ocm.header.frame_id = "base_link";
  ocm.orientation.w = 1.0;
  ocm.absolute_x_axis_tolerance = 0.1;
  ocm.absolute_y_axis_tolerance = 0.1;
  ocm.absolute_z_axis_tolerance = 0.1;
  ocm.weight = 1.0;

  // 现在，将其设置为组的路径约束。
  moveit_msgs::Constraints test_constraints;
  test_constraints.orientation_constraints.push_back(ocm);
  move_group.setPathConstraints(test_constraints);

  // 我们将重复使用我们已经计划的旧目标。 请注意，只有在当前状态已经满足路径约束的情况下，这才会起作用。（显然当前状态不符合约束） 
  // 所以，我们需要将开始状态设置为一个新的姿势。
  robot_state::RobotState start_state(*move_group.getCurrentState());
  geometry_msgs::Pose start_pose2;
  start_pose2.orientation.w = 1.0;
  start_pose2.position.x = 0.55;
  start_pose2.position.y = -0.05;
  start_pose2.position.z = 0.8;
  start_state.setFromIK(joint_model_group, start_pose2);
  move_group.setStartState(start_state);

  // 按照新的起始位置和原来的目标位置进行规划
  move_group.setPoseTarget(target_pose1);

  // 用约束进行规划可能会很慢，因为每个样本必须调用一个反向运动学求解器。
  // 让我们从默认的5秒增加计划时间，以确保计划者有足够的时间成功。
  move_group.setPlanningTime(10.0);

  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");

  // Visualize the plan in Rviz
  visual_tools.deleteAllMarkers();
  visual_tools.publishAxisLabeled(start_pose2, "start");
  visual_tools.publishAxisLabeled(target_pose1, "goal");
  visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("next step");

  //当完成路径约束时，一定要清除约束。
  move_group.clearPathConstraints();

  // Cartesian Paths
  // ^^^^^^^^^^^^^^^
  // 您可以通过指定末端路点（waypoint）列表直接规划笛卡尔路径。 请注意，我们从上面的新开始状态开始。 
  // 初始姿势（开始状态）不需要添加到航点（waypoint）列表，但添加它可以帮助进行可视化
  std::vector waypoints;
  waypoints.push_back(start_pose2);

  geometry_msgs::Pose target_pose3 = start_pose2;

  target_pose3.position.z += 0.2;
  waypoints.push_back(target_pose3);  // up 

  target_pose3.position.y -= 0.1;
  waypoints.push_back(target_pose3);  // left

  target_pose3.position.z -= 0.2;
  target_pose3.position.y += 0.2;
  target_pose3.position.x -= 0.2;
  waypoints.push_back(target_pose3);  // down and right 

  // 经常需要笛卡尔运动，例如接近和撤回抓握动作等。 
  // 在这里，我们演示如何通过每个关节的最大速度的缩放因子来降低机器人手臂的速度。 请注意，这不是最终效应器的速度。
  move_group.setMaxVelocityScalingFactor(0.1);

  // 我们希望以1厘米的分辨率插入笛卡尔路径，这就是为什么我们将0.01指定为笛卡儿平移中的最大步长。 
  // 我们将跳转阈值指定为0.0，有效地禁用它。 
  // 警告 - 在操作真实硬件时禁用跳转阈值可能导致冗余连接发生大量不可预测的动作，这可能是一个安全问题
  moveit_msgs::RobotTrajectory trajectory;
  const double jump_threshold = 0.0;
  const double eef_step = 0.01;
  double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (cartesian path) (%.2f%% acheived)", fraction * 100.0);

  // Visualize the plan in Rviz
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Cartesian Paths", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
  for (std::size_t i = 0; i < waypoints.size(); ++i)
    visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);//显示坐标系
  visual_tools.trigger();
  visual_tools.prompt("next step");

  // Adding/Removing Objects and Attaching/Detaching Objects
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 定义一个碰撞对象ROS消息。
  moveit_msgs::CollisionObject collision_object;
  collision_object.header.frame_id = move_group.getPlanningFrame();
  collision_object.id = "box1";// 该对象的ID用于标识它。

  // Define a box to add to the world.
  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[0] = 0.4;
  primitive.dimensions[1] = 0.1;
  primitive.dimensions[2] = 0.4;

  //为盒子定义姿势（相对于frame_id）
  geometry_msgs::Pose box_pose;
  box_pose.orientation.w = 1.0;
  box_pose.position.x = 0.6;
  box_pose.position.y = -0.4;
  box_pose.position.z = 1.2;

  collision_object.primitives.push_back(primitive);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD;

  std::vector collision_objects;
  collision_objects.push_back(collision_object);

  // 现在，让我们添加碰撞对象到世界中
  ROS_INFO_NAMED("tutorial", "Add an object into the world");
  planning_scene_interface.addCollisionObjects(collision_objects);

  // Show text in Rviz of status
  visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  // 停1s以允许move group接收并处理碰撞对象消息
  ros::Duration(1.0).sleep();

  // 现在当我们计划一个轨迹时，它将避开障碍物
  move_group.setStartState(*move_group.getCurrentState());
  move_group.setPoseTarget(target_pose1);

  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (pose goal move around cuboid) %s", success ? "" : "FAILED");

  // Visualize the plan in Rviz
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("next step");//按钮？

  // 现在，让我们将碰撞对象附加到机器人。碰撞对象会变紫色，干什么用我还没学到
  ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
  move_group.attachObject(collision_object.id);

  // Show text in Rviz of status
  visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* Sleep to allow MoveGroup to recieve and process the attached collision object message */
  ros::Duration(1.0).sleep();
  visual_tools.prompt("next step");//按钮？

  // 现在，让我们从机器人分离碰撞对象。
  ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
  move_group.detachObject(collision_object.id);

  // Show text in Rviz of status
  visual_tools.publishText(text_pose, "Object dettached from robot", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* Sleep to allow MoveGroup to recieve and process the detach collision object message */
  ros::Duration(1.0).sleep();

  // Now, let's remove the collision object from the world.
  ROS_INFO_NAMED("tutorial", "Remove the object from the world");
  std::vector<std::string> object_ids;
  object_ids.push_back(collision_object.id);
  planning_scene_interface.removeCollisionObjects(object_ids);

  // Show text in Rviz of status
  visual_tools.publishText(text_pose, "Object removed", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* Sleep to give Rviz time to show the object is no longer there.*/
  ros::Duration(1.0).sleep();
  visual_tools.prompt("next step");//按钮？

  // Dual-arm pose goals
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  // 首先定义一个新的小组来处理两只手臂。
  static const std::string PLANNING_GROUP2 = "arms";
  moveit::planning_interface::MoveGroupInterface two_arms_move_group(PLANNING_GROUP2);

  // 定义两个独立的姿态目标，每个末端执行器一个。 请注意，我们正在重新使用右上方的目标
  two_arms_move_group.setPoseTarget(target_pose1, "r_wrist_roll_link");

  geometry_msgs::Pose target_pose4;
  target_pose4.orientation.w = 1.0;
  target_pose4.position.x = 0.7;
  target_pose4.position.y = 0.15;
  target_pose4.position.z = 1.0;

  two_arms_move_group.setPoseTarget(target_pose4, "l_wrist_roll_link");

  // Now, we can plan and visualize
  moveit::planning_interface::MoveGroupInterface::Plan two_arms_plan;

  success = (two_arms_move_group.plan(two_arms_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 7 (dual arm plan) %s", success ? "" : "FAILED");

  // Visualize the plan in Rviz
  visual_tools.deleteAllMarkers();
  visual_tools.publishAxisLabeled(target_pose1, "goal1");
  visual_tools.publishAxisLabeled(target_pose4, "goal2");
  visual_tools.publishText(text_pose, "Two Arm Goal", rvt::WHITE, rvt::XLARGE);
  joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP2);
  visual_tools.publishTrajectoryLine(two_arms_plan.trajectory_, joint_model_group);
  visual_tools.trigger();

  // END_TUTORIAL

  ros::shutdown();
  return 0;
}

---