MoveIt! 学习笔记11

MoveIt! 学习笔记11

此博文主要是用来记录ROS-Kinetic 中，用于机器人轨迹规划的MoveIt功能包的学习记录。 

英文原版教程见此链接：.html

引：这个教程主要是通过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.size(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. */size(1);posture.points[0].size(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.size(2);posture.joint_names[0] = "panda_finger_joint1";posture.joint_names[1] = "panda_finger_joint2";/* Set them as closed. */size(1);posture.points[0].size(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.// 设定抓取对象，并且抓取尝试次数设置为1std::vector<moveit_msgs::Grasp> size(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_ientation = 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 ve_group.setSupportSurfaceName("table1");// Call pick to pick up the object using the grasps givenmove_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;size(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_ientation = 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 up.setSupportSurfaceName("table2");// Call place to place the object using the place up.place("object", place_location);// END_SUB_TUTORIAL
}void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface)
{// BEGIN_SUB_TUTORIAL table1//// Step1.1 创建一个Vector， 并且设置容量为3,用来保存三个objectsstd::vector<moveit_msgs::CollisionObject> collision_objects;size(3);//×××××××××××××××××××××××××××××××第一个table（box）××××××××××××××××××××××××××××××××××××××××// Step 1.2 创建第一个table，被抓取的物品放在其上// Add the first table where the cube will originally llision_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.4collision_objects[0].size(1);collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;collision_objects[0].primitives[0].size(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].size(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 llision_objects[1].id = "table2";collision_objects[1].header.frame_id = "panda_link0";/* Define the primitive and its dimensions. */collision_objects[1].size(1);collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;collision_objects[1].primitives[0].size(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].size(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_TUTORIALcollision_objects[1].operation = collision_objects[1].ADD;//×××××××××××××××××××××××××××××××第三个table（box）--被抓取的那个××××××××××××××××××××××××××××××××××××××××//具体过程与box1上边类似// BEGIN_SUB_TUTORIAL object// Define the object that we will be manipulatingcollision_objects[2].header.frame_id = "panda_link0";collision_objects[2].id = "object";/* Define the primitive and its dimensions. */collision_objects[2].size(1);collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;collision_objects[2].primitives[0].size(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].size(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_TUTORIALcollision_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，若休眠被打断，在休眠恢复后，将重新休眠1smoveit::planning_interface::PlanningSceneInterface planning_scene_interface; //创建planning_scenemoveit::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 initializeros::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