此博文主要是用来记录ROS-Kinetic 中，用于机器人轨迹规划的MoveIt功能包的学习记录。
引： MoveIt是基于MoveGroup这个类，MoveIt提供了一个相对简单的方式，令操作人员可以较为容易的操作机器人。 操作人员仅需发送各个关节的指定角度或者TCP的目标位置，即可控制机器人执行指令，移动到位。 MoveIt是通过ROS内部的Topic/Service和Action机制，向MoveGroup的节点发送指令。
主要内容：
在上面的链接教程中，主要涉及到了C++的MoveIt！的接口。
以及如何创建规划组Move_Group，如何创建关节类型/目标点类型的轨迹，并使用Moveit自带规划器进行轨迹规划。 并且在RVIZ中显示出来规划完成的轨迹。
官方教程主要以代码实例为主，所以，在下边的代码中，主要通过注释的方式，解释代码含义，通过代码实例，学习MoveIt内部内容。
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, SRI International
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of SRI International nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Sachin Chitta, Dave Coleman, Mike Lautman */
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// BEGIN_TUTORIAL
//
// Setup
// ^^^^^
//
// MoveIt! operates on sets of joints called "planning groups" and stores them in an object called
// the `JointModelGroup`. Throughout MoveIt! the terms "planning group" and "joint model group"
// are used interchangably.设定运动规划组名称
static const std::string PLANNING_GROUP = "panda_arm";
// The :move_group_interface:`MoveGroup` class can be easily
// setup using just the name of the planning group you would like to control and plan for.
//初始化运动规划组，并将前面设置的名称输入进来
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
// We will use the :planning_scene_interface:`PlanningSceneInterface`
// class to add and remove collision objects in our "virtual world" scene
// 创建一个对象，用于添加和移除在仿真环境中的‘碰撞元件’（用来进行避障的轨迹仿真）
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
// Raw pointers are frequently used to refer to the planning group for improved performance.
// 创建一个指针类型变量，用来表示机器人在当前规划组的状态（可以提高效率）
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Visualization
// ^^^^^^^^^^^^^
//
// The package MoveItVisualTools provides many capabilties for visualizing objects, robots,
// and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script
//RVIZ可视化工具
namespace rvt = rviz_visual_tools;// 使用rvt来代表 程序库中的 rviz_visual_tools，以便简洁
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); //设定RVIZ visualtool
visual_tools.deleteAllMarkers(); //删除rviz内所有的标记
// Remote control is an introspection tool that allows users to step through a high level script
// via buttons and keyboard shortcuts in RViz
//Remote control的主要作用是： 允许操作人员通过RVIZ内建的按钮和键盘快捷键进行控制
visual_tools.loadRemoteControl();
// RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
//RVIZ中，可以使用很多种类的标记（标识类型）， 在Demo中，使用了文本+圆柱标记+表面类型。
Eigen::Affine3d text_pose = Eigen::Affine3d::Identity(); //创建一个text_pose 类型是仿射矩阵，并赋值为单位阵
text_pose.translation().z() = 1.75; //设置translation z=1.75
visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
//通过visual——tool对象发布信息（汇总一批信息后，统一发送可以减少数据量，提升效率）
visual_tools.trigger();
// Getting Basic Information
// ^^^^^^^^^^^^^^^^^^^^^^^^^
//
//使用ROS_INFO这个功能，在命令行中显示move_group的信息（规划组名称/末端执行器连杆名称等）
// We can print the name of the reference frame for this robot.
ROS_INFO_NAMED("tutorial", "Reference frame: %s", move_group.getPlanningFrame().c_str());
// We can also print the name of the end-effector link for this group.
ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());
// Start the demo
// ^^^^^^^^^^^^^^^^^^^^^^^^^
//！！至此正式开始仿真，下边的语句会在命令行中显示“等待按下next键”，并等待用户按下rviz内部的next按键，以便继续运行。
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
/**Section 1: 规划一个三维空间XYZW四元数坐标点，并规划执行**/
/**Part2 重要！！！ 此后主要是进行目标点设置+规划，并采集路径规划成功与否**/
// Planning to a Pose goal
// ^^^^^^^^^^^^^^^^^^^^^^^
// We can plan a motion for this group to a desired pose for the
// end-effector.
// Step1：创建一个geometry_msgs::Pose对象，用于存储目标点位置（四元数）
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.2;
target_pose1.position.z = 0.5;
move_group.setPoseTarget(target_pose1);//！！将设置的目标点，作为输入参数存入move_group规划组中
// Now, we call the planner to compute the plan and visualize it.
// Note that we are just planning, not asking move_group
// to actually move the robot.
// Step2：创建一个‘Plan对象’，用于规划到上边设置的目标点轨迹
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
// Step3： 执行move_group.中的轨迹规划指令，并且采集ERROR_Code ,检查是否运行成功
// ！！实际上在运行move_group.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");
/**Part3 上边的轨迹规划完毕之后，需要进行可视化显示**/
// 具体过程为： 在命令行中显示目标点信息/目标点内容/
// Visualizing plans
// ^^^^^^^^^^^^^^^^^
// We can also visualize the plan as a line with markers in RViz.
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
visual_tools.publishAxisLabeled(target_pose1, "pose1"); //在rviz中，显示目标点，显示名称为“pose1”
visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE); //在RVIZ中，显示目标点信息
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); //在RVIZ中，显示机器人轨迹线
visual_tools.trigger(); //将上边三条语句，统一执行
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");//在GUI界面中，按按钮继续执行程序。
/**Part4 上边语句已经完成了轨迹规划+在rviz中显示机器人轨迹，下边的内容控制机器人执行这个轨迹**/
// Moving to a pose goal
// Moving to a pose goal is similar to the step above
// except we now use the move() function. Note that
// the pose goal we had set earlier is still active
// and so the robot will try to move to that goal. We will
// not use that function in this tutorial since it is
// a blocking function and requires a controller to be active
// and report success on execution of a trajectory.
// 注意！！ 下边的语句是一个阻塞型语句，需要真实的机器人执行，执行完毕后需要回传完成信号才可以；
// 因此在这个教程中，不使用这个语句
/* Uncomment below line when working with a real robot */
/* move_group.move(); */
/**Section 2: 规划一个轴空间目标点（规定每个轴的转角），并执行**/
// Part1：设定一个轴空间坐标点，并存到Move_Group中（将会把之前的pose点给替代掉）
// Planning to a joint-space goal
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// Let's set a joint space goal and move towards it. This will replace the
// pose target we set above.
//
// To start, we'll create an pointer that references the current robot's state.
// RobotState is the object that contains all the current position/velocity/acceleration data.
// Step1.1： 首先创建一个指针对象current_state，并将当前机器人位置/速度/加速度等设置信息进行存储。
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
// Step1.2 创建一个double类型数组，并将上边得到的current_state中的机器人各轴坐标提取出来，存入其中
// Next get the current set of joint values for the group.
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
// Step1.2 在上边得到的存有机器人各关节信息的数组，将第1个轴的坐标改为-1.0；
// 之后使用修改后的关节数组，作为轴空间的目标点，设置到move_group中
// 之后使用move_group.plan这个方法，进行规划
joint_group_positions[0] = -1.0; // radians
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();//清除RVIZ环境内的其他痕迹
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("Press 'next' in the RvizVisualToolsGui window to continue the demo");//等待操作人员按键
/**Section 3: 规划一个坐标空间的轨迹，并且设置轨迹的约束条件，并执行**/
// Step3.1：定义一个轨迹规划的约束条件
// Planning with Path Constraints
// Path constraints can easily be specified for a link on the robot.
// Let's specify a path constraint and a pose goal for our group.
// First define the path constraint.
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "panda_link7"; //设置被约束的link
ocm.header.frame_id = "panda_link0"; //设置base_link，就是约束是想对于哪个指定的？
ocm.orientation.w = 1.0; //设置角度约束条件？ TODO：W是什么？
ocm.absolute_x_axis_tolerance = 0.1; //设置xyz轴的各轴允许的最大误差
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.1;
ocm.weight = 1.0; //设置这个约束所占的比重，当有很多其他约束时，所占比重越高的，优先级越高。
//越接近0,所占优先级越低
// Step3.2：将上边设置好的约束条件，应用在规划组中。
// Now, set it as the path constraint for the group.
moveit_msgs::Constraints test_constraints; //定义一个总的约束对象，并将上边方向约束添加到其中。
test_constraints.orientation_constraints.push_back(ocm);
move_group.setPathConstraints(test_constraints); //！！此处是给move_group设置约束！！
// Step3.3：设置新的起始点坐标
// We will reuse the old goal that we had and plan to it.
// Note that this will only work if the current state already
// satisfies the path constraints. So, we need to set the start
// state to a new pose.
robot_state::RobotState start_state(*move_group.getCurrentState()); //创建一个start_state对象，并将当前的机器人坐标设置为下一个运动的起点
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,然后设置新的位姿start——pose2作为新的起点;
// 尝试通过运动学逆解，从当前位置，回到设定的start_pose2这个坐标点
start_state.setFromIK(joint_model_group, start_pose2);
move_group.setStartState(start_state); //将新定义的start_pose2作为新的起始点。
// Step3.4： 设置新的终点位姿坐标，并将其加载到move_group里面（这个终点位姿与第一个运动规划终点一致）
// Now we will plan to the earlier pose target from the new
// start state that we have just created.
move_group.setPoseTarget(target_pose1);
// Step3.5： 由于设置了move_group的约束条件，路径规划的时间会变长（因为从当前位姿移动到新的起点时，已经应用了约束条件，所以每次均需要进行逆运动学求解，时间长）
// 同时，默认的规划时间为5秒，可能不够，所以把路径规划时间延长至10s
// Planning with constraints can be slow because every sample must call an inverse kinematics solver.
// Lets increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed.
move_group.setPlanningTime(10.0);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); //把move_group中的轨迹进行规划，并存储在my_plan内！！！！
ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");
// Step3.6：将规划好的起始点/目标点+轨迹显示在rviz中
// 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"); //等待操作人员操作按钮或者键盘
// Step3.7：当规划完带有约束条件的轨迹时，并且执行完毕后，记得一定要清除所有的约束条件
// When done with the path constraint be sure to clear it.
move_group.clearPathConstraints();
// Step3.8：清除start state以便于能够进行后续的规划
// Since we set the start state we have to clear it before planning other paths
move_group.setStartStateToCurrentState();
/**Section 4: 规划一个迪卡尔坐标系下的机器人轨迹，这个实现方式是设置很多路径点，让机器人依次执行！！！重要**/
// Cartesian Paths
// ^^^^^^^^^^^^^^^
// You can plan a Cartesian path directly by specifying a list of waypoints
// for the end-effector to go through. Note that we are starting
// from the new start state above. The initial pose (start state) does not
// need to be added to the waypoint list but adding it can help with visualizations
// Step4.1：设置路径点
geometry_msgs::Pose target_pose3 = move_group.getCurrentPose().pose; //首先将当前位姿，存入新的pose对象中。
std::vector<geometry_msgs::Pose> waypoints; //设置一个存储格式为<geometry_msgs::Pose> 的vector对象，用于存储路径点！！
waypoints.push_back(target_pose3);
target_pose3.position.z -= 0.2;
waypoints.push_back(target_pose3); // down
target_pose3.position.y -= 0.2;
waypoints.push_back(target_pose3); // right
target_pose3.position.z += 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3); // up and left
// Step4.2：设置机器人运动减速指标（展示了如何通过标量参数，设置每个Joint的最大速度）
// Cartesian motions are frequently needed to be slower for actions such as approach and retreat
// grasp motions. Here we demonstrate how to reduce the speed of the robot arm via a scaling factor
// of the maxiumum speed of each joint. Note this is not the speed of the end effector point.
move_group.setMaxVelocityScalingFactor(0.1); //设置一个速度的标量系数！
// Step4.3：设置机器人运动分辨率，设置1cm是机器人的运动步长（也就是下边的0.01的意思）
// We want the Cartesian path to be interpolated at a resolution of 1 cm
// which is why we will specify 0.01 as the max step in Cartesian
// translation. We will specify the jump threshold as 0.0, effectively disabling it.
// Warning - disabling the jump threshold while operating real hardware can cause
// large unpredictable motions of redundant joints and could be a safety issue
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);// 将上边的步长+参数+轨迹点输入； 最终的轨迹存入到trajectory中！！
ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);
// Step4.4：在RVIZ中，显示规划好的轨迹
// Visualize the plan in RViz
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Joint Space Goal", 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("Press 'next' in the RvizVisualToolsGui window to continue the demo");
/**Section 5: 在 RVIZ的仿真环境中，添加collison的object，就是仿真里面的box，用来做轨迹规划时的避障！！！重要**/
// Adding/Removing Objects and Attaching/Detaching Objects
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// Step5.1 ：首先定义一个模拟障碍物的对象
// Define a collision object ROS message.
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group.getPlanningFrame(); //定义障碍物的frame id 用来确定障碍物放置位置
// The id of the object is used to identify it.
collision_object.id = "box1"; //定义障碍物的本体id，用来定位，并且用来区分
// Step5.2 ：创建一个Box对象，包含：大小尺寸，放置位置
// 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;
//Step5.3: 设定box的放置位置
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.4;
box_pose.position.y = -0.2;
box_pose.position.z = 1.0;
//Step5.4: 将刚才设置好的box尺寸，位置，分别加载到障碍物对象里（collision_object）
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
// Now, let's add the collision object into the world
ROS_INFO_NAMED("tutorial", "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects); //在RVIZ环境中，加载这个障碍物
// Show text in RViz of status
visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
// Wait for MoveGroup to recieve and process the collision object message
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
//Step5.5: 障碍物设置好之后，规划一个避障轨迹
// Now when we plan a trajectory it will avoid the obstacle
move_group.setStartState(*move_group.getCurrentState());
geometry_msgs::Pose another_pose;
another_pose.orientation.w = 1.0;
another_pose.position.x = 0.4;
another_pose.position.y = -0.4;
another_pose.position.z = 0.9;
move_group.setPoseTarget(another_pose);
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");
// Now, let's attach the collision object to the robot.
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();
/* Wait for MoveGroup to recieve and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object attaches to the "
"robot");
// Now, let's detach the collision object from the robot.
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();
/* Wait for MoveGroup to recieve and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object detaches to the "
"robot");
// 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();
/* Wait for MoveGroup to recieve and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");
// END_TUTORIAL
ros::shutdown();
return 0;
}
版权声明：本文为weixin_42503785原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。