diff --git a/doc/pick_place/pick_place_tutorial.rst b/doc/pick_place/pick_place_tutorial.rst
new file mode 100644
index 000000000..e18e5f75b
--- /dev/null
+++ b/doc/pick_place/pick_place_tutorial.rst
@@ -0,0 +1,50 @@
+Pick and Place Tutorial
+============================
+
+In MoveIt!, grasping is done using the MoveGroup interface. In order to grasp an object we need to create ``moveit_msgs::Grasp`` msg which will allow defining the various poses and postures involved in a grasping operation.
+Watch this video to see the output of this tutorial:
+
+.. raw:: html
+
+
+
+
+
+Getting Started
+---------------
+If you haven't already done so, make sure you've completed the steps in `Getting Started <../getting_started/getting_started.html>`_.
+
+Running The Demo
+----------------
+Open two terminals. In the first terminal start RViz and wait for everything to finish loading: ::
+
+ roslaunch panda_moveit_config demo.launch
+
+In the second terminal run the pick and place tutorial: ::
+
+ rosrun moveit_tutorials pick_place_tutotrial
+
+You should see something similar to the video at the beginning of this tutorial.
+
+Understanding ``moveit_msgs::Grasp``
+------------------------------------
+For complete documentation refer to `moveit_msgs/Grasp.msg. `_
+
+The relevant fields of the message are:-
+
+* ``trajectory_msgs/JointTrajectory pre_grasp_posture`` - 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`` - This defines the trajectory position of the joints in the end effector group for grasping the object.
+* ``geometry_msgs/PoseStamped grasp_pose`` - Pose of the end effector in which it should attempt grasping.
+* ``moveit_msgs/GripperTranslation pre_grasp_approach`` - This is used to define the direction from which to approach the object and the distance to travel.
+* ``moveit_msgs/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.
+* ``moveit_msgs/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.
+
+The Entire Code
+---------------
+The entire code can be seen :codedir:`here ` in the moveit_tutorials GitHub project.
+
+.. |br| raw:: html
+
+
+
+.. tutorial-formatter:: ./src/pick_place_tutorial.cpp
diff --git a/doc/pick_place/src/pick_place_tutorial.cpp b/doc/pick_place/src/pick_place_tutorial.cpp
index c5a298a00..9e02e168e 100644
--- a/doc/pick_place/src/pick_place_tutorial.cpp
+++ b/doc/pick_place/src/pick_place_tutorial.cpp
@@ -1,246 +1,279 @@
/*********************************************************************
- * 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 */
-
+* 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
// MoveIt!
-#include
+#include
#include
-#include
-
-static const std::string ROBOT_DESCRIPTION = "robot_description";
void openGripper(trajectory_msgs::JointTrajectory& posture)
{
- posture.joint_names.resize(6);
- posture.joint_names[0] = "r_gripper_joint";
- posture.joint_names[1] = "r_gripper_motor_screw_joint";
- posture.joint_names[2] = "r_gripper_l_finger_joint";
- posture.joint_names[3] = "r_gripper_r_finger_joint";
- posture.joint_names[4] = "r_gripper_r_finger_tip_joint";
- posture.joint_names[5] = "r_gripper_l_finger_tip_joint";
+ // 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(6);
- posture.points[0].positions[0] = 1;
- posture.points[0].positions[1] = 1.0;
- posture.points[0].positions[2] = 0.477;
- posture.points[0].positions[3] = 0.477;
- posture.points[0].positions[4] = 0.477;
- posture.points[0].positions[5] = 0.477;
+ posture.points[0].positions.resize(2);
+ posture.points[0].positions[0] = 0.04;
+ posture.points[0].positions[1] = 0.04;
+ // END_SUB_TUTORIAL
}
void closedGripper(trajectory_msgs::JointTrajectory& posture)
{
- posture.joint_names.resize(6);
- posture.joint_names[0] = "r_gripper_joint";
- posture.joint_names[1] = "r_gripper_motor_screw_joint";
- posture.joint_names[2] = "r_gripper_l_finger_joint";
- posture.joint_names[3] = "r_gripper_r_finger_joint";
- posture.joint_names[4] = "r_gripper_r_finger_tip_joint";
- posture.joint_names[5] = "r_gripper_l_finger_tip_joint";
+ // 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(6);
- posture.points[0].positions[0] = 0;
- posture.points[0].positions[1] = 0;
- posture.points[0].positions[2] = 0.002;
- posture.points[0].positions[3] = 0.002;
- posture.points[0].positions[4] = 0.002;
- posture.points[0].positions[5] = 0.002;
+ posture.points[0].positions.resize(2);
+ posture.points[0].positions[0] = 0.00;
+ posture.points[0].positions[1] = 0.00;
+ // END_SUB_TUTORIAL
}
-void pick(moveit::planning_interface::MoveGroupInterface& group)
+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 grasps;
-
- geometry_msgs::PoseStamped p;
- p.header.frame_id = "base_footprint";
- p.pose.position.x = 0.34;
- p.pose.position.y = -0.7;
- p.pose.position.z = 0.5;
- p.pose.orientation.x = 0;
- p.pose.orientation.y = 0;
- p.pose.orientation.z = 0;
- p.pose.orientation.w = 1;
- moveit_msgs::Grasp g;
- g.grasp_pose = p;
-
- g.pre_grasp_approach.direction.vector.x = 1.0;
- g.pre_grasp_approach.direction.header.frame_id = "r_wrist_roll_link";
- g.pre_grasp_approach.min_distance = 0.2;
- g.pre_grasp_approach.desired_distance = 0.4;
-
- g.post_grasp_retreat.direction.header.frame_id = "base_footprint";
- g.post_grasp_retreat.direction.vector.z = 1.0;
- g.post_grasp_retreat.min_distance = 0.1;
- g.post_grasp_retreat.desired_distance = 0.25;
-
- openGripper(g.pre_grasp_posture);
-
- closedGripper(g.grasp_posture);
-
- grasps.push_back(g);
- group.setSupportSurfaceName("table");
- group.pick("part", 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";
+ grasps[0].grasp_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(-M_PI / 2, -M_PI / 4, -M_PI / 2);
+ 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 given
+ move_group.pick("object", grasps);
+ // END_SUB_TUTORIAL
}
void place(moveit::planning_interface::MoveGroupInterface& group)
{
- std::vector loc;
-
- geometry_msgs::PoseStamped p;
- p.header.frame_id = "base_footprint";
- p.pose.position.x = 0.7;
- p.pose.position.y = 0.0;
- p.pose.position.z = 0.5;
- p.pose.orientation.x = 0;
- p.pose.orientation.y = 0;
- p.pose.orientation.z = 0;
- p.pose.orientation.w = 1;
- moveit_msgs::PlaceLocation g;
- g.place_pose = p;
-
- g.pre_place_approach.direction.vector.z = -1.0;
- g.post_place_retreat.direction.vector.x = -1.0;
- g.post_place_retreat.direction.header.frame_id = "base_footprint";
- g.pre_place_approach.direction.header.frame_id = "r_wrist_roll_link";
- g.pre_place_approach.min_distance = 0.1;
- g.pre_place_approach.desired_distance = 0.2;
- g.post_place_retreat.min_distance = 0.1;
- g.post_place_retreat.desired_distance = 0.25;
-
- openGripper(g.post_place_posture);
-
- loc.push_back(g);
- group.setSupportSurfaceName("table");
-
- // add path constraints
- moveit_msgs::Constraints constr;
- constr.orientation_constraints.resize(1);
- moveit_msgs::OrientationConstraint& ocm = constr.orientation_constraints[0];
- ocm.link_name = "r_wrist_roll_link";
- ocm.header.frame_id = p.header.frame_id;
- ocm.orientation.x = 0.0;
- ocm.orientation.y = 0.0;
- ocm.orientation.z = 0.0;
- ocm.orientation.w = 1.0;
- ocm.absolute_x_axis_tolerance = 0.2;
- ocm.absolute_y_axis_tolerance = 0.2;
- ocm.absolute_z_axis_tolerance = M_PI;
- ocm.weight = 1.0;
- // group.setPathConstraints(constr);
- group.setPlannerId("RRTConnectkConfigDefault");
-
- group.place("part", loc);
+ // 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 place_location;
+ place_location.resize(1);
+
+ // Setting place location pose
+ // +++++++++++++++++++++++++++
+ place_location[0].place_pose.header.frame_id = "panda_link0";
+ place_location[0].place_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI / 2);
+
+ /* 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 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_TUTORIAL
+
+ collision_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_TUTORIAL
+
+ collision_objects[1].operation = collision_objects[1].ADD;
+
+ // 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;
+
+ 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::NodeHandle nh;
- ros::Publisher pub_co = nh.advertise("collision_object", 10);
- ros::Publisher pub_aco = nh.advertise("attached_collision_object", 10);
-
ros::WallDuration(1.0).sleep();
-
+ moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
moveit::planning_interface::MoveGroupInterface group("panda_arm");
group.setPlanningTime(45.0);
- moveit_msgs::CollisionObject co;
- co.header.stamp = ros::Time::now();
- co.header.frame_id = "base_footprint";
-
- // remove pole
- co.id = "pole";
- co.operation = moveit_msgs::CollisionObject::REMOVE;
- pub_co.publish(co);
-
- // add pole
- co.operation = moveit_msgs::CollisionObject::ADD;
- co.primitives.resize(1);
- co.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
- co.primitives[0].dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value);
- 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] = 1.0;
- co.primitive_poses.resize(1);
- co.primitive_poses[0].position.x = 0.7;
- co.primitive_poses[0].position.y = -0.4;
- co.primitive_poses[0].position.z = 0.85;
- co.primitive_poses[0].orientation.w = 1.0;
- pub_co.publish(co);
-
- // remove table
- co.id = "table";
- co.operation = moveit_msgs::CollisionObject::REMOVE;
- pub_co.publish(co);
-
- // add table
- co.operation = moveit_msgs::CollisionObject::ADD;
- co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.5;
- co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 1.5;
- co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.35;
- co.primitive_poses[0].position.x = 0.7;
- co.primitive_poses[0].position.y = -0.2;
- co.primitive_poses[0].position.z = 0.175;
- pub_co.publish(co);
-
- co.id = "part";
- co.operation = moveit_msgs::CollisionObject::REMOVE;
- pub_co.publish(co);
-
- moveit_msgs::AttachedCollisionObject aco;
- aco.object = co;
- pub_aco.publish(aco);
-
- co.operation = moveit_msgs::CollisionObject::ADD;
- co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.15;
- co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.1;
- co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.3;
-
- co.primitive_poses[0].position.x = 0.6;
- co.primitive_poses[0].position.y = -0.7;
- co.primitive_poses[0].position.z = 0.5;
- pub_co.publish(co);
-
- // wait a bit for ros things to initialize
+ addCollisionObjects(planning_scene_interface);
+
+ // Wait a bit for ROS things to initialize
ros::WallDuration(1.0).sleep();
pick(group);
@@ -252,3 +285,25 @@ int main(int argc, char** argv)
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
diff --git a/index.rst b/index.rst
index c6749f4de..18beefdd2 100644
--- a/index.rst
+++ b/index.rst
@@ -42,6 +42,7 @@ Building more complex applications with MoveIt! often requires developers to dig
doc/visualizing_collisions/visualizing_collisions_tutorial
doc/time_parameterization/time_parameterization_tutorial
doc/planning_with_approximated_constraint_manifolds/planning_with_approximated_constraint_manifolds_tutorial
+ doc/pick_place/pick_place_tutorial
Integration with a New Robot
----------------------------