From 96596c5ba5b7855cd008a4d7de023ad9afbdcf0e Mon Sep 17 00:00:00 2001 From: ridhwanluthra Date: Wed, 13 Jun 2018 14:47:31 +0530 Subject: [PATCH 1/7] A very basic implimentation of pick and place is working --- doc/pick_place/CMakeLists.txt | 4 + doc/pick_place/src/new_pick.cpp | 355 ++++++++++++++++++++++++++++++++ 2 files changed, 359 insertions(+) create mode 100644 doc/pick_place/src/new_pick.cpp diff --git a/doc/pick_place/CMakeLists.txt b/doc/pick_place/CMakeLists.txt index 808ab68f2..b0a193de6 100644 --- a/doc/pick_place/CMakeLists.txt +++ b/doc/pick_place/CMakeLists.txt @@ -1,3 +1,7 @@ add_executable(pick_place_tutorial src/pick_place_tutorial.cpp) target_link_libraries(pick_place_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES}) install(TARGETS pick_place_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +add_executable(new_pick src/new_pick.cpp) +target_link_libraries(new_pick ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +install(TARGETS new_pick DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) \ No newline at end of file diff --git a/doc/pick_place/src/new_pick.cpp b/doc/pick_place/src/new_pick.cpp new file mode 100644 index 000000000..1685dfe38 --- /dev/null +++ b/doc/pick_place/src/new_pick.cpp @@ -0,0 +1,355 @@ +/********************************************************************* + * 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 */ + +#include + +// MoveIt! +#include +#include +#include +#include +// #include +// #include + +static const std::string ROBOT_DESCRIPTION = "robot_description"; + +void openGripper(trajectory_msgs::JointTrajectory& posture) +{ + posture.joint_names.resize(2); + posture.joint_names[0] = "panda_finger_joint1"; + posture.joint_names[1] = "panda_finger_joint2"; + + posture.points.resize(1); + posture.points[0].positions.resize(2); + posture.points[0].positions[0] = 0.04; + posture.points[0].positions[1] = 0.04; +} + +void closedGripper(trajectory_msgs::JointTrajectory& posture) +{ + posture.joint_names.resize(2); + posture.joint_names[0] = "panda_finger_joint1"; + posture.joint_names[1] = "panda_finger_joint2"; + + posture.points.resize(1); + posture.points[0].positions.resize(2); + // NEED TO SET THIS ABHI!!! + posture.points[0].positions[0] = 0.00; + posture.points[0].positions[1] = 0.00; +} + +void pick(moveit::planning_interface::MoveGroupInterface& move_group) +{ + std::vector grasps; + // double i= 0.05; + geometry_msgs::PoseStamped p; + // while (i <= 0.13) + // { + // std::cout< loc; + + geometry_msgs::PoseStamped p; + // while (i <= 0.13) + // { + // std::cout<("collision_object", 10); + ros::Publisher pub_co1 = 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 collision_object; + + // remove table + collision_object.id = "table1"; + collision_object.operation = moveit_msgs::CollisionObject::REMOVE; + pub_co1.publish(collision_object); + + // // add table + // collision_object.operation = moveit_msgs::CollisionObject::ADD; + // collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.1; + // collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.3; + // collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.2; + // collision_object.primitive_poses[0].position.x = 1.5; + // collision_object.primitive_poses[0].position.y = 0; + // collision_object.primitive_poses[0].position.z = 1.2; + // pub_co.publish(collision_object); + + + + shape_msgs::SolidPrimitive primitive; + // primitive.type = primitive.CYLINDER; + // primitive.dimensions.resize(2); + // // height of cylinder + // primitive.dimensions[0] = 0.1; + // // radius of cylinder + // primitive.dimensions[1] = 0.02; + + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + // height of cylinder + primitive.dimensions[0] = 0.2; + // radius of cylinder + primitive.dimensions[1] = 0.6; + primitive.dimensions[2] = 0.4; + + // Define a pose for the cylinder (specified relative to frame_id) + geometry_msgs::Pose cylinder_pose; + // cylinder_pose.orientation.w = 1; + // Set the position of cylinder + cylinder_pose.position.x = 0.5; + cylinder_pose.position.y = 0; + cylinder_pose.position.z = 0.2; + + // Add cylinder as collision object + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(cylinder_pose); + collision_object.operation = collision_object.ADD; + pub_co1.publish(collision_object); + + + + + + + + + // remove table + // collision_object.id = "table2"; + // collision_object.operation = moveit_msgs::CollisionObject::REMOVE; + // pub_co.publish(collision_object); + + // // add table + // collision_object.operation = moveit_msgs::CollisionObject::ADD; + // collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.5; + // collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 1.5; + // collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.35; + // collision_object.primitive_poses[0].position.x = 0.7; + // collision_object.primitive_poses[0].position.y = -0.2; + // collision_object.primitive_poses[0].position.z = 0.175; + // pub_co.publish(collision_object); + + collision_object.header.frame_id = "panda_link0"; + collision_object.id = "cylinder"; + + collision_object.operation = moveit_msgs::CollisionObject::REMOVE; + pub_co.publish(collision_object); + + moveit_msgs::AttachedCollisionObject aco; + // aco.link_name = "panda_link0"; + aco.object = collision_object; + pub_aco.publish(aco); + + // moveit_msgs::AttachedCollisionObject aco; + // aco.object = collision_object; + // pub_aco.publish(aco); + + // Define a cylinder which will be added to the world. + // shape_msgs::SolidPrimitive primitive; + // primitive.type = primitive.CYLINDER; + // primitive.dimensions.resize(2); + // // height of cylinder + // primitive.dimensions[0] = 0.1; + // // radius of cylinder + // primitive.dimensions[1] = 0.02; + + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + // height of cylinder + primitive.dimensions[0] = 0.02; + // radius of cylinder + primitive.dimensions[1] = 0.02; + primitive.dimensions[2] = 0.2; + + // Define a pose for the cylinder (specified relative to frame_id) + // geometry_msgs::Pose cylinder_pose; + // cylinder_pose.orientation.w = 0; + // Set the position of cylinder + cylinder_pose.position.x = 0.5; + cylinder_pose.position.y = 0; + cylinder_pose.position.z = 0.5; + + // Add cylinder as collision object + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(cylinder_pose); + collision_object.operation = collision_object.ADD; + pub_co.publish(collision_object); + // std::vector collision_objects; + // collision_objects.push_back(collision_object); + // planning_scene_interface.applyCollisionObjects(collision_objects); + + // wait a bit for ros things to initialize + ros::WallDuration(1.0).sleep(); + + pick(group); + + ros::WallDuration(1.0).sleep(); + + place(group); + + ros::waitForShutdown(); + return 0; +} From 6440c3766509c0c08eca56ae194ba51a204940ad Mon Sep 17 00:00:00 2001 From: ridhwanluthra Date: Wed, 13 Jun 2018 15:23:01 +0530 Subject: [PATCH 2/7] Added support surfaces and now we have a sensible looking pipeline --- doc/pick_place/src/new_pick.cpp | 112 +++++++++++++------------------- 1 file changed, 45 insertions(+), 67 deletions(-) diff --git a/doc/pick_place/src/new_pick.cpp b/doc/pick_place/src/new_pick.cpp index 1685dfe38..78d8b5fe2 100644 --- a/doc/pick_place/src/new_pick.cpp +++ b/doc/pick_place/src/new_pick.cpp @@ -135,9 +135,9 @@ void place(moveit::planning_interface::MoveGroupInterface& group) tf::createQuaternionMsgFromRollPitchYaw(0, 0, 1.5707963267948966); // p.orientation.z = -0.383; // p.pose.orientation.w = 1; - // there is additional 0.165 to the base of the fingers + // While placing it is the exact location of the center of the object p.pose.position.x = 0; - p.pose.position.y = 0.415; + p.pose.position.y = 0.5; p.pose.position.z = 0.5; // geometry_msgs::PoseStamped p; @@ -160,7 +160,7 @@ void place(moveit::planning_interface::MoveGroupInterface& group) // g.pre_grasp_approach.desired_distance = i; g.post_place_retreat.direction.header.frame_id = "panda_link0"; - g.post_place_retreat.direction.vector.x = -1.0; + g.post_place_retreat.direction.vector.y = -1.0; g.post_place_retreat.min_distance = 0.1; g.post_place_retreat.desired_distance = 0.25; @@ -183,7 +183,7 @@ void place(moveit::planning_interface::MoveGroupInterface& group) openGripper(g.post_place_posture); loc.push_back(g); - // group.setSupportSurfaceName("table2"); + group.setSupportSurfaceName("table2"); // add path constraints // moveit_msgs::Constraints constr; @@ -212,96 +212,71 @@ int main(int argc, char** argv) ros::AsyncSpinner spinner(1); spinner.start(); - ros::Publisher pub_co = nh.advertise("collision_object", 10); - ros::Publisher pub_co1 = 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 collision_object; - - // remove table - collision_object.id = "table1"; - collision_object.operation = moveit_msgs::CollisionObject::REMOVE; - pub_co1.publish(collision_object); - - // // add table - // collision_object.operation = moveit_msgs::CollisionObject::ADD; - // collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.1; - // collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.3; - // collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.2; - // collision_object.primitive_poses[0].position.x = 1.5; - // collision_object.primitive_poses[0].position.y = 0; - // collision_object.primitive_poses[0].position.z = 1.2; - // pub_co.publish(collision_object); - + std::vector collision_objects; + collision_objects.resize(3); + collision_objects[0].id = "table1"; + collision_objects[0].header.frame_id = "panda_link0"; shape_msgs::SolidPrimitive primitive; - // primitive.type = primitive.CYLINDER; - // primitive.dimensions.resize(2); - // // height of cylinder - // primitive.dimensions[0] = 0.1; - // // radius of cylinder - // primitive.dimensions[1] = 0.02; - primitive.type = primitive.BOX; primitive.dimensions.resize(3); - // height of cylinder primitive.dimensions[0] = 0.2; - // radius of cylinder - primitive.dimensions[1] = 0.6; + primitive.dimensions[1] = 0.4; primitive.dimensions[2] = 0.4; // Define a pose for the cylinder (specified relative to frame_id) geometry_msgs::Pose cylinder_pose; - // cylinder_pose.orientation.w = 1; - // Set the position of cylinder cylinder_pose.position.x = 0.5; cylinder_pose.position.y = 0; cylinder_pose.position.z = 0.2; - // Add cylinder as collision object - collision_object.primitives.push_back(primitive); - collision_object.primitive_poses.push_back(cylinder_pose); - collision_object.operation = collision_object.ADD; - pub_co1.publish(collision_object); + collision_objects[0].primitives.push_back(primitive); + collision_objects[0].primitive_poses.push_back(cylinder_pose); + collision_objects[0].operation = collision_objects[0].ADD; + + collision_objects[1].id = "table2"; + collision_objects[1].header.frame_id = "panda_link0"; + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + primitive.dimensions[0] = 0.4; + primitive.dimensions[1] = 0.2; + primitive.dimensions[2] = 0.4; + // Define a pose for the cylinder (specified relative to frame_id) + cylinder_pose.position.x = 0; + cylinder_pose.position.y = 0.5; + cylinder_pose.position.z = 0.2; + + collision_objects[1].primitives.push_back(primitive); + collision_objects[1].primitive_poses.push_back(cylinder_pose); + collision_objects[1].operation = collision_objects[1].ADD; - // remove table - // collision_object.id = "table2"; - // collision_object.operation = moveit_msgs::CollisionObject::REMOVE; - // pub_co.publish(collision_object); - // // add table - // collision_object.operation = moveit_msgs::CollisionObject::ADD; - // collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.5; - // collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 1.5; - // collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.35; - // collision_object.primitive_poses[0].position.x = 0.7; - // collision_object.primitive_poses[0].position.y = -0.2; - // collision_object.primitive_poses[0].position.z = 0.175; - // pub_co.publish(collision_object); - collision_object.header.frame_id = "panda_link0"; - collision_object.id = "cylinder"; - collision_object.operation = moveit_msgs::CollisionObject::REMOVE; - pub_co.publish(collision_object); - moveit_msgs::AttachedCollisionObject aco; - // aco.link_name = "panda_link0"; - aco.object = collision_object; - pub_aco.publish(aco); + collision_objects[2].header.frame_id = "panda_link0"; + collision_objects[2].id = "cylinder"; + + // collision_object.operation = moveit_msgs::CollisionObject::REMOVE; + // pub_co.publish(collision_object); + + // moveit_msgs::AttachedCollisionObject aco; + // // aco.link_name = "panda_link0"; + // aco.object = collision_object; + // pub_aco.publish(aco); // moveit_msgs::AttachedCollisionObject aco; // aco.object = collision_object; @@ -333,10 +308,13 @@ int main(int argc, char** argv) cylinder_pose.position.z = 0.5; // Add cylinder as collision object - collision_object.primitives.push_back(primitive); - collision_object.primitive_poses.push_back(cylinder_pose); - collision_object.operation = collision_object.ADD; - pub_co.publish(collision_object); + collision_objects[2].primitives.push_back(primitive); + collision_objects[2].primitive_poses.push_back(cylinder_pose); + collision_objects[2].operation = collision_objects[2].ADD; + // pub_co.publish(collision_object); + + // collision_objects.push_back(collision_object); + planning_scene_interface.applyCollisionObjects(collision_objects); // std::vector collision_objects; // collision_objects.push_back(collision_object); // planning_scene_interface.applyCollisionObjects(collision_objects); From 539d78452fc79868959658ce2c86fcdeab46f976 Mon Sep 17 00:00:00 2001 From: ridhwanluthra Date: Tue, 19 Jun 2018 13:03:47 +0530 Subject: [PATCH 3/7] streamlining --- doc/pick_place/CMakeLists.txt | 4 - doc/pick_place/src/new_pick.cpp | 333 -------------------- doc/pick_place/src/pick_place_tutorial.cpp | 347 +++++++++++++-------- 3 files changed, 213 insertions(+), 471 deletions(-) delete mode 100644 doc/pick_place/src/new_pick.cpp diff --git a/doc/pick_place/CMakeLists.txt b/doc/pick_place/CMakeLists.txt index b0a193de6..808ab68f2 100644 --- a/doc/pick_place/CMakeLists.txt +++ b/doc/pick_place/CMakeLists.txt @@ -1,7 +1,3 @@ add_executable(pick_place_tutorial src/pick_place_tutorial.cpp) target_link_libraries(pick_place_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES}) install(TARGETS pick_place_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -add_executable(new_pick src/new_pick.cpp) -target_link_libraries(new_pick ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS new_pick DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) \ No newline at end of file diff --git a/doc/pick_place/src/new_pick.cpp b/doc/pick_place/src/new_pick.cpp deleted file mode 100644 index 78d8b5fe2..000000000 --- a/doc/pick_place/src/new_pick.cpp +++ /dev/null @@ -1,333 +0,0 @@ -/********************************************************************* - * 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 */ - -#include - -// MoveIt! -#include -#include -#include -#include -// #include -// #include - -static const std::string ROBOT_DESCRIPTION = "robot_description"; - -void openGripper(trajectory_msgs::JointTrajectory& posture) -{ - posture.joint_names.resize(2); - posture.joint_names[0] = "panda_finger_joint1"; - posture.joint_names[1] = "panda_finger_joint2"; - - posture.points.resize(1); - posture.points[0].positions.resize(2); - posture.points[0].positions[0] = 0.04; - posture.points[0].positions[1] = 0.04; -} - -void closedGripper(trajectory_msgs::JointTrajectory& posture) -{ - posture.joint_names.resize(2); - posture.joint_names[0] = "panda_finger_joint1"; - posture.joint_names[1] = "panda_finger_joint2"; - - posture.points.resize(1); - posture.points[0].positions.resize(2); - // NEED TO SET THIS ABHI!!! - posture.points[0].positions[0] = 0.00; - posture.points[0].positions[1] = 0.00; -} - -void pick(moveit::planning_interface::MoveGroupInterface& move_group) -{ - std::vector grasps; - // double i= 0.05; - geometry_msgs::PoseStamped p; - // while (i <= 0.13) - // { - // std::cout< loc; - - geometry_msgs::PoseStamped p; - // while (i <= 0.13) - // { - // std::cout< collision_objects; - collision_objects.resize(3); - - collision_objects[0].id = "table1"; - collision_objects[0].header.frame_id = "panda_link0"; - - shape_msgs::SolidPrimitive primitive; - primitive.type = primitive.BOX; - primitive.dimensions.resize(3); - primitive.dimensions[0] = 0.2; - primitive.dimensions[1] = 0.4; - primitive.dimensions[2] = 0.4; - - // Define a pose for the cylinder (specified relative to frame_id) - geometry_msgs::Pose cylinder_pose; - cylinder_pose.position.x = 0.5; - cylinder_pose.position.y = 0; - cylinder_pose.position.z = 0.2; - - collision_objects[0].primitives.push_back(primitive); - collision_objects[0].primitive_poses.push_back(cylinder_pose); - collision_objects[0].operation = collision_objects[0].ADD; - - - - - collision_objects[1].id = "table2"; - collision_objects[1].header.frame_id = "panda_link0"; - - primitive.type = primitive.BOX; - primitive.dimensions.resize(3); - primitive.dimensions[0] = 0.4; - primitive.dimensions[1] = 0.2; - primitive.dimensions[2] = 0.4; - - // Define a pose for the cylinder (specified relative to frame_id) - cylinder_pose.position.x = 0; - cylinder_pose.position.y = 0.5; - cylinder_pose.position.z = 0.2; - - collision_objects[1].primitives.push_back(primitive); - collision_objects[1].primitive_poses.push_back(cylinder_pose); - collision_objects[1].operation = collision_objects[1].ADD; - - - - - - - - collision_objects[2].header.frame_id = "panda_link0"; - collision_objects[2].id = "cylinder"; - - // collision_object.operation = moveit_msgs::CollisionObject::REMOVE; - // pub_co.publish(collision_object); - - // moveit_msgs::AttachedCollisionObject aco; - // // aco.link_name = "panda_link0"; - // aco.object = collision_object; - // pub_aco.publish(aco); - - // moveit_msgs::AttachedCollisionObject aco; - // aco.object = collision_object; - // pub_aco.publish(aco); - - // Define a cylinder which will be added to the world. - // shape_msgs::SolidPrimitive primitive; - // primitive.type = primitive.CYLINDER; - // primitive.dimensions.resize(2); - // // height of cylinder - // primitive.dimensions[0] = 0.1; - // // radius of cylinder - // primitive.dimensions[1] = 0.02; - - primitive.type = primitive.BOX; - primitive.dimensions.resize(3); - // height of cylinder - primitive.dimensions[0] = 0.02; - // radius of cylinder - primitive.dimensions[1] = 0.02; - primitive.dimensions[2] = 0.2; - - // Define a pose for the cylinder (specified relative to frame_id) - // geometry_msgs::Pose cylinder_pose; - // cylinder_pose.orientation.w = 0; - // Set the position of cylinder - cylinder_pose.position.x = 0.5; - cylinder_pose.position.y = 0; - cylinder_pose.position.z = 0.5; - - // Add cylinder as collision object - collision_objects[2].primitives.push_back(primitive); - collision_objects[2].primitive_poses.push_back(cylinder_pose); - collision_objects[2].operation = collision_objects[2].ADD; - // pub_co.publish(collision_object); - - // collision_objects.push_back(collision_object); - planning_scene_interface.applyCollisionObjects(collision_objects); - // std::vector collision_objects; - // collision_objects.push_back(collision_object); - // planning_scene_interface.applyCollisionObjects(collision_objects); - - // wait a bit for ros things to initialize - ros::WallDuration(1.0).sleep(); - - pick(group); - - ros::WallDuration(1.0).sleep(); - - place(group); - - ros::waitForShutdown(); - return 0; -} diff --git a/doc/pick_place/src/pick_place_tutorial.cpp b/doc/pick_place/src/pick_place_tutorial.cpp index c5a298a00..78d8b5fe2 100644 --- a/doc/pick_place/src/pick_place_tutorial.cpp +++ b/doc/pick_place/src/pick_place_tutorial.cpp @@ -37,74 +37,71 @@ #include // MoveIt! -#include +#include #include #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"; + posture.joint_names.resize(2); + posture.joint_names[0] = "panda_finger_joint1"; + posture.joint_names[1] = "panda_finger_joint2"; 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; } 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"; + posture.joint_names.resize(2); + posture.joint_names[0] = "panda_finger_joint1"; + posture.joint_names[1] = "panda_finger_joint2"; 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); + // NEED TO SET THIS ABHI!!! + posture.points[0].positions[0] = 0.00; + posture.points[0].positions[1] = 0.00; } -void pick(moveit::planning_interface::MoveGroupInterface& group) +void pick(moveit::planning_interface::MoveGroupInterface& move_group) { std::vector grasps; - + // double i= 0.05; geometry_msgs::PoseStamped p; - p.header.frame_id = "base_footprint"; - p.pose.position.x = 0.34; - p.pose.position.y = -0.7; + // while (i <= 0.13) + // { + // std::cout< loc; geometry_msgs::PoseStamped p; - p.header.frame_id = "base_footprint"; - p.pose.position.x = 0.7; - p.pose.position.y = 0.0; + // while (i <= 0.13) + // { + // std::cout<("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); + std::vector collision_objects; + collision_objects.resize(3); + + collision_objects[0].id = "table1"; + collision_objects[0].header.frame_id = "panda_link0"; + + shape_msgs::SolidPrimitive primitive; + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + primitive.dimensions[0] = 0.2; + primitive.dimensions[1] = 0.4; + primitive.dimensions[2] = 0.4; + + // Define a pose for the cylinder (specified relative to frame_id) + geometry_msgs::Pose cylinder_pose; + cylinder_pose.position.x = 0.5; + cylinder_pose.position.y = 0; + cylinder_pose.position.z = 0.2; + + collision_objects[0].primitives.push_back(primitive); + collision_objects[0].primitive_poses.push_back(cylinder_pose); + collision_objects[0].operation = collision_objects[0].ADD; + + + + + collision_objects[1].id = "table2"; + collision_objects[1].header.frame_id = "panda_link0"; + + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + primitive.dimensions[0] = 0.4; + primitive.dimensions[1] = 0.2; + primitive.dimensions[2] = 0.4; + + // Define a pose for the cylinder (specified relative to frame_id) + cylinder_pose.position.x = 0; + cylinder_pose.position.y = 0.5; + cylinder_pose.position.z = 0.2; + + collision_objects[1].primitives.push_back(primitive); + collision_objects[1].primitive_poses.push_back(cylinder_pose); + collision_objects[1].operation = collision_objects[1].ADD; + + + + + + + + collision_objects[2].header.frame_id = "panda_link0"; + collision_objects[2].id = "cylinder"; + + // collision_object.operation = moveit_msgs::CollisionObject::REMOVE; + // pub_co.publish(collision_object); + + // moveit_msgs::AttachedCollisionObject aco; + // // aco.link_name = "panda_link0"; + // aco.object = collision_object; + // pub_aco.publish(aco); + + // moveit_msgs::AttachedCollisionObject aco; + // aco.object = collision_object; + // pub_aco.publish(aco); + + // Define a cylinder which will be added to the world. + // shape_msgs::SolidPrimitive primitive; + // primitive.type = primitive.CYLINDER; + // primitive.dimensions.resize(2); + // // height of cylinder + // primitive.dimensions[0] = 0.1; + // // radius of cylinder + // primitive.dimensions[1] = 0.02; + + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + // height of cylinder + primitive.dimensions[0] = 0.02; + // radius of cylinder + primitive.dimensions[1] = 0.02; + primitive.dimensions[2] = 0.2; + + // Define a pose for the cylinder (specified relative to frame_id) + // geometry_msgs::Pose cylinder_pose; + // cylinder_pose.orientation.w = 0; + // Set the position of cylinder + cylinder_pose.position.x = 0.5; + cylinder_pose.position.y = 0; + cylinder_pose.position.z = 0.5; + + // Add cylinder as collision object + collision_objects[2].primitives.push_back(primitive); + collision_objects[2].primitive_poses.push_back(cylinder_pose); + collision_objects[2].operation = collision_objects[2].ADD; + // pub_co.publish(collision_object); + + // collision_objects.push_back(collision_object); + planning_scene_interface.applyCollisionObjects(collision_objects); + // std::vector collision_objects; + // collision_objects.push_back(collision_object); + // planning_scene_interface.applyCollisionObjects(collision_objects); // wait a bit for ros things to initialize ros::WallDuration(1.0).sleep(); From 1172e2654efdb2fa95e50c27bb34ea3429e4425d Mon Sep 17 00:00:00 2001 From: ridhwanluthra Date: Tue, 19 Jun 2018 17:11:39 +0530 Subject: [PATCH 4/7] Built Tutorial for pick_place --- doc/pick_place/pick_place_tutorial.rst | 46 +++ doc/pick_place/src/pick_place_tutorial.cpp | 400 ++++++++------------- index.rst | 1 + 3 files changed, 202 insertions(+), 245 deletions(-) create mode 100644 doc/pick_place/pick_place_tutorial.rst diff --git a/doc/pick_place/pick_place_tutorial.rst b/doc/pick_place/pick_place_tutorial.rst new file mode 100644 index 000000000..f64fc1d8a --- /dev/null +++ b/doc/pick_place/pick_place_tutorial.rst @@ -0,0 +1,46 @@ +Pick and Place Tutorial +============================ + +In MoveIt!, grasping is done using the move group 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 shells. In the first shell start RViz and wait for everything to finish loading: :: + + roslaunch panda_moveit_config demo.launch + +In the second shell 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 `this. `_ + +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. + +.. tutorial-formatter:: ./src/pick_place_tutorial.cpp \ No newline at end of file diff --git a/doc/pick_place/src/pick_place_tutorial.cpp b/doc/pick_place/src/pick_place_tutorial.cpp index 78d8b5fe2..a07fda131 100644 --- a/doc/pick_place/src/pick_place_tutorial.cpp +++ b/doc/pick_place/src/pick_place_tutorial.cpp @@ -1,208 +1,128 @@ -/********************************************************************* - * 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 */ - +// ROS #include // MoveIt! #include #include -#include -#include -// #include -// #include - -static const std::string ROBOT_DESCRIPTION = "robot_description"; 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; + // 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); - // NEED TO SET THIS ABHI!!! posture.points[0].positions[0] = 0.00; posture.points[0].positions[1] = 0.00; + // 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 grasps; - // double i= 0.05; - geometry_msgs::PoseStamped p; - // while (i <= 0.13) - // { - // std::cout< loc; + loc.resize(1); - geometry_msgs::PoseStamped p; - // while (i <= 0.13) - // { - // std::cout< collision_objects; collision_objects.resize(3); + // Add the first table where the cube will orignally be kept. collision_objects[0].id = "table1"; collision_objects[0].header.frame_id = "panda_link0"; - shape_msgs::SolidPrimitive primitive; - primitive.type = primitive.BOX; - primitive.dimensions.resize(3); - primitive.dimensions[0] = 0.2; - primitive.dimensions[1] = 0.4; - primitive.dimensions[2] = 0.4; - - // Define a pose for the cylinder (specified relative to frame_id) - geometry_msgs::Pose cylinder_pose; - cylinder_pose.position.x = 0.5; - cylinder_pose.position.y = 0; - cylinder_pose.position.z = 0.2; - - collision_objects[0].primitives.push_back(primitive); - collision_objects[0].primitive_poses.push_back(cylinder_pose); - collision_objects[0].operation = collision_objects[0].ADD; - - + // Define the primitive and its dimentions. + 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"; - primitive.type = primitive.BOX; - primitive.dimensions.resize(3); - primitive.dimensions[0] = 0.4; - primitive.dimensions[1] = 0.2; - primitive.dimensions[2] = 0.4; + // Define the primitive and its dimentions. + 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 - // Define a pose for the cylinder (specified relative to frame_id) - cylinder_pose.position.x = 0; - cylinder_pose.position.y = 0.5; - cylinder_pose.position.z = 0.2; - - collision_objects[1].primitives.push_back(primitive); - collision_objects[1].primitive_poses.push_back(cylinder_pose); collision_objects[1].operation = collision_objects[1].ADD; - - - - - - + // BEGIN_SUB_TUTORIAL object collision_objects[2].header.frame_id = "panda_link0"; - collision_objects[2].id = "cylinder"; - - // collision_object.operation = moveit_msgs::CollisionObject::REMOVE; - // pub_co.publish(collision_object); - - // moveit_msgs::AttachedCollisionObject aco; - // // aco.link_name = "panda_link0"; - // aco.object = collision_object; - // pub_aco.publish(aco); - - // moveit_msgs::AttachedCollisionObject aco; - // aco.object = collision_object; - // pub_aco.publish(aco); - - // Define a cylinder which will be added to the world. - // shape_msgs::SolidPrimitive primitive; - // primitive.type = primitive.CYLINDER; - // primitive.dimensions.resize(2); - // // height of cylinder - // primitive.dimensions[0] = 0.1; - // // radius of cylinder - // primitive.dimensions[1] = 0.02; - - primitive.type = primitive.BOX; - primitive.dimensions.resize(3); - // height of cylinder - primitive.dimensions[0] = 0.02; - // radius of cylinder - primitive.dimensions[1] = 0.02; - primitive.dimensions[2] = 0.2; - - // Define a pose for the cylinder (specified relative to frame_id) - // geometry_msgs::Pose cylinder_pose; - // cylinder_pose.orientation.w = 0; - // Set the position of cylinder - cylinder_pose.position.x = 0.5; - cylinder_pose.position.y = 0; - cylinder_pose.position.z = 0.5; - - // Add cylinder as collision object - collision_objects[2].primitives.push_back(primitive); - collision_objects[2].primitive_poses.push_back(cylinder_pose); + collision_objects[2].id = "object"; + + // Define the primitive and its dimentions. + 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; - // pub_co.publish(collision_object); - // collision_objects.push_back(collision_object); planning_scene_interface.applyCollisionObjects(collision_objects); - // std::vector collision_objects; - // collision_objects.push_back(collision_object); - // planning_scene_interface.applyCollisionObjects(collision_objects); // wait a bit for ros things to initialize ros::WallDuration(1.0).sleep(); @@ -331,3 +223,21 @@ 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 +// CALL_SUB_TUTORIAL open_gripper +// CALL_SUB_TUTORIAL pick2 +// CALL_SUB_TUTORIAL closed_gripper +// CALL_SUB_TUTORIAL pick3 +// +// Place Pipeline +// ^^^^^^^^^^^^^^ +// CALL_SUB_TUTORIAL place +// END_TUTORIAL \ No newline at end of file diff --git a/index.rst b/index.rst index 929c8e198..901c443b3 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 ---------------------------- From 82ad19d1e4009e7c26edfd5ad00651b8c18e8b2c Mon Sep 17 00:00:00 2001 From: ridhwanluthra Date: Thu, 28 Jun 2018 02:28:37 +0530 Subject: [PATCH 5/7] handling reviews and improving formating * making sections for better explaination * using M_PI * separate function for adding collision objects * using /**/ comments to not break flow * other minor fixes --- doc/pick_place/pick_place_tutorial.rst | 8 +- doc/pick_place/src/pick_place_tutorial.cpp | 149 +++++++++++++++------ 2 files changed, 111 insertions(+), 46 deletions(-) diff --git a/doc/pick_place/pick_place_tutorial.rst b/doc/pick_place/pick_place_tutorial.rst index f64fc1d8a..17e74c0cf 100644 --- a/doc/pick_place/pick_place_tutorial.rst +++ b/doc/pick_place/pick_place_tutorial.rst @@ -1,7 +1,7 @@ Pick and Place Tutorial ============================ -In MoveIt!, grasping is done using the move group 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. +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 @@ -16,11 +16,11 @@ If you haven't already done so, make sure you've completed the steps in `Getting Running The Demo ---------------- -Open two shells. In the first shell start RViz and wait for everything to finish loading: :: +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 shell run the pick and place tutorial: :: +In the second terminal run the pick and place tutorial: :: rosrun moveit_tutorials pick_place_tutotrial @@ -28,7 +28,7 @@ You should see something similar to the video at the beginning of this tutorial. Understanding ``moveit_msgs::Grasp`` ------------------------------------ -For complete documentation refer to `this. `_ +For complete documentation refer to `moveit_msgs/Grasp.msg. `_ The relevant fields of the message are:- diff --git a/doc/pick_place/src/pick_place_tutorial.cpp b/doc/pick_place/src/pick_place_tutorial.cpp index a07fda131..c711ca5e3 100644 --- a/doc/pick_place/src/pick_place_tutorial.cpp +++ b/doc/pick_place/src/pick_place_tutorial.cpp @@ -1,3 +1,40 @@ +/********************************************************************* +* 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 @@ -8,12 +45,12 @@ void openGripper(trajectory_msgs::JointTrajectory& posture) { // BEGIN_SUB_TUTORIAL open_gripper - // Add both finger joints of panda robot. + /* 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. + /* 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; @@ -24,12 +61,12 @@ void openGripper(trajectory_msgs::JointTrajectory& posture) void closedGripper(trajectory_msgs::JointTrajectory& posture) { // BEGIN_SUB_TUTORIAL closed_gripper - // Add both finger joints of panda robot. + /* 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. + /* Set them as closed. */ posture.points.resize(1); posture.points[0].positions.resize(2); posture.points[0].positions[0] = 0.00; @@ -45,37 +82,46 @@ void pick(moveit::planning_interface::MoveGroupInterface& move_group) std::vector grasps; grasps.resize(1); - // Setting the grasp pose + // Setting grasp pose + // ++++++++++++++++++++++ grasps[0].grasp_pose.header.frame_id = "panda_link0"; grasps[0].grasp_pose.pose.orientation = - tf::createQuaternionMsgFromRollPitchYaw(-1.5707963267948966, -0.7853981633974483, -1.5707963267948966); - // This is the pose of panda_link8. - // 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). - // Therfore, the posotion for panda_link8 = 5 - (length of cube/2 - distance b/w panda_link8 and palm of eef - some - // extra padding + tf::createQuaternionMsgFromRollPitchYaw(-M_PI / 2, -M_PI / 4, -M_PI / 2); + /* This is the pose of panda_link8. */ + /* 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). */ + /* Therfore, the posotion for panda_link8 = 5 - (length of cube/2 - distance b/w panda_link8 and palm of eef - some */ + /* extra padding) */ 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 the pre_grasp_approach with direction as positive x axis. - grasps[0].pre_grasp_approach.direction.vector.x = 1.0; + // 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 the post_grasp_retreat with direction as positive z axis. + // 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; - // Set the posture of the eef before we start the grasp. + // Setting posture of eef before grasp + // +++++++++++++++++++++++++++++++++++ openGripper(grasps[0].pre_grasp_posture); // END_SUB_TUTORIAL // BEGIN_SUB_TUTORIAL pick2 - // Set the posture of the eef for the grasp + // Setting posture of eef during grasp + // +++++++++++++++++++++++++++++++++++ closedGripper(grasps[0].grasp_posture); // END_SUB_TUTORIAL @@ -94,28 +140,37 @@ void place(moveit::planning_interface::MoveGroupInterface& group) std::vector loc; loc.resize(1); - // Setting the place location pose. + // Setting place location pose + // +++++++++++++++++++++++++++ loc[0].place_pose.header.frame_id = "panda_link0"; - loc[0].place_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 1.5707963267948966); + loc[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 + /* While placing it is the exact location of the center of the object. */ loc[0].place_pose.pose.position.x = 0; loc[0].place_pose.pose.position.y = 0.5; loc[0].place_pose.pose.position.z = 0.5; - // Setting the pre_place_approach with direction as -ve z-axis. - loc[0].pre_place_approach.direction.vector.z = -1.0; + // Setting pre-place approach + // ++++++++++++++++++++++++++ + /* Defined with respect to frame_id */ loc[0].pre_place_approach.direction.header.frame_id = "panda_link0"; + /* Direction is set as negative z axis */ + loc[0].pre_place_approach.direction.vector.z = -1.0; loc[0].pre_place_approach.min_distance = 0.095; loc[0].pre_place_approach.desired_distance = 0.115; - // Setting the post_grasp_retreat with direction as -ve y-axis + // Setting post-grasp retreat + // ++++++++++++++++++++++++++ + /* Defined with respect to frame_id */ loc[0].post_place_retreat.direction.header.frame_id = "panda_link0"; + /* Direction is set as negative y axis */ loc[0].post_place_retreat.direction.vector.y = -1.0; loc[0].post_place_retreat.min_distance = 0.1; loc[0].post_place_retreat.desired_distance = 0.25; - // Set the posture of the eef after placing the object similarly to the pick case. + // Setting posture of eef after placing object + // +++++++++++++++++++++++++++++++++++++++++++ + /* similar to the pick case */ openGripper(loc[0].post_place_posture); // Set support surface as table2. @@ -125,22 +180,12 @@ void place(moveit::planning_interface::MoveGroupInterface& group) // END_SUB_TUTORIAL } -int main(int argc, char** argv) +void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface) { - 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); - // BEGIN_SUB_TUTORIAL table1 // - // Adding Collision Objects - // ^^^^^^^^^^^^^^^^^^^^^^^^ + // Creating Environment + // ^^^^^^^^^^^^^^^^^^^^ // Create vector to hold 3 collision objects. std::vector collision_objects; collision_objects.resize(3); @@ -149,7 +194,7 @@ int main(int argc, char** argv) collision_objects[0].id = "table1"; collision_objects[0].header.frame_id = "panda_link0"; - // Define the primitive and its dimentions. + /* Define the primitive and its dimentions. */ 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); @@ -157,7 +202,7 @@ int main(int argc, char** argv) collision_objects[0].primitives[0].dimensions[1] = 0.4; collision_objects[0].primitives[0].dimensions[2] = 0.4; - // Define the pose of the table. + /* 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; @@ -171,7 +216,7 @@ int main(int argc, char** argv) collision_objects[1].id = "table2"; collision_objects[1].header.frame_id = "panda_link0"; - // Define the primitive and its dimentions. + /* Define the primitive and its dimentions. */ 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); @@ -179,7 +224,7 @@ int main(int argc, char** argv) collision_objects[1].primitives[0].dimensions[1] = 0.2; collision_objects[1].primitives[0].dimensions[2] = 0.4; - // Define the pose of the table. + /* 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; @@ -189,10 +234,11 @@ int main(int argc, char** argv) 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 dimentions. + /* Define the primitive and its dimentions. */ 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); @@ -200,7 +246,7 @@ int main(int argc, char** argv) collision_objects[2].primitives[0].dimensions[1] = 0.02; collision_objects[2].primitives[0].dimensions[2] = 0.2; - // Define the pose of the object. + /* 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; @@ -210,6 +256,21 @@ int main(int argc, char** argv) 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::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 initialize ros::WallDuration(1.0).sleep(); @@ -232,8 +293,12 @@ int main(int argc, char** argv) // 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 // From 8665f8c76d14367fe395e5ec24e7154e730ebb89 Mon Sep 17 00:00:00 2001 From: ridhwanluthra Date: Thu, 12 Jul 2018 02:46:17 +0530 Subject: [PATCH 6/7] fixing spelling and adding a note and todo for a known issue --- doc/pick_place/src/pick_place_tutorial.cpp | 92 +++++++++++----------- 1 file changed, 47 insertions(+), 45 deletions(-) diff --git a/doc/pick_place/src/pick_place_tutorial.cpp b/doc/pick_place/src/pick_place_tutorial.cpp index c711ca5e3..179d980b1 100644 --- a/doc/pick_place/src/pick_place_tutorial.cpp +++ b/doc/pick_place/src/pick_place_tutorial.cpp @@ -1,39 +1,38 @@ -/********************************************************************* -* 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*/ +/********************************************************************* +* 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 @@ -85,12 +84,13 @@ void pick(moveit::planning_interface::MoveGroupInterface& move_group) // Setting grasp pose // ++++++++++++++++++++++ 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.orientation = tf::createQuaternionMsgFromRollPitchYaw(-M_PI / 2, -M_PI / 4, -M_PI / 2); /* This is the pose of panda_link8. */ - /* 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 */ + /* 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). */ - /* Therfore, the posotion for panda_link8 = 5 - (length of cube/2 - distance b/w panda_link8 and palm of eef - some */ + /* 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.pose.position.x = 0.415; grasps[0].grasp_pose.pose.position.y = 0; @@ -136,6 +136,8 @@ void pick(moveit::planning_interface::MoveGroupInterface& move_group) void place(moveit::planning_interface::MoveGroupInterface& group) { // BEGIN_SUB_TUTORIAL place + // Note(TODO) - 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. // Create a vector of placings to be attempted, currently only creating single place location. std::vector loc; loc.resize(1); @@ -190,11 +192,11 @@ void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& pla std::vector collision_objects; collision_objects.resize(3); - // Add the first table where the cube will orignally be kept. + // 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 dimentions. */ + /* 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); @@ -216,7 +218,7 @@ void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& pla collision_objects[1].id = "table2"; collision_objects[1].header.frame_id = "panda_link0"; - /* Define the primitive and its dimentions. */ + /* 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); @@ -238,7 +240,7 @@ void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& pla collision_objects[2].header.frame_id = "panda_link0"; collision_objects[2].id = "object"; - /* Define the primitive and its dimentions. */ + /* 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); @@ -272,7 +274,7 @@ int main(int argc, char** argv) addCollisionObjects(planning_scene_interface); - // wait a bit for ros things to initialize + // wait a bit for ROS things to initialize ros::WallDuration(1.0).sleep(); pick(group); @@ -305,4 +307,4 @@ int main(int argc, char** argv) // Place Pipeline // ^^^^^^^^^^^^^^ // CALL_SUB_TUTORIAL place -// END_TUTORIAL \ No newline at end of file +// END_TUTORIAL From 662440dcffb5492c06fd3f884b12f24e81f102ae Mon Sep 17 00:00:00 2001 From: ridhwanluthra Date: Thu, 12 Jul 2018 16:59:15 +0530 Subject: [PATCH 7/7] fixing styling, typos and naming --- doc/pick_place/pick_place_tutorial.rst | 6 ++- doc/pick_place/src/pick_place_tutorial.cpp | 55 +++++++++++----------- 2 files changed, 32 insertions(+), 29 deletions(-) diff --git a/doc/pick_place/pick_place_tutorial.rst b/doc/pick_place/pick_place_tutorial.rst index 17e74c0cf..e18e5f75b 100644 --- a/doc/pick_place/pick_place_tutorial.rst +++ b/doc/pick_place/pick_place_tutorial.rst @@ -43,4 +43,8 @@ The Entire Code --------------- The entire code can be seen :codedir:`here ` in the moveit_tutorials GitHub project. -.. tutorial-formatter:: ./src/pick_place_tutorial.cpp \ No newline at end of file +.. |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 179d980b1..9e02e168e 100644 --- a/doc/pick_place/src/pick_place_tutorial.cpp +++ b/doc/pick_place/src/pick_place_tutorial.cpp @@ -83,15 +83,13 @@ void pick(moveit::planning_interface::MoveGroupInterface& move_group) // 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); - /* This is the pose of panda_link8. */ - /* 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). */ - /* 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.pose.position.x = 0.415; grasps[0].grasp_pose.pose.position.y = 0; grasps[0].grasp_pose.pose.position.z = 0.5; @@ -136,49 +134,50 @@ void pick(moveit::planning_interface::MoveGroupInterface& move_group) void place(moveit::planning_interface::MoveGroupInterface& group) { // BEGIN_SUB_TUTORIAL place - // Note(TODO) - 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. + // 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 loc; - loc.resize(1); + std::vector place_location; + place_location.resize(1); // Setting place location pose // +++++++++++++++++++++++++++ - loc[0].place_pose.header.frame_id = "panda_link0"; - loc[0].place_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI / 2); + 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. */ - loc[0].place_pose.pose.position.x = 0; - loc[0].place_pose.pose.position.y = 0.5; - loc[0].place_pose.pose.position.z = 0.5; + 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 */ - loc[0].pre_place_approach.direction.header.frame_id = "panda_link0"; + place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0"; /* Direction is set as negative z axis */ - loc[0].pre_place_approach.direction.vector.z = -1.0; - loc[0].pre_place_approach.min_distance = 0.095; - loc[0].pre_place_approach.desired_distance = 0.115; + 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 */ - loc[0].post_place_retreat.direction.header.frame_id = "panda_link0"; + place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0"; /* Direction is set as negative y axis */ - loc[0].post_place_retreat.direction.vector.y = -1.0; - loc[0].post_place_retreat.min_distance = 0.1; - loc[0].post_place_retreat.desired_distance = 0.25; + 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(loc[0].post_place_posture); + /* 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", loc); + group.place("object", place_location); // END_SUB_TUTORIAL } @@ -274,7 +273,7 @@ int main(int argc, char** argv) addCollisionObjects(planning_scene_interface); - // wait a bit for ROS things to initialize + // Wait a bit for ROS things to initialize ros::WallDuration(1.0).sleep(); pick(group);