此博文主要是用来记录ROS-Kinetic 中，用于机器人轨迹规划的MoveIt功能包的学习记录。
引：这个教程主要是通过Moveit工具箱，并且使用MoveGroup Interface， 同时创建一个 moveit_msgs::Grasp对象，实现机器人抓取物品，执行轨迹，并放下物品的全过程仿真。
关注点： 这个教程不仅仅是关于抓取和放下，同时也包含了RVIZ可视化，与机器人的运动学规划，非常具有学习意义。
第一部分： 了解 moveit_msgs/Grasp.msg.
定义：这是一个消息类， .msg文件是ROS在进行Topic（话题通信时），所预先定义的传送信息内容的预设格式。
其中包含内容：
（1）trajectory_msgs/JointTrajectory pre_grasp_posture： 这个变量定义了，机器人在进行抓取之前，在end_effector末端执行器这个运动组（group）所对应的机器人的轨迹位置。-----实际上就是抓之前的位姿。
（2）trajectory_msgs/JointTrajectory grasp_posture： 这个变量定义了，机器人在进行抓取时，在end_effector末端执行器这个运动组（group）所对应的机器人的轨迹位置。-----实际上就是抓取时的位姿
（3）geometry_msgs/PoseStamped grasp_pose： 这个是一个Pose变量，定义了在机器人尝试抓取时，末端执行其的pose（位置坐标+转角）
（4）moveit_msgs/GripperTranslation pre_grasp_approach： 这个变量定义了，在进行抓取之前，机器人末端的前进方向，以及前进距离；（从哪个方向接近被抓物体？ 沿着这个方向，前进多少距离）
（5）moveit_msgs/GripperTranslation post_grasp_retreat： 这个变量定义了，在进行抓取到了之后，机器人末端的回撤方向，以及在这个回撤方向上回撤距离。（抓到了物体，从哪个方向离开？ 离开距离是多少？）
（6）moveit_msgs/GripperTranslation post_place_retreat： 这个变量定义了，在进行放下了物体之后，机器人末端的回撤方向，以及在这个回撤方向上回撤距离。（放下了物体，从哪个方向离开？ 离开距离是多少？）
第二部分： 源代码
-> 官方教程主要以代码实例为主，所以，在下边的代码中，主要通过注释的方式，解释代码含义，通过代码实例，学习MoveIt内部内容。
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2012, Willow Garage, Inc.
* 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 Willow Garage 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: Ioan Sucan, Ridhwan Luthra*/
// ROS
#include <ros/ros.h>
// MoveIt!
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
// TF2
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
void openGripper(trajectory_msgs::JointTrajectory& posture)
{
// BEGIN_SUB_TUTORIAL open_gripper
/* Add both finger joints of panda robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2";
/* Set them as open, wide enough for the object to fit. */
posture.points.resize(1);
posture.points[0].positions.resize(2);
posture.points[0].positions[0] = 0.04;
posture.points[0].positions[1] = 0.04;
posture.points[0].time_from_start = ros::Duration(0.5);
// END_SUB_TUTORIAL
}
void closedGripper(trajectory_msgs::JointTrajectory& posture)
{
// BEGIN_SUB_TUTORIAL closed_gripper
/* Add both finger joints of panda robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2";
/* Set them as closed. */
posture.points.resize(1);
posture.points[0].positions.resize(2);
posture.points[0].positions[0] = 0.00;
posture.points[0].positions[1] = 0.00;
posture.points[0].time_from_start = ros::Duration(0.5);
// END_SUB_TUTORIAL
}
//×××××××××××××××××××××××××××××××Part2： 抓取物品××××××××××××××××××××××××××××××××××××××××
void pick(moveit::planning_interface::MoveGroupInterface& move_group)
{
// BEGIN_SUB_TUTORIAL pick1
// Create a vector of grasps to be attempted, currently only creating single grasp.
// This is essentially useful when using a grasp generator to generate and test multiple grasps.
// 设定抓取对象，并且抓取尝试次数设置为1
std::vector<moveit_msgs::Grasp> grasps;
grasps.resize(1);
// Setting grasp pose
// ++++++++++++++++++++++
// This is the pose of panda_link8. |br|
// From panda_link8 to the palm of the eef the distance is 0.058, the cube starts 0.01 before 5.0 (half of the length
// of the cube). |br|
// Therefore, the position for panda_link8 = 5 - (length of cube/2 - distance b/w panda_link8 and palm of eef - some
// extra padding)
// Step2.1 设定最终抓取位置（四元数形式表示，xyz+orientation）
grasps[0].grasp_pose.header.frame_id = "panda_link0";
tf2::Quaternion orientation;
orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2);
grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);
grasps[0].grasp_pose.pose.position.x = 0.415;
grasps[0].grasp_pose.pose.position.y = 0;
grasps[0].grasp_pose.pose.position.z = 0.5;
// Setting pre-grasp approach
// ++++++++++++++++++++++++++
/* Defined with respect to frame_id */
// Step2.2 设置预抓取位置（xyz，相对于grasp——pose而言）
grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
/* Direction is set as positive x axis */
grasps[0].pre_grasp_approach.direction.vector.x = 1.0;
grasps[0].pre_grasp_approach.min_distance = 0.095;
grasps[0].pre_grasp_approach.desired_distance = 0.115;
// Setting post-grasp retreat
// ++++++++++++++++++++++++++
/* Defined with respect to frame_id */
// Step2.3 设定抓取完毕以后，撤出的位置（抓取后，撤出多远？沿着哪个轴？）
grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
/* Direction is set as positive z axis */
grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
grasps[0].post_grasp_retreat.min_distance = 0.1;
grasps[0].post_grasp_retreat.desired_distance = 0.25;
// Setting posture of eef before grasp
// +++++++++++++++++++++++++++++++++++
// Step2.4 在预先抓取位置，打开夹爪
openGripper(grasps[0].pre_grasp_posture);
// END_SUB_TUTORIAL
// BEGIN_SUB_TUTORIAL pick2
// Setting posture of eef during grasp
// +++++++++++++++++++++++++++++++++++
// Step2.5 在抓取位置，和上夹爪
closedGripper(grasps[0].grasp_posture);
// END_SUB_TUTORIAL
// BEGIN_SUB_TUTORIAL pick3
// Set support surface as table1.
move_group.setSupportSurfaceName("table1");
// Call pick to pick up the object using the grasps given
move_group.pick("object", grasps);
// END_SUB_TUTORIAL
}
void place(moveit::planning_interface::MoveGroupInterface& group)
{
// BEGIN_SUB_TUTORIAL place
// TODO(@ridhwanluthra) - Calling place function may lead to "All supplied place locations failed. Retrying last
// location in
// verbose mode." This is a known issue and we are working on fixing it. |br|
// Create a vector of placings to be attempted, currently only creating single place location.
//与pick类似，也是设置最终的放下位置，设置放下之前的接近位置，以及放下之后的回撤的位置
std::vector<moveit_msgs::PlaceLocation> place_location;
place_location.resize(1);
// Setting place location pose
// +++++++++++++++++++++++++++
place_location[0].place_pose.header.frame_id = "panda_link0";
tf2::Quaternion orientation;
orientation.setRPY(0, 0, M_PI / 2);
place_location[0].place_pose.pose.orientation = tf2::toMsg(orientation);
/* While placing it is the exact location of the center of the object. */
place_location[0].place_pose.pose.position.x = 0;
place_location[0].place_pose.pose.position.y = 0.5;
place_location[0].place_pose.pose.position.z = 0.5;
// Setting pre-place approach
// ++++++++++++++++++++++++++
/* Defined with respect to frame_id */
place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
/* Direction is set as negative z axis */
place_location[0].pre_place_approach.direction.vector.z = -1.0;
place_location[0].pre_place_approach.min_distance = 0.095;
place_location[0].pre_place_approach.desired_distance = 0.115;
// Setting post-grasp retreat
// ++++++++++++++++++++++++++
/* Defined with respect to frame_id */
place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
/* Direction is set as negative y axis */
place_location[0].post_place_retreat.direction.vector.y = -1.0;
place_location[0].post_place_retreat.min_distance = 0.1;
place_location[0].post_place_retreat.desired_distance = 0.25;
// Setting posture of eef after placing object
// +++++++++++++++++++++++++++++++++++++++++++
/* Similar to the pick case */
openGripper(place_location[0].post_place_posture);
// Set support surface as table2.
group.setSupportSurfaceName("table2");
// Call place to place the object using the place locations given.
group.place("object", place_location);
// END_SUB_TUTORIAL
}
void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface)
{
// BEGIN_SUB_TUTORIAL table1
//
// Step1.1 创建一个Vector， 并且设置容量为3,用来保存三个objects
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.resize(3);
//×××××××××××××××××××××××××××××××第一个table（box）××××××××××××××××××××××××××××××××××××××××
// Step 1.2 创建第一个table，被抓取的物品放在其上
// Add the first table where the cube will originally be kept.
collision_objects[0].id = "table1";
collision_objects[0].header.frame_id = "panda_link0";
/* Define the primitive（预设值） and its dimensions. */
// 设置第一个object的类型为box，尺寸为0.2×0.4×0.4
collision_objects[0].primitives.resize(1);
collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
collision_objects[0].primitives[0].dimensions.resize(3);
collision_objects[0].primitives[0].dimensions[0] = 0.2;
collision_objects[0].primitives[0].dimensions[1] = 0.4;
collision_objects[0].primitives[0].dimensions[2] = 0.4;
/* Define the pose of the table. */
//设置第一个object的摆放位置
collision_objects[0].primitive_poses.resize(1);
collision_objects[0].primitive_poses[0].position.x = 0.5;
collision_objects[0].primitive_poses[0].position.y = 0;
collision_objects[0].primitive_poses[0].position.z = 0.2;
// END_SUB_TUTORIAL
// 将这个box的object，添加到仿真环境中
collision_objects[0].operation = collision_objects[0].ADD;
//×××××××××××××××××××××××××××××××第二个table（box）××××××××××××××××××××××××××××××××××××××××
//具体过程与box1上边类似
// BEGIN_SUB_TUTORIAL table2
// Add the second table where we will be placing the cube.
collision_objects[1].id = "table2";
collision_objects[1].header.frame_id = "panda_link0";
/* Define the primitive and its dimensions. */
collision_objects[1].primitives.resize(1);
collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
collision_objects[1].primitives[0].dimensions.resize(3);
collision_objects[1].primitives[0].dimensions[0] = 0.4;
collision_objects[1].primitives[0].dimensions[1] = 0.2;
collision_objects[1].primitives[0].dimensions[2] = 0.4;
/* Define the pose of the table. */
collision_objects[1].primitive_poses.resize(1);
collision_objects[1].primitive_poses[0].position.x = 0;
collision_objects[1].primitive_poses[0].position.y = 0.5;
collision_objects[1].primitive_poses[0].position.z = 0.2;
// END_SUB_TUTORIAL
collision_objects[1].operation = collision_objects[1].ADD;
//×××××××××××××××××××××××××××××××第三个table（box）--被抓取的那个××××××××××××××××××××××××××××××××××××××××
//具体过程与box1上边类似
// BEGIN_SUB_TUTORIAL object
// Define the object that we will be manipulating
collision_objects[2].header.frame_id = "panda_link0";
collision_objects[2].id = "object";
/* Define the primitive and its dimensions. */
collision_objects[2].primitives.resize(1);
collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;
collision_objects[2].primitives[0].dimensions.resize(3);
collision_objects[2].primitives[0].dimensions[0] = 0.02;
collision_objects[2].primitives[0].dimensions[1] = 0.02;
collision_objects[2].primitives[0].dimensions[2] = 0.2;
/* Define the pose of the object. */
collision_objects[2].primitive_poses.resize(1);
collision_objects[2].primitive_poses[0].position.x = 0.5;
collision_objects[2].primitive_poses[0].position.y = 0;
collision_objects[2].primitive_poses[0].position.z = 0.5;
// END_SUB_TUTORIAL
collision_objects[2].operation = collision_objects[2].ADD;
//重要！！在设置完全部object之后，记得将这些object添加到planning_scene这个场景中。
planning_scene_interface.applyCollisionObjects(collision_objects);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "panda_arm_pick_place");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
ros::WallDuration(1.0).sleep(); //休眠1s，若休眠被打断，在休眠恢复后，将重新休眠1s
moveit::planning_interface::PlanningSceneInterface planning_scene_interface; //创建planning_scene
moveit::planning_interface::MoveGroupInterface group("panda_arm"); //创建运动学规划组，并指定规划组为"panda_arm"
group.setPlanningTime(45.0); //设定运动学规划时间：45秒
//Part1： 创建实验台+被抓取物体 （在这个planning_scene规划场景中添加物品）
addCollisionObjects(planning_scene_interface); //×！调用子函数，添加碰撞物体（RVIZ中的实验台+被抓取物品）
// Wait a bit for ROS things to initialize
ros::WallDuration(1.0).sleep();
//Part2：抓取物体（使用panda_arm这一个运动group，抓取）
pick(group);
ros::WallDuration(1.0).sleep();
//Part3：放下物体
place(group);
ros::waitForShutdown();
return 0;
}
// BEGIN_TUTORIAL
// CALL_SUB_TUTORIAL table1
// CALL_SUB_TUTORIAL table2
// CALL_SUB_TUTORIAL object
//
// Pick Pipeline
// ^^^^^^^^^^^^^
// CALL_SUB_TUTORIAL pick1
// openGripper function
// """"""""""""""""""""
// CALL_SUB_TUTORIAL open_gripper
// CALL_SUB_TUTORIAL pick2
// closedGripper function
// """"""""""""""""""""""
// CALL_SUB_TUTORIAL closed_gripper
// CALL_SUB_TUTORIAL pick3
//
// Place Pipeline
// ^^^^^^^^^^^^^^
// CALL_SUB_TUTORIAL place
// END_TUTORIAL