ros高效编程第七章

7.4.3 7.5 抓取和放置

场景规划

两种方式：三维感知，通过rgd
通过编程加入物体。
他这里其实就是在rviz中通过编程手动加入了几个物体，位置刚好与gazebo的对应上了，与点云信息也同时对应。

感知

这里就是上面提到的感知方式，他这里一笔带过了。

桌子作为点云检查到的水平面，一旦桌子被识别，就可以通过原始的点云获取目标对象。
将目标对象近似为圆柱体或者方盒。
使用相同的方法在moveit中添加对象，对象的位姿和尺寸将来自3d点云和分割算法的输出。

使用由rgbd传感器提供的点云感知和分割由相关概念轻松完成。
4. 可能出现精度不足，通过在对象上放基准标记。这里提到了aruco

Aruco

他有ros封装
http://wiki.ros.org/aruco_ros
http://wiki.ros.org/aruco
http://www.uco.es/investiga/grupos/ava/node/26
https://github.com/pal-robotics/aruco_ros

This is a wrapper around the marker detection library ArUco.
http://wiki.ros.org/tuw_aruco

https://gregorkovalcik.github.io/opencv_contrib/tutorial_aruco_detection.html
http://wiki.ros.org/aruco_mapping
http://wiki.ros.org/aruco_pose

用于机器人自身定位，基准点
http://wiki.ros.org/fiducials
http://wiki.ros.org/aruco_detect

抓取

这里用到了 moveit simple grasps
https://github.com/davetcoleman/moveit_simple_grasps/
kinetic需要修补分支

moveit_msgs::Grasp

http://docs.ros.org/en/kinetic/api/moveit_msgs/html/msg/Grasp.html
该信息用于表示一个特殊的末端执行器来抓取一个物体，包含如何接近以及抓取他。
这信息不包含 a grasp point（a position ON the object）
生成此消息的任何对象都应该已经将有关抓取点的信息与有关末端效应器几何体的信息结合起来，以计算此消息中的抓取姿势。

A name for this grasp

trajectory_msgs/JointTrajectory pre_grasp_posture
预握时手的内部姿势 only positions are used
This defines the trajectory position of the joints in the end effector group before we go in for the grasp.关节的轨迹位置

trajectory_msgs/JointTrajectory grasp_posture
The internal posture of the hand for the grasp / positions and efforts are used
This defines the trajectory position of the joints in the end effector group for grasping the object.关节的轨迹位置

geometry_msgs/PoseStamped grasp_pose
Typically this would be the pose of the most distal wrist link before the hand (end-effector) links began.

float64 grasp_quality
The estimated probability of success for this grasp,or some other
measure of how “good” it is.

GripperTranslation pre_grasp_approach
The approach direction to take before picking an object
This is used to define the direction from which to approach the object and the distance to travel.

GripperTranslation post_grasp_retreat
抓取完成后的后退方向（物体已附着）
This is used to define the direction in which to move once the object is grasped and the distance to travel.

GripperTranslation post_place_retreat
释放物体时要执行的后退动作；抓取本身并不需要这些信息，但当释放物体时，这些信息是必要的。用于执行拾取的抓取作为结果的一部分返回，因此此信息可供以后使用。
This is used to define the direction in which to move once the object is placed at some location and the distance to travel.

float32 max_contact_force
the maximum contact force to use while grasping (<=0 to disable)

string[] allowed_touch_objects
一个可选的障碍列表，我们有语义信息，在抓取过程中可以触摸/推/移动

moveit_msgs::Grasp

string id
trajectory_msgs/JointTrajectory pre_grasp_posture
trajectory_msgs/JointTrajectory grasp_posture
geometry_msgs/PoseStamped grasp_pose
float64 grasp_quality
moveit_msgs/GripperTranslation pre_grasp_approach
moveit_msgs/GripperTranslation post_grasp_retreat
moveit_msgs/GripperTranslation post_place_retreat
float32 max_contact_force
string[] allowed_touch_objects

moveit_msgs/GripperTranslation

geometry_msgs/Vector3Stamped direction定义夹持器的平移，用于拾取或放置任务，例如将物体从桌子上提起或靠近桌子放置std_msgs/Header headergeometry_msgs/Vector3 vector
float32 desired_distance 所需的平移距离
float32 min_distance在尝试抓取之前必须考虑的最小距离

trajectory_msgs/JointTrajectory pre_grasp_posture

std_msgs/Header header
string[] joint_names
trajectory_msgs/JointTrajectoryPoint[] points

trajectory_msgs/JointTrajectoryPoint[] points

float64[] positions
float64[] velocities
float64[] accelerations
float64[] effort
duration time_from_start

当然也可以对positon位置修改或者删掉点，注意首尾两个点不能删，该点速度为0删去可能会让机械臂发生未知问题。
用for循环修改每一个关节，当然也可以不用他的plan来封装，可以用自己写的代码，只要符合封装相应的数据结构。用自己的规划算法，也可以用插件的形式，插件的形式比较复杂。
positions和joint names顺序是对应的
他这里joint name 和轨迹点的对应关系：
points size为n i从0到n
points【i】.time from start直接改
n个joint，设为j 从0到n
points【i】.velocities【j】
也就是说，轨迹规划了n个点。n个点里面，每个点拆分，下级为各个关节这个时刻的速度，position以及别的参数。至于这里为什么是float 我理解为关节空间中的定义。

Grasp实践

古月居里面的是定义 a=Grasp（），然后针对里面的不断定义去尝试
moveit里面只用了一次抓取

std::vector<moveit_msgs::Grasp> grasps;
grasps.resize(1);

他这里grasp pose给到了link8，和上面的也正好一致。
他这里位置计算为立方体的中心在位置5
5-（立方体长度的一半 +link8与eef手掌的距离+一些间隙）

他moveit里面官方给到的例子只是从x方向上接近，然后pre-grasp里面给到了

/*********************************************************************
* 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
}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.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)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 */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 */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// +++++++++++++++++++++++++++++++++++openGripper(grasps[0].pre_grasp_posture);// END_SUB_TUTORIAL// BEGIN_SUB_TUTORIAL pick2// Setting posture of eef during grasp// +++++++++++++++++++++++++++++++++++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 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.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//// Creating Environment// ^^^^^^^^^^^^^^^^^^^^// Create vector to hold 3 collision objects.std::vector<moveit_msgs::CollisionObject> collision_objects;collision_objects.resize(3);// 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. */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. */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_TUTORIALcollision_objects[0].operation = collision_objects[0].ADD;// 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_TUTORIALcollision_objects[1].operation = collision_objects[1].ADD;// 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].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_TUTORIALcollision_objects[2].operation = collision_objects[2].ADD;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();moveit::planning_interface::PlanningSceneInterface planning_scene_interface;moveit::planning_interface::MoveGroupInterface group("panda_arm");group.setPlanningTime(45.0);addCollisionObjects(planning_scene_interface);// Wait a bit for ROS things to initializeros::WallDuration(1.0).sleep();pick(group);ros::WallDuration(1.0).sleep();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

一些代码

#!/usr/bin/env python
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface
from moveit_commander import roscpp_initialize, roscpp_shutdown
from actionlib import SimpleActionClient, GoalStatus
from geometry_msgs.msg import Pose, PoseStamped, PoseArray, Quaternion
from moveit_msgs.msg import PickupAction, PickupGoal
from moveit_msgs.msg import PlaceAction, PlaceGoal
from moveit_msgs.msg import PlaceLocation
from moveit_msgs.msg import MoveItErrorCodes
from moveit_simple_grasps.msg import GenerateGraspsAction, GenerateGraspsGoal, GraspGeneratorOptions
from tf.transformations import quaternion_from_euler
import sys
import copy
import numpy#!/usr/bin/env python
# -*- coding: utf-8 -*-import rospy, sys
import moveit_commander
from geometry_msgs.msg import PoseStamped, Pose
from moveit_commander import MoveGroupCommander, PlanningSceneInterface
from moveit_msgs.msg import PlanningScene, ObjectColor
from moveit_msgs.msg import Grasp, GripperTranslation, MoveItErrorCodesfrom trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from tf.transformations import quaternion_from_euler
from copy import deepcopy