Advertisement
Guest User

Untitled

a guest
Jul 31st, 2018
930
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  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
Advertisement
Advertisement
RAW Paste Data Copied
Advertisement