API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jul 31st, 2018
1,346
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 14.98 KB | None | 0 0
raw download clone embed print report
  1. /*********************************************************************
  2. *Software License Agreement (BSD License)
  3. *
  4. * Copyright (c) 2012, Willow Garage, Inc.
  5. * All rights reserved.
  6. *
  7. * Redistribution and use in source and binary forms, with or without
  8. * modification, are permitted provided that the following conditions
  9. * are met:
  10. *
  11. * * Redistributions of source code must retain the above copyright
  12. * notice, this list of conditions and the following disclaimer.
  13. * * Redistributions in binary form must reproduce the above
  14. * copyright notice, this list of conditions and the following
  15. * disclaimer in the documentation and/or other materials provided
  16. * with the distribution.
  17. * * Neither the name of Willow Garage nor the names of its
  18. * contributors may be used to endorse or promote products derived
  19. * from this software without specific prior written permission.
  20. *
  21. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  22. * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  23. * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  24. * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  25. * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  26. * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  27. * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  28. * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  29. * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  30. * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  31. * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  32. * POSSIBILITY OF SUCH DAMAGE.
  33. *********************************************************************/
  34.  
  35. /* Author: Ioan Sucan, Ridhwan Luthra*/
  36.  
  37. // ROS
  38. #include <ros/ros.h>
  39.  
  40. // MoveIt!
  41. #include <moveit/planning_scene_interface/planning_scene_interface.h>
  42. #include <moveit/move_group_interface/move_group_interface.h>
  43.  
  44. void openGripper(trajectory_msgs::JointTrajectory& posture)
  45. {
  46. // BEGIN_SUB_TUTORIAL open_gripper
  47. /* Add both finger joints of panda robot. */
  48. posture.joint_names.resize(12);
  49. posture.joint_names[0] = "torso_1_joint";
  50. posture.joint_names[1] = "torso_2_joint";
  51. posture.joint_names[2] = "lh_arm_0_joint";
  52. posture.joint_names[3] = "lh_arm_1_joint";
  53. posture.joint_names[4] = "lh_arm_2_joint";
  54. posture.joint_names[5] = "lh_arm_3_joint";
  55. posture.joint_names[6] = "lh_arm_4_joint";
  56. posture.joint_names[7] = "rh_arm_0_joint";
  57. posture.joint_names[8] = "rh_arm_1_joint";
  58. posture.joint_names[9] = "rh_arm_2_joint";
  59. posture.joint_names[10] = "rh_arm_3_joint";
  60. posture.joint_names[11] = "rh_arm_4_joint";
  61.  
  62.  
  63. /* Set them as open, wide enough for the object to fit. */
  64. posture.points.resize(1); //number of waypoints
  65. posture.points[0].positions.resize(12); //number of joints
  66. /*point p and joint j --- joint_names<j> corresponds to points<p>.positions<j>*/
  67. // positions are in the joint space
  68. // open - gripper -- ready
  69. posture.points[0].positions[0] = 0.00;
  70. posture.points[0].positions[1] = 0.00;
  71. posture.points[0].positions[2] = 0.1792;
  72. posture.points[0].positions[3] = -1.0268;
  73. posture.points[0].positions[4] = 0.1629;
  74. posture.points[0].positions[5] = 1.6461;
  75. posture.points[0].positions[6] = -1.0593;
  76. posture.points[0].positions[7] = -0.1792;
  77. posture.points[0].positions[8] = 1.2224;
  78. posture.points[0].positions[9] = -0.1629;
  79. posture.points[0].positions[10] = -1.6461;
  80. posture.points[0].positions[11] = 1.0593;
  81.  
  82. // END_SUB_TUTORIAL
  83. }
  84.  
  85. void closedGripper(trajectory_msgs::JointTrajectory& posture)
  86. {
  87. // BEGIN_SUB_TUTORIAL closed_gripper
  88. /* Add both finger joints of panda robot. */
  89. posture.joint_names.resize(12);
  90. posture.joint_names[0] = "torso_1_joint";
  91. posture.joint_names[1] = "torso_2_joint";
  92. posture.joint_names[2] = "lh_arm_0_joint";
  93. posture.joint_names[3] = "lh_arm_1_joint";
  94. posture.joint_names[4] = "lh_arm_2_joint";
  95. posture.joint_names[5] = "lh_arm_3_joint";
  96. posture.joint_names[6] = "lh_arm_4_joint";
  97. posture.joint_names[7] = "rh_arm_0_joint";
  98. posture.joint_names[8] = "rh_arm_1_joint";
  99. posture.joint_names[9] = "rh_arm_2_joint";
  100. posture.joint_names[10] = "rh_arm_3_joint";
  101. posture.joint_names[11] = "rh_arm_4_joint";
  102.  
  103.  
  104. /* Set them as closed. */
  105. posture.points.resize(1); //number of waypoints
  106. posture.points[0].positions.resize(12); //number of joints
  107. /*point p and joint j --- joint_names<j> corresponds to points<p>.positions<j>*/
  108. // closed - gripper -- pick
  109. posture.points[0].positions[0] = -0.4712;
  110. posture.points[0].positions[1] = -1.0613;
  111. posture.points[0].positions[2] = 0.7008;
  112. posture.points[0].positions[3] = -1.5480;
  113. posture.points[0].positions[4] = 0.2029;
  114. posture.points[0].positions[5] = 0.0000;
  115. posture.points[0].positions[6] = 0.0000;
  116. posture.points[0].positions[7] = -0.7008;
  117. posture.points[0].positions[8] = 1.5480;
  118. posture.points[0].positions[9] = -0.2029;
  119. posture.points[0].positions[10] = 0.0000;
  120. posture.points[0].positions[11] = 0.0000;
  121.  
  122. // END_SUB_TUTORIAL
  123. }
  124.  
  125. // end effector frame: rh_palm, lh_palm
  126.  
  127. void pick(moveit::planning_interface::MoveGroupInterface& move_group)
  128. {
  129. // BEGIN_SUB_TUTORIAL pick1
  130. // Create a vector of grasps to be attempted, currently only creating single grasp.
  131. // This is essentially useful when using a grasp generator to generate and test multiple grasps.
  132. std::vector<moveit_msgs::Grasp> grasps;
  133. grasps.resize(1);
  134.  
  135. // Setting grasp pose
  136. // ++++++++++++++++++++++
  137. // This is the pose of panda_link8. |br|
  138. // 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
  139. // of the cube). |br|
  140. // Therefore, the position for panda_link8 = 5 - (length of cube/2 - distance b/w panda_link8 and palm of eef - some
  141. // extra padding)
  142.  
  143. //The pose of the parent link of the end effector during a grasp --> not the position of any link in the end effector
  144.  
  145. grasps[0].grasp_pose.header.frame_id = "base_footprint"; //base frame
  146. grasps[0].grasp_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(-M_PI / 2, -M_PI / 4, -M_PI / 2); // (roll,pitch,yaw)
  147. grasps[0].grasp_pose.pose.position.x = 0.379;
  148. grasps[0].grasp_pose.pose.position.y = 0.4545;
  149. grasps[0].grasp_pose.pose.position.z = 0.544;
  150.  
  151. // Setting pre-grasp approach
  152. // ++++++++++++++++++++++++++
  153. // The approach direction to take before picking up the object
  154. /* Defined with respect to frame_id */
  155. grasps[0].pre_grasp_approach.direction.header.frame_id = "base_footprint";
  156. /* Direction is set as positive x axis */
  157. grasps[0].pre_grasp_approach.direction.vector.z = 1.0;
  158. grasps[0].pre_grasp_approach.min_distance = 0.2;
  159. grasps[0].pre_grasp_approach.desired_distance = 0.4;
  160.  
  161. // Setting post-grasp retreat
  162. // ++++++++++++++++++++++++++
  163. /* Defined with respect to frame_id */
  164. // The retreat direction to take after the grasp has been completed --> object is attached to the gripper
  165. grasps[0].post_grasp_retreat.direction.header.frame_id = "base_footprint";
  166. /* Direction is set as positive z axis */
  167. grasps[0].post_grasp_retreat.direction.vector.y = 1.0;
  168. grasps[0].post_grasp_retreat.min_distance = 0.1;
  169. grasps[0].post_grasp_retreat.desired_distance = 0.25;
  170.  
  171. // Setting posture of eef before grasp
  172. // +++++++++++++++++++++++++++++++++++
  173. openGripper(grasps[0].pre_grasp_posture);
  174. // END_SUB_TUTORIAL
  175.  
  176. // BEGIN_SUB_TUTORIAL pick2
  177. // Setting posture of eef during grasp
  178. // +++++++++++++++++++++++++++++++++++
  179. closedGripper(grasps[0].grasp_posture);
  180. // END_SUB_TUTORIAL
  181.  
  182. // BEGIN_SUB_TUTORIAL pick3
  183. // Set support surface as table1.
  184. move_group.setSupportSurfaceName("table1");
  185. // Call pick to pick up the object using the grasps given
  186. move_group.pick("object", grasps);
  187. // END_SUB_TUTORIAL
  188. }
  189.  
  190. void place(moveit::planning_interface::MoveGroupInterface& group)
  191. {
  192. // BEGIN_SUB_TUTORIAL place
  193. // TODO(@ridhwanluthra) - Calling place function may lead to "All supplied place locations failed. Retrying last
  194. // location in
  195. // verbose mode." This is a known issue and we are working on fixing it. |br|
  196. // Create a vector of placings to be attempted, currently only creating single place location.
  197. std::vector<moveit_msgs::PlaceLocation> place_location;
  198. place_location.resize(1);
  199.  
  200. // Setting place location pose
  201. // +++++++++++++++++++++++++++
  202. place_location[0].place_pose.header.frame_id = "base_footprint";
  203. place_location[0].place_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI / 2);
  204.  
  205. /* While placing it is the exact location of the center of the object. */
  206. place_location[0].place_pose.pose.position.x = 0.379;
  207. place_location[0].place_pose.pose.position.y = -0.4545;
  208. place_location[0].place_pose.pose.position.z = 0.544;
  209.  
  210. // Setting pre-place approach
  211. // ++++++++++++++++++++++++++
  212. /* Defined with respect to frame_id */
  213. place_location[0].pre_place_approach.direction.header.frame_id = "base_footprint";
  214. /* Direction is set as negative z axis */
  215. place_location[0].pre_place_approach.direction.vector.x = 1.0;
  216. place_location[0].pre_place_approach.min_distance = 0.1;
  217. place_location[0].pre_place_approach.desired_distance = 0.2;
  218.  
  219. // Setting post-grasp retreat
  220. // ++++++++++++++++++++++++++
  221. /* Defined with respect to frame_id */
  222. place_location[0].post_place_retreat.direction.header.frame_id = "base_footprint";
  223. /* Direction is set as negative y axis */
  224. place_location[0].post_place_retreat.direction.vector.z = -1.0;
  225. place_location[0].post_place_retreat.min_distance = 0.1;
  226. place_location[0].post_place_retreat.desired_distance = 0.25;
  227.  
  228. // Setting posture of eef after placing object
  229. // +++++++++++++++++++++++++++++++++++++++++++
  230. /* Similar to the pick case */
  231. openGripper(place_location[0].post_place_posture);
  232.  
  233. // Set support surface as table2.
  234. group.setSupportSurfaceName("table2");
  235. // Call place to place the object using the place locations given.
  236. group.place("object", place_location);
  237. // END_SUB_TUTORIAL
  238. }
  239.  
  240. void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface)
  241. {
  242. // BEGIN_SUB_TUTORIAL table1
  243. //
  244. // Creating Environment
  245. // ^^^^^^^^^^^^^^^^^^^^
  246. // Create vector to hold 3 collision objects.
  247. std::vector<moveit_msgs::CollisionObject> collision_objects;
  248. collision_objects.resize(3);
  249.  
  250. // Add the first table where the cube will originally be kept.
  251. collision_objects[0].id = "table1";
  252. collision_objects[0].header.frame_id = "base_footprint";
  253.  
  254. /* Define the primitive and its dimensions. */
  255. collision_objects[0].primitives.resize(1);
  256. collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
  257. collision_objects[0].primitives[0].dimensions.resize(3);
  258. collision_objects[0].primitives[0].dimensions[0] = 0.4;
  259. collision_objects[0].primitives[0].dimensions[1] = 0.4;
  260. collision_objects[0].primitives[0].dimensions[2] = 0.2;
  261.  
  262. /* Define the pose of the table. */
  263. // position of the object for picking
  264. collision_objects[0].primitive_poses.resize(1);
  265. collision_objects[0].primitive_poses[0].position.x = 0.379;
  266. collision_objects[0].primitive_poses[0].position.y = 0.4545;
  267. collision_objects[0].primitive_poses[0].position.z = 0;
  268. // END_SUB_TUTORIAL
  269.  
  270. collision_objects[0].operation = collision_objects[0].ADD;
  271.  
  272. // BEGIN_SUB_TUTORIAL table2
  273. // Add the second table where we will be placing the cube.
  274. // position of the object for placing
  275. collision_objects[1].id = "table2";
  276. collision_objects[1].header.frame_id = "base_footprint";
  277.  
  278. /* Define the primitive and its dimensions. */
  279. collision_objects[1].primitives.resize(1);
  280. collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
  281. collision_objects[1].primitives[0].dimensions.resize(3);
  282. collision_objects[1].primitives[0].dimensions[0] = 0.4;
  283. collision_objects[1].primitives[0].dimensions[1] = 0.4;
  284. collision_objects[1].primitives[0].dimensions[2] = 0.2;
  285.  
  286. /* Define the pose of the table. */
  287. collision_objects[1].primitive_poses.resize(1);
  288. collision_objects[1].primitive_poses[0].position.x = 0.379;
  289. collision_objects[1].primitive_poses[0].position.y = -0.4545;
  290. collision_objects[1].primitive_poses[0].position.z = 0;
  291. // END_SUB_TUTORIAL
  292.  
  293. collision_objects[1].operation = collision_objects[1].ADD;
  294.  
  295. // BEGIN_SUB_TUTORIAL object
  296. // Define the object that we will be manipulating
  297. collision_objects[2].header.frame_id = "base_footprint";
  298. collision_objects[2].id = "object";
  299.  
  300. /* Define the primitive and its dimensions. */
  301. collision_objects[2].primitives.resize(1);
  302. collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;
  303. collision_objects[2].primitives[0].dimensions.resize(3);
  304. collision_objects[2].primitives[0].dimensions[0] = 0.38;
  305. collision_objects[2].primitives[0].dimensions[1] = 0.38;
  306. collision_objects[2].primitives[0].dimensions[2] = 0.38;
  307.  
  308. /* Define the pose of the object. */
  309. collision_objects[2].primitive_poses.resize(1);
  310. collision_objects[2].primitive_poses[0].position.x = 0.379;
  311. collision_objects[2].primitive_poses[0].position.y = 0.4545;
  312. collision_objects[2].primitive_poses[0].position.z = 0.2;
  313. // END_SUB_TUTORIAL
  314.  
  315. collision_objects[2].operation = collision_objects[2].ADD;
  316.  
  317. planning_scene_interface.applyCollisionObjects(collision_objects);
  318. }
  319.  
  320. int main(int argc, char** argv)
  321. {
  322. ROS_INFO_STREAM("1");
  323.  
  324. ros::init(argc, argv, "pem_pick_place"); //Node Handle
  325.  
  326. ROS_INFO_STREAM("2");
  327.  
  328. ros::NodeHandle nh;
  329. ROS_INFO_STREAM("3");
  330.  
  331. ros::AsyncSpinner spinner(1);
  332.  
  333. ROS_INFO_STREAM("4");
  334.  
  335. spinner.start();
  336.  
  337. ROS_INFO_STREAM("5");
  338.  
  339. ros::WallDuration(1.0).sleep();
  340.  
  341. ROS_INFO_STREAM("6");
  342.  
  343. moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  344.  
  345. ROS_INFO_STREAM("7");
  346.  
  347. moveit::planning_interface::MoveGroupInterface group("complete_robot"); //Move Group for the poses
  348. //try -> adding more move groups here
  349. ROS_INFO_STREAM("8");
  350.  
  351. group.setPlanningTime(45.0);
  352.  
  353. ROS_INFO_STREAM("9");
  354.  
  355. addCollisionObjects(planning_scene_interface);
  356.  
  357. ROS_INFO_STREAM("10");
  358.  
  359. // Wait a bit for ROS things to initialize
  360. ros::WallDuration(1.0).sleep();
  361.  
  362. ROS_INFO_STREAM("11");
  363.  
  364. pick(group);
  365.  
  366. ROS_INFO_STREAM("12");
  367.  
  368. ros::WallDuration(1.0).sleep();
  369.  
  370. ROS_INFO_STREAM("13");
  371.  
  372. place(group);
  373.  
  374. ros::waitForShutdown();
  375. return 0;
  376. }
  377.  
  378. // BEGIN_TUTORIAL
  379. // CALL_SUB_TUTORIAL table1
  380. // CALL_SUB_TUTORIAL table2
  381. // CALL_SUB_TUTORIAL object
  382. //
  383. // Pick Pipeline
  384. // ^^^^^^^^^^^^^
  385. // CALL_SUB_TUTORIAL pick1
  386. // openGripper function
  387. // """"""""""""""""""""
  388. // CALL_SUB_TUTORIAL open_gripper
  389. // CALL_SUB_TUTORIAL pick2
  390. // closedGripper function
  391. // """"""""""""""""""""""
  392. // CALL_SUB_TUTORIAL closed_gripper
  393. // CALL_SUB_TUTORIAL pick3
  394. //
  395. // Place Pipeline
  396. // ^^^^^^^^^^^^^^
  397. // CALL_SUB_TUTORIAL place
  398. // END_TUTORIAL
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!