pick_place源码涉及到moveit的机械臂路径规划以及手指的抓取，其中涉及了moveit的move_group,Planning Scene, Planning Scene ROS API,Motion Planners,Motion Planning Pipeline的相关接口，根据源码顺序记录相关解释。
1、robot_model初始化
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model_ = robot_model_loader.getModel();Motion Planner及 Motion Planning Pipeline以及planning Scene创建之前创建的对象,对应的头文件是
#include <moveit/robot_model_loader/robot_model_loader.h>在载入规划器之前，需要创建两个对象，一个RobotModel和一个PlanningScene对象。首先，我们创建一个RobotModelLoader对象，这个对象可以从ROS的参数服务器中寻找robot_description（通过URDF和SRDF创建），并且能够创建一个RobotModel供我们使用。
2、planning_scene以及planning_scene_monitor初始化
planning_scene_.reset(new planning_scene::PlanningScene(robot_model_));
planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description"));对于Planning Scene以及Planning Scene ROS API的接口函数，对应的头文件是
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/planning_pipeline/planning_pipeline.h>对于我们使用的PlanningScene，仅仅只是MoveGroup所持有的一个动态场景（scene）在某个时间的截图，因此我们可以创建多个场景，并且互相不受影响。
对于场景的接口PlanningSceneInterface，包含在moveit::planning_interface::PlanningSceneInterface中。
如果我们想要向着场景中添加一个物体，应该调用该类的addCollisionObject()函数来实现，对于函数本质就是向/planning_scene主题发布了一PlanningScene类型的消息，我们的MoveGroup订阅了这个消息，所以MoveGroup就知道了这个物体的存在。
利用机器人在参数服务器上的robot_description来定义的planningScene本身并不知道碰撞物体的存在，因为其没有与MoveGroup进行通信，因此此时的碰撞检测是错误的，要想知道碰撞物体的存在，就需要使用planningSceneMonitor对象。
这里在pick_plack.h文件中，定义了planning_scene_与planning_scene_monitor_对象，分别是
planning_scene::PlanningScenePtr planning_scene_;
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;对于planning_scene_monitor_对象，因为是用robot_description来进行初始化，所以它知道机器人的存在。它的一个函数getPlanningScene()接口基于可以得到当前的场景，可以等同于在动态场景中的一次拍照。
而因为是Ptr，所以对应的是智能指针，该指针可以用reset指向新的对象。对于智能指针最大的方便就是不需要释放，而本身就是一个类，当类对象消失时，会自动执行析构函数实现指针的释放，这样会避免普通指针的内存泄漏。
3、robot_state初始化
// robot_state::RobotState& robot_state = planning_scene_->getCurrentStateNonConst();
// const robot_state::JointModelGroup *joint_model_group = robot_state.getJointModelGroup("arm");当你想要实时索取当前机器人的状态信息以及关节信息，可以用上述接口函数调用。对应的头文件为
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>4、actionlib客户端的订阅
finger_client_ = new actionlib::SimpleActionClient<kinova_msgs::SetFingersPositionAction>
("/" + robot_type_ + "_driver/fingers_action/finger_positions", false);定义kinova_msgs::SetFingersPositionAction类型的action客户端。.action文件放在了kinova_msg文件夹里。action服务器的名字为/j2n6s300_driver/fingers_action/finger_position，默认false即可以用默认线程。
5、发布主题的类型
pub_co_ = nh_.advertise<moveit_msgs::CollisionObject>("/collision_object", 10);
pub_aco_ = nh_.advertise<moveit_msgs::AttachedCollisionObject>("/attached_collision_object", 10);
pub_planning_scene_diff_ = nh_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);分别发布名为/collision_object与/attatched_collision_object名字的主题，用于添加碰撞对象以及附着在机器人上的对象。另外，利用了Planning Scene ROS API。Planning scene 发布者的ROS API是通过用"diffs"的主题接口。一个planning scene diff是当前的planning scene（由move_group节点来得到）与用户所想要的新的planning scene的区分。这里是发布了名为planning_scene的主题
6、定义关节名字
for (uint i = 0; i<joint_names_.size(); i++)
{
joint_names_[i] = robot_type_ + "_joint_" + boost::lexical_cast<std::string>(i+1);
}定义各个关节的名字，其中boost::lexical_cast是类型转换,将Int类型的数转化为std::string类型。
7、之后定义了一个define_cartesian_pose()函数，来定义抓手的笛卡尔坐标，其中定义了抓手抓取前的初始位姿以及抓取位置的位姿。除了直接输入位置和姿态，也构建了一个generate_gripper_align_pose()函数来设置抓取位姿。
8、generate_gripper_align_pose()函数解析
geometry_msgs::PoseStamped PickPlace::generate_gripper_align_pose(geometry_msgs::PoseStamped targetpose_msg, double dist, double azimuth, double polar, double rot_gripper_z)
{
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = "root";
// computer pregrasp position w.r.t. location of grasp_pose in spherical coordinate. Orientation is w.r.t. fixed world (root) reference frame.
double delta_x = -dist * cos(azimuth) * sin(polar);
double delta_y = -dist * sin(azimuth) * sin(polar);
double delta_z = -dist * cos(polar);
// computer the orientation of gripper w.r.t. fixed world (root) reference frame. The gripper (z axis) should point(open) to the grasp_pose.
tf::Quaternion q = EulerZYZ_to_Quaternion(azimuth, polar, rot_gripper_z);
pose_msg.pose.position.x = targetpose_msg.pose.position.x + delta_x;
pose_msg.pose.position.y = targetpose_msg.pose.position.y + delta_y;
pose_msg.pose.position.z = targetpose_msg.pose.position.z + delta_z;
pose_msg.pose.orientation.x = q.x();
pose_msg.pose.orientation.y = q.y();
pose_msg.pose.orientation.z = q.z();
pose_msg.pose.orientation.w = q.w();
ROS_DEBUG_STREAM(__PRETTY_FUNCTION__ << ": LINE: " << __LINE__ << ": " << "pose_msg: x " << pose_msg.pose.position.x << ", y " << pose_msg.pose.position.y << ", z " << pose_msg.pose.position.z << ", qx " << pose_msg.pose.orientation.x << ", qy " << pose_msg.pose.orientation.y << ", qz " << pose_msg.pose.orientation.z << ", qw " << pose_msg.pose.orientation.w );
return pose_msg;
}对于形参，
geometry_msgs::PoseStamped类型的targetpose_msg：定义了抓取/放置的位姿，相当于抓取物体的位姿，在这个位姿在抓手抓取或者释放实现抓取和放置功能。
double类型的参数dist：返回位姿与抓取位姿的距离
double类型的参数azimuth：在球面坐标系下，该位置向量向着x-y平面投影后与x轴的夹角，用theta表示，范围0-2pi
double类型的参数polar：polar也可以名为zenith和colatitude，用phi表示，范围0-pi。它是从z正半轴到该向量的夹角。phi= pi/2 - delta，delta是纬度
double类型的参数rot_gripper_z：沿着抓手参考坐标系z轴的旋转
最终返回一个定义在球面坐标系下的位姿，原点定义在目标位姿，一般来说这是一个预抓点或者是释放后回归点的位姿。这里抓手的坐标系（最后关节的坐标轴）指向了目标。即和最后抓取位置的旋转矩阵相同（都是想对于root坐标系），位置有delta的差距。
相当于在抓取点设置了一个球面坐标，通过设置角度来找到一个位置，这个位置就是预抓取或者抓取后回归位置，这个位姿的方向与目标的位姿的姿态（旋转矩阵）相同，有个delta的位置的差距。
9、定义抓取，预抓取，抓取后的位姿。
grasp_pose_= generate_gripper_align_pose(grasp_pose_, 0.03999, M_PI/4, M_PI/2, M_PI);
pregrasp_pose_ = generate_gripper_align_pose(grasp_pose_, 0.1, M_PI/4, M_PI/2, M_PI);
postgrasp_pose_ = grasp_pose_;oid PickPlace::define_cartesian_pose()
{
tf::Quaternion q;
// define start pose before grasp
start_pose_.header.frame_id = "root";
start_pose_.header.stamp = ros::Time::now();
start_pose_.pose.position.x = 0.5;
start_pose_.pose.position.y = -0.5;
start_pose_.pose.position.z = 0.5;
q = EulerZYZ_to_Quaternion(-M_PI/4, M_PI/2, M_PI);
start_pose_.pose.orientation.x = q.x();
start_pose_.pose.orientation.y = q.y();
start_pose_.pose.orientation.z = q.z();
start_pose_.pose.orientation.w = q.w();
// define grasp pose
grasp_pose_.header.frame_id = "root";
grasp_pose_.header.stamp = ros::Time::now();
// Euler_ZYZ (-M_PI/4, M_PI/2, M_PI/2)
grasp_pose_.pose.position.x = 0.0;
grasp_pose_.pose.position.y = 0.6;
grasp_pose_.pose.position.z = 0.3;
q = EulerZYZ_to_Quaternion(M_PI/4, M_PI/2, M_PI);
grasp_pose_.pose.orientation.x = q.x();
grasp_pose_.pose.orientation.y = q.y();
grasp_pose_.pose.orientation.z = q.z();
grasp_pose_.pose.orientation.w = q.w();
// generate_pregrasp_pose(double dist, double azimuth, double polar, double rot_gripper_z)
grasp_pose_= generate_gripper_align_pose(grasp_pose_, 0.03999, M_PI/4, M_PI/2, M_PI);
pregrasp_pose_ = generate_gripper_align_pose(grasp_pose_, 0.1, M_PI/4, M_PI/2, M_PI);
postgrasp_pose_ = grasp_pose_;
postgrasp_pose_.pose.position.z = grasp_pose_.pose.position.z + 0.05;
}
postgrasp_pose_.pose.position.z = grasp_pose_.pose.position.z + 0.05;在设置好generate_gripper_align_pose()函数之后，分别通过不同的参数来定义抓取的点，预抓取，抓取后的位姿。（不将抓取位姿和目标位姿设为相同值的原因应该是根据不同的物体形状会产生一个误差来保证抓取的稳定性）。
10、定义笛卡尔空间位姿
oid PickPlace::define_cartesian_pose()
{
tf::Quaternion q;
// define start pose before grasp
start_pose_.header.frame_id = "root";
start_pose_.header.stamp = ros::Time::now();
start_pose_.pose.position.x = 0.5;
start_pose_.pose.position.y = -0.5;
start_pose_.pose.position.z = 0.5;
q = EulerZYZ_to_Quaternion(-M_PI/4, M_PI/2, M_PI);
start_pose_.pose.orientation.x = q.x();
start_pose_.pose.orientation.y = q.y();
start_pose_.pose.orientation.z = q.z();
start_pose_.pose.orientation.w = q.w();
// define grasp pose
grasp_pose_.header.frame_id = "root";
grasp_pose_.header.stamp = ros::Time::now();
// Euler_ZYZ (-M_PI/4, M_PI/2, M_PI/2)
grasp_pose_.pose.position.x = 0.0;
grasp_pose_.pose.position.y = 0.6;
grasp_pose_.pose.position.z = 0.3;
q = EulerZYZ_to_Quaternion(M_PI/4, M_PI/2, M_PI);
grasp_pose_.pose.orientation.x = q.x();
grasp_pose_.pose.orientation.y = q.y();
grasp_pose_.pose.orientation.z = q.z();
grasp_pose_.pose.orientation.w = q.w();
// generate_pregrasp_pose(double dist, double azimuth, double polar, double rot_gripper_z)
grasp_pose_= generate_gripper_align_pose(grasp_pose_, 0.03999, M_PI/4, M_PI/2, M_PI);
pregrasp_pose_ = generate_gripper_align_pose(grasp_pose_, 0.1, M_PI/4, M_PI/2, M_PI);
postgrasp_pose_ = grasp_pose_;
postgrasp_pose_.pose.position.z = grasp_pose_.pose.position.z + 0.05;
}分别定义了不同目标点（抓取，目标等）的笛卡尔坐标空间的位姿，设置参考坐标系，时间戳，并且通过之前定义的
generate_pregrasp_pose来定义在抓取位置的基础上的预抓取和抓取后得位姿。
11、得到当前信息
void PickPlace::get_current_state(const sensor_msgs::JointStateConstPtr &msg)
{
boost::mutex::scoped_lock lock(mutex_state_);
current_state_ = *msg;
}
void PickPlace::get_current_pose(const geometry_msgs::PoseStampedConstPtr &msg)
{
boost::mutex::scoped_lock lock(mutex_pose_);
current_pose_ = *msg;
}使用boost进行线程管理简单使用boost::mutex.mutex对象本身并不知道它代表什么，它仅仅是被多个消费者线程使用的资源访问的锁定解锁标志。在某个时刻，只有一个线程可以锁定这个mutex对象，这就阻止了同一时刻有多个线程并发访问共享资源。一个mutex就是一个简单的信号机制。
给mutex加解锁有多种策略，最简单的是使用scoped_lock类，它使用一个mutex参数来构造，并一直锁定这个mutex直到对象被销毁。如果这个正在被构造的mutex已经被别的线程锁定的话，当前线程就会进入wait状态，直到这个锁被解开。
12、定义抓手行为
bool PickPlace::gripper_action(double finger_turn)
{
if(robot_connected_ == false)
{
if (finger_turn>0.5*FINGER_MAX)
{
gripper_group_->setNamedTarget("Close");
}
else
{
gripper_group_->setNamedTarget("Open");
}
gripper_group_->move();
return true;
}
if (finger_turn < 0)
{bool PickPlace::gripper_action(double finger_turn)
{ std::min(finger_turn, FINGER_MAX);
}
kinova_msgs::SetFingersPositionGoal goal;
goal.fingers.finger1 = finger_turn;
goal.fingers.finger2 = goal.fingers.finger1;
goal.fingers.finger3 = goal.fingers.finger1;
finger_client_->sendGoal(goal);
if (finger_client_->waitForResult(ros::Duration(5.0)))
{
finger_client_->getResult();
return true;
}
else
{
finger_client_->cancelAllGoals();
ROS_WARN_STREAM("The gripper action timed-out");
return false;
}
}
参数gripper_rad是表示状态信息，当为6400时，抓手为关，当为0时，抓手为开。
当机器人没有连接时，判断finger_turn是否大于0.5*FINGER_MAX,如果大于，则设置到名为close得位置，否则设置到open位置，这些位置是设置机械臂得SRDF模型时设置的。当finger_turn<0时，设置为0，否则取finger_turn与FINGER_MAX得最小值
接着定义kinova_msgs::SetFingersPositionGoal类型变量goal.这是通过action文件编译后生成得消息类型，分别设置goal得不同手指得状态，最后通过actionlib得客户端得sendGoal将目标传进actionlib服务器中进行处理。
当服务器处理后输出结果，通过客户端得getResult()函数得到结果，否则则通过cancelALLGoals()函数来取消
13、处理工作空间及添加物体
void PickPlace::clear_workscene()
{
// remove table
co_.id = "table";
co_.operation = moveit_msgs::CollisionObject::REMOVE;
pub_co_.publish(co_);
// remove target
co_.id = "target_cylinder";
co_.operation = moveit_msgs::CollisionObject::REMOVE;
pub_co_.publish(co_);
//remove attached target
aco_.object.operation = moveit_msgs::CollisionObject::REMOVE;
pub_aco_.publish(aco_);
planning_scene_msg_.world.collision_objects.clear();
planning_scene_msg_.is_diff = true;
pub_planning_scene_diff_.publish(planning_scene_msg_);
clear_obstacle();
}
void PickPlace::build_workscene()
{
co_.header.frame_id = "root";
co_.header.stamp = ros::Time::now();
// remove table
co_.id = "table";
co_.operation = moveit_msgs::CollisionObject::REMOVE;
pub_co_.publish(co_);
// add table
co_.primitives.resize(1);
co_.primitive_poses.resize(1);
co_.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
co_.primitives[0].dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>::value);
co_.operation = moveit_msgs::CollisionObject::ADD;
co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 2.4;
co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 2.4;
co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.03;
co_.primitive_poses[0].position.x = 0;
co_.primitive_poses[0].position.y = 0.0;
co_.primitive_poses[0].position.z = -0.03/2.0;
pub_co_.publish(co_);
planning_scene_msg_.world.collision_objects.push_back(co_);
planning_scene_msg_.is_diff = true;
pub_planning_scene_diff_.publish(planning_scene_msg_);
ros::WallDuration(0.1).sleep();}
void PickPlace::clear_obstacle()
{
co_.id = "pole";
co_.operation = moveit_msgs::CollisionObject::REMOVE;
pub_co_.publish(co_);
planning_scene_msg_.world.collision_objects.push_back(co_);
co_.id = "bot_obstacle";
co_.operation = moveit_msgs::CollisionObject::REMOVE;
pub_co_.publish(co_);
planning_scene_msg_.world.collision_objects.push_back(co_);
co_.id = "top_obstacle";
pub_co_.publish(co_);
planning_scene_msg_.world.collision_objects.push_back(co_);
planning_scene_msg_.is_diff = true;
pub_planning_scene_diff_.publish(planning_scene_msg_);
ros::WallDuration(0.1).sleep();
// ROS_WARN_STREAM(__PRETTY_FUNCTION__ << ": LINE " << __LINE__ << ": remove pole ");
// std::cin >> pause_;
}
void PickPlace::add_obstacle()
{
clear_obstacle();
co_.id = "pole";
co_.primitives.resize(1);
co_.primitive_poses.resize(1);
co_.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
co_.primitives[0].dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>::value);
co_.operation = moveit_msgs::CollisionObject::ADD;
co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.3;
co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.1;
co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.4;
co_.primitive_poses[0].position.x = 0.5;
co_.primitive_poses[0].position.y = -0.1;
co_.primitive_poses[0].position.z = 0.4/2.0;
pub_co_.publish(co_);
planning_scene_msg_.world.collision_objects.push_back(co_);
planning_scene_msg_.is_diff = true;
pub_planning_scene_diff_.publish(planning_scene_msg_);
ros::WallDuration(0.1).sleep();
}
void PickPlace::add_complex_obstacle()
{
clear_obstacle();
// add obstacle between robot and object
co_.id = "bot_obstacle";
co_.primitives.resize(1);
co_.primitive_poses.resize(1);
co_.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
co_.primitives[0].dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>::value);
co_.operation = moveit_msgs::CollisionObject::ADD;
double box_h = 0.2;
co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 2.4;
co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.1;
co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = box_h;
co_.primitive_poses[0].position.x = 0;
co_.primitive_poses[0].position.y = 0.3;
co_.primitive_poses[0].position.z = box_h/2.0;
co_.primitive_poses[0].orientation.w = 1.0;
pub_co_.publish(co_);
planning_scene_msg_.world.collision_objects.push_back(co_);
co_.id = "top_obstacle";
co_.primitive_poses[0].position.z = box_h/2.0 + box_h + 0.4;
pub_co_.publish(co_);
planning_scene_msg_.world.collision_objects.push_back(co_);
planning_scene_msg_.is_diff = true;
pub_planning_scene_diff_.publish(planning_scene_msg_);
ros::WallDuration(0.1).sleep();
// ROS_WARN_STREAM(__PRETTY_FUNCTION__ << ": LINE " << __LINE__ << ": ADD pole ");
// std::cin >> pause_;
}
void PickPlace::add_attached_obstacle()
{
//once the object is know to be grasped
//we remove obstacle from work scene
co_.id = "target_cylinder";
co_.operation = moveit_msgs::CollisionObject::REMOVE;
pub_co_.publish(co_);
//and then we declare it as an attached obstacle
aco_.object.operation = moveit_msgs::CollisionObject::ADD;
aco_.link_name = robot_type_ + "_end_effector";
aco_.touch_links.push_back(robot_type_ + "_end_effector");
aco_.touch_links.push_back(robot_type_ + "_link_finger_1");
aco_.touch_links.push_back(robot_type_ + "_link_finger_2");
aco_.touch_links.push_back(robot_type_ + "_link_finger_3");
aco_.touch_links.push_back(robot_type_ + "_link_finger_tip_1");
aco_.touch_links.push_back(robot_type_ + "_link_finger_tip_2");
aco_.touch_links.push_back(robot_type_ + "_link_finger_tip_3");
pub_aco_.publish(aco_);
}
void PickPlace::add_target()
{
//remove target_cylinder
co_.id = "target_cylinder";
co_.operation = moveit_msgs::CollisionObject::REMOVE;
pub_co_.publish(co_);
//add target_cylinder
co_.primitives.resize(1);
co_.primitive_poses.resize(1);
co_.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER;
co_.primitives[0].dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::CYLINDER>::value);
co_.operation = moveit_msgs::CollisionObject::ADD;
co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = 0.5;
co_.primitives[0].dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = 0.04;
co_.primitive_poses[0].position.x = 0.0;
co_.primitive_poses[0].position.y = 0.6;
co_.primitive_poses[0].position.z = 0.3;
can_pose_.pose.position.x = co_.primitive_poses[0].position.x;
can_pose_.pose.position.y = co_.primitive_poses[0].position.y;
can_pose_.pose.position.z = co_.primitive_poses[0].position.z;
pub_co_.publish(co_);
planning_scene_msg_.world.collision_objects.push_back(co_);
planning_scene_msg_.is_diff = true;
pub_planning_scene_diff_.publish(planning_scene_msg_);
aco_.object = co_;
ros::WallDuration(0.1).sleep();
}分别定义了clear_workscene(),build_workscene(),clear_obstacle(),add_obstacle(),add_complex_obstacle(),add_attached_obstacle(),add_target()
通过moveit_msgs::CollisionObject对象来设置物体的id,操作行为，并且通过发布者进行发布。
定义好后，通过moveit_msgs::PlanningScene类型的对象.world.collision_objects进行工作空间清理，进而发布。
在加入物体时，要设置物体得参考坐标系和时间戳，并且要设置物体得尺寸，位姿，类型，操作方式。其中geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>::value通过static_cast<int>将shape_msgs::SolidPrimitive::BOX_X，Y，Z类型强制转换为int类型，对其赋值。
另外，我们想要抓取物体时，当到达了物体所在位姿后，需要先将物体移除环境场景并将其附着到机器人上。
14、碰撞检测
void PickPlace::check_collision()
{
collision_detection::CollisionRequest collision_request;
collision_detection::CollisionResult collision_result;
collision_request.contacts = true;
collision_request.max_contacts = 1000;
collision_result.clear();
planning_scene_->checkCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 1: Current state is "
<< (collision_result.collision ? "in" : "not in")
<< " collision");
collision_request.group_name = "arm";
collision_result.clear();
planning_scene_->checkCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 3: Current state is "
<< (collision_result.collision ? "in" : "not in")
<< " collision");
// check contact
planning_scene_->checkCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 4: Current state is "
<< (collision_result.collision ? "in" : "not in")
<< " collision");
collision_detection::CollisionResult::ContactMap::const_iterator it;
for(it = collision_result.contacts.begin();
it != collision_result.contacts.end();
++it)
{
ROS_INFO("Contact between: %s and %s",
it->first.first.c_str(),
it->first.second.c_str());
}
// allowed collision matrix
collision_detection::AllowedCollisionMatrix acm = planning_scene_->getAllowedCollisionMatrix();
robot_state::RobotState copied_state = planning_scene_->getCurrentState();
collision_detection::CollisionResult::ContactMap::const_iterator it2;
for(it2 = collision_result.contacts.begin();
it2 != collision_result.contacts.end();
++it2)
{
acm.setEntry(it2->first.first, it2->first.second, true);
}
collision_result.clear();
planning_scene_->checkCollision(collision_request, collision_result, copied_state, acm);
ROS_INFO_STREAM("Test 5: Current state is "
<< (collision_result.collision ? "in" : "not in")
<< " collision");
}首先初始化collision_detection::CollisionRequest与collision_detection::CollisionResult变量，并且能够通过request来得到接触信息并且设置收到的信息的最大值。
最终通过planning_scene::PlanningScenePtr对象的checkCollision函数进行检测，同样可以设置允许碰撞矩阵。通过collision_detection::AllowedCollisionMatrix创建对象，通过setEntry函数进行检测，形参中为true则忽略碰撞。注意的是，每次碰转检测后，需要对collision_result进行clear操作。
15、设置限制
void PickPlace::setup_constrain(geometry_msgs::Pose target_pose, bool orientation, bool position)
{
if ( (!orientation) && (!position) )
{
ROS_WARN("Neither orientation nor position constrain applied.");
return;
}
moveit_msgs::Constraints grasp_constrains;
// setup constrains
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = robot_type_ + "_end_effector";
ocm.header.frame_id = "root";
ocm.orientation = target_pose.orientation;
ocm.absolute_x_axis_tolerance = 2*M_PI;
ocm.absolute_y_axis_tolerance = 2*M_PI;
ocm.absolute_z_axis_tolerance = M_PI/10;
ocm.weight = 0.5;
if (orientation)
{
grasp_constrains.orientation_constraints.push_back(ocm);
}
/* Define position constrain box based on current pose and target pose. */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
// group_->getCurrentPose() does not work.
// current_pose_ = group_->getCurrentPose();
geometry_msgs::Pose current_pose;
{ // scope for mutex update
boost::mutex::scoped_lock lock_pose(mutex_pose_);
current_pose = current_pose_.pose;
// ROS_DEBUG_STREAM(__PRETTY_FUNCTION__ << ": LINE: " << __LINE__ << ": " << "current_pose_: x " << current_pose_.pose.position.x << ", y " << current_pose_.pose.position.y << ", z " << current_pose_.pose.position.z << ", qx " << current_pose_.pose.orientation.x << ", qy " << current_pose_.pose.orientation.y << ", qz " << current_pose_.pose.orientation.z << ", qw " << current_pose_.pose.orientation.w );
}
double constrain_box_scale = 2.0;
primitive.dimensions[0] = constrain_box_scale * std::abs(target_pose.position.x - current_pose.position.x);
primitive.dimensions[1] = constrain_box_scale * std::abs(target_pose.position.y - current_pose.position.y);
primitive.dimensions[2] = constrain_box_scale * std::abs(target_pose.position.z - current_pose.position.z);
/* A pose for the box (specified relative to frame_id) */
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
// place between start point and goal point.
box_pose.position.x = (target_pose.position.x + current_pose.position.x)/2.0;
box_pose.position.y = (target_pose.position.y + current_pose.position.y)/2.0;
box_pose.position.z = (target_pose.position.z + current_pose.position.z)/2.0;
moveit_msgs::PositionConstraint pcm;
pcm.link_name = robot_type_+"_end_effector";
pcm.header.frame_id = "root";
pcm.constraint_region.primitives.push_back(primitive);
pcm.constraint_region.primitive_poses.push_back(box_pose);
pcm.weight = 0.5;
if(position)
{
grasp_constrains.position_constraints.push_back(pcm);
}
group_->setPathConstraints(grasp_constrains);传入geometry_msgs::Pose target_pose, bool orientation, bool position，分别是目标的位姿，以及判断是否需要位置和姿态的限制
如果没有位置和姿态的限制，则打印消息。
之后初始化一个moveit_msgs::Constraints类型的对象，并且设置姿态限制，建立moveit_msgs::OrientationConstraint对象，设置参考坐标系和需要限制的坐标系，姿态初始化为目标的姿态，让抓取时末端执行器的姿态和目标物体相同，然后设置x,y,z方向的允许误差和权值，通过grasp_constrains.orientation_constraints传入姿态限制
定义位置限制是通过在世界设置一个物体，位置限制是基于当前位姿和目标位姿。通过geometry_msgs::PoseStamped对象的pose函数来初始化geometry_msgs::Pose对象current_pose.对于geometry_msgs::PoseStamped类型，除了geometry_msgs::Pose之外，还有一个std_msgs::Header类型，它是用于高等级的戳数据类型的标准元数据。用于在一个特定坐标系与时间戳数据的通信。
在源码中，这个障碍物体尺寸设置为目标位置与当前位置的位置在各个方向的差的绝对值的两倍。位姿设置为起始点和目标点的中间，并且通过moveit_msgs::PositionConstraint对象传入grasp_constrains中，方法与姿态限制方法类似，只不过函数有不同。
最后 group_->setPathConstraints(grasp_constrains)传入最终结果。
16、限制检测
void PickPlace::check_constrain()
{
moveit_msgs::Constraints grasp_constrains = group_->getPathConstraints();
bool has_constrain = false;
ROS_INFO("check constrain result: ");
if (!grasp_constrains.orientation_constraints.empty())
{
has_constrain = true;
ROS_INFO("Has orientation constrain. ");
}
if(!grasp_constrains.position_constraints.empty())
{
has_constrain = true;
ROS_INFO("Has position constrain. ");
}
if(has_constrain == false)
{
ROS_INFO("No constrain. ");
}
}通过 group_->getPathConstraints()接口初始化moveit_msgs::Constraints对象，并进行检测。
17、估计计划
void PickPlace::evaluate_plan(moveit::planning_interface::MoveGroup &group)
{
bool replan = true;
int count = 0;
moveit::planning_interface::MoveGroup::Plan my_plan;
while (replan == true && ros::ok())
{
// reset flag for replan
count = 0;
result_ = false;
// try to find a success plan.
double plan_time;
while (result_ == false && count < 5)
{
count++;
plan_time = 20+count*10;
ROS_INFO("Setting plan time to %f sec", plan_time);
group.setPlanningTime(plan_time);
result_ = group.plan(my_plan);
std::cout << "at attemp: " << count << std::endl;
ros::WallDuration(0.1).sleep();
}
// found a plan
if (result_ == true)
{
std::cout << "plan success at attemp: " << count << std::endl;
replan = false;
std::cout << "please input e to execute the plan, r to replan, others to skip: ";
std::cin >> pause_;
ros::WallDuration(0.5).sleep();
if (pause_ == "r" || pause_ == "R" )
{
replan = true;
}
else
{
replan = false;
}
}
else // not found
{
std::cout << "Exit since plan failed until reach maximum attemp: " << count << std::endl;
replan = false;
break;
}
}
if(result_ == true)
{
if (pause_ == "e" || pause_ == "E")
{
group.execute(my_plan);
}
}
ros::WallDuration(1.0).sleep();
}形参为moveit::planning_interface::MoveGroup规划组对象
先初始化moveit::planning_interface::MoveGroup::Plan对象。并且建立计数变量。当结果为false且计数小于5时，计数+1，规划时间为20+10倍计数，并通过group.setPlanningTime(plan_time)传入规划时间，通过group.plan(my_plan)来更新result，等待0.1秒进入下次循环。
当找到规划后，进行一系列的输出，保留重新规划选项，并且执行计划。
18、我的选取任务
bool PickPlace::my_pick()
{
clear_workscene();
ros::WallDuration(1.0).sleep();
build_workscene();
ros::WallDuration(1.0).sleep();
ROS_INFO_STREAM("Press any key to send robot to home position ...");
std::cin >> pause_;
group_->clearPathConstraints();
group_->setNamedTarget("Home");
evaluate_plan(*group_);
ros::WallDuration(1.0).sleep();
gripper_group_->setNamedTarget("Open");
gripper_group_->move();
///////////////////////////////////////////////////////////
//// joint space without obstacle
///////////////////////////////////////////////////////////
ROS_INFO_STREAM("Joint space motion planing without obstacle");
ROS_INFO_STREAM("Demonstrates moving robot from one joint position to another");
ROS_INFO_STREAM("Planning to go to start pose ...");
group_->setJointValueTarget(start_joint_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Planning to go to pre-grasp joint pose ...");
group_->setJointValueTarget(pregrasp_joint_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Approaching grasp position ...");
group_->setJointValueTarget(grasp_joint_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Grasping ...");
gripper_action(0.75*FINGER_MAX); // partially close
ROS_INFO_STREAM("Planning to go to retract position ...");
group_->setJointValueTarget(postgrasp_joint_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Planning to go to start position ...");
group_->setJointValueTarget(start_joint_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Releasing ...");
gripper_action(0.0); // full open
///////////////////////////////////////////////////////////
//// joint space with obstacle
///////////////////////////////////////////////////////////
ROS_INFO_STREAM("*************************");
ROS_INFO_STREAM("*************************");
ROS_INFO_STREAM("*************************");
ROS_INFO_STREAM("Press any key to start motion plan in joint space with obstacle ...");
std::cin >> pause_;
add_obstacle();
group_->setJointValueTarget(pregrasp_joint_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Planning to come back to start position ...");
group_->setJointValueTarget(start_joint_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Releasing gripper ...");
gripper_action(0.0); // full open
///////////////////////////////////////////////////////////
//// Cartesian space without obstacle
///////////////////////////////////////////////////////////
ROS_INFO_STREAM("*************************");
ROS_INFO_STREAM("*************************");
ROS_INFO_STREAM("*************************");
ROS_INFO_STREAM("Motion planning in cartesian space without obstacles ...");
clear_workscene();
build_workscene();
add_target();
ros::WallDuration(0.1).sleep();
ROS_INFO_STREAM("Press any key to move to start pose ...");
std::cin >> pause_;
group_->setPoseTarget(start_pose_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Planning to go to pre-grasp position ...");
group_->setPoseTarget(pregrasp_pose_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Approaching grasp position ...");
group_->setPoseTarget(grasp_pose_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Grasping ...");
add_attached_obstacle();
gripper_action(0.75*FINGER_MAX); // partially close
ROS_INFO_STREAM("Planning to return to start position ...");
group_->setPoseTarget(start_pose_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Releasing gripper ...");
gripper_action(0.0); // full open
///////////////////////////////////////////////////////////
//// Cartesian space with obstacle
///////////////////////////////////////////////////////////
ROS_INFO_STREAM("*************************");
ROS_INFO_STREAM("*************************");
ROS_INFO_STREAM("*************************");
ROS_INFO_STREAM("Press any key to start motion plan in cartesian space with obstacle ...");
std::cin >> pause_;
clear_workscene();
build_workscene();
add_target();
add_complex_obstacle();
ros::WallDuration(1.0).sleep();
//ROS_INFO_STREAM("Planning to go to start position ...");
//group_->setPoseTarget(start_pose_);
//evaluate_plan(*group_);
ros::WallDuration(0.1).sleep();
ROS_INFO_STREAM("Planning to go to pre-grasp position ...");
group_->setPoseTarget(pregrasp_pose_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Planning to go to grasp position ...");
group_->setPoseTarget(grasp_pose_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Press any key to grasp ...");
std::cin >> pause_;
add_attached_obstacle();
gripper_action(0.75*FINGER_MAX); // partially close
ROS_INFO_STREAM("Planning to go to start position ...");
group_->setPoseTarget(start_pose_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Releasing gripper ...");
gripper_action(0.0); // full open
ROS_INFO_STREAM("Press any key to start motion planning with orientation constrains ...");
ROS_INFO_STREAM("Constraints are setup so that the robot keeps the target vertical");
std::cin >> pause_;
clear_workscene();
build_workscene();
add_target();
//ROS_INFO_STREAM("Planning to go to start position ...");
//group_->setPoseTarget(start_pose_);
//evaluate_plan(*group_);
ros::WallDuration(0.1).sleep();
ROS_INFO_STREAM("Planning to go to pre-grasp position ...");
group_->setPoseTarget(pregrasp_pose_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Planning to go to grasp position ...");
group_->setPoseTarget(grasp_pose_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Press any key to grasp ...");
std::cin >> pause_;
add_attached_obstacle();
gripper_action(0.75*FINGER_MAX); // partially close
setup_constrain(grasp_pose_.pose, true, false);
ROS_INFO_STREAM("Planning to go to start position ...");
group_->setPoseTarget(start_pose_);
evaluate_plan(*group_);
ROS_INFO_STREAM("Releasing gripper ...");
gripper_action(0.0); // full open
// If need to double check if reach target position.
// replacement of group_->getCurrentJointValues();
// { // scope for mutex update
// boost::mutex::scoped_lock lock_state(mutex_state_);
// sensor_msgs::JointState copy_state = current_state_;
// }
// replacement of group_->getCurrentPose();
// { // scope for mutex update
// boost::mutex::scoped_lock lock_state(mutex_state_);
// geometry_msgs::PoseStamped copy_pose = current_pose_;
// }
clear_workscene();
ROS_INFO_STREAM("Press any key to quit ...");
std::cin >> pause_;
return true;
}首先清除并建立工作空间，并且将手臂设置到home位置，抓手设置到open位置。
接着在关节空间中设置位置进行移动，分别到达开始位置，抓取位置和预抓取位置，通过gripper_action来进行抓取，参数为手具体位姿，接着移动到开始位置vu并且释放。同样可以通过在笛卡尔坐标空间进行上述操作。
之后是通过在空间中增加障碍物进行抓取测试，同样可以设置位姿限制进行抓取。如果需要到达目标位置后需要双重检测，进行相应的转换。