moveit2
The MoveIt Motion Planning Framework for ROS 2.
move_group_pick_place_test.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2020, Tyler Weaver
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 PickNik Inc. nor the
18  * names of its contributors may be used to endorse or promote
19  * products derived from this software without specific prior
20  * written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Tyler Weaver */
37 
38 /* These integration tests are based on the tutorials for using move_group to do a pick and place:
39  * https://ros-planning.github.io/moveit_tutorials/doc/pick_place/pick_place_tutorial.html
40  */
41 
42 // C++
43 #include <vector>
44 
45 // ROS
46 #include <ros/ros.h>
47 
48 // The Testing Framework and Utils
49 #include <gtest/gtest.h>
50 
51 // MoveIt
54 
55 // TF2
56 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
57 
58 static const std::string PLANNING_GROUP = "panda_arm";
59 constexpr double PLANNING_TIME_S = 45.0;
60 constexpr double MAX_VELOCITY_SCALE = 1.0;
61 constexpr double MAX_ACCELERATION_SCALE = 1.0;
62 
63 class PickPlaceTestFixture : public ::testing::Test
64 {
65 public:
66  void SetUp() override
67  {
68  nh_ = ros::NodeHandle("/move_group_pick_place_test");
69  move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(PLANNING_GROUP);
70 
71  // set velocity and acceleration scaling factors (full speed)
72  move_group_->setMaxVelocityScalingFactor(MAX_VELOCITY_SCALE);
73  move_group_->setMaxAccelerationScalingFactor(MAX_ACCELERATION_SCALE);
74 
75  // allow more time for planning
76  move_group_->setPlanningTime(PLANNING_TIME_S);
77  }
78 
79 protected:
80  ros::NodeHandle nh_;
81  moveit::planning_interface::MoveGroupInterfacePtr move_group_;
83 };
84 
86 {
87  // Create vector to hold 3 collision objects.
88  std::vector<moveit_msgs::CollisionObject> collision_objects;
89  collision_objects.resize(3);
90 
91  // Add the first table where the cube will originally be kept.
92  collision_objects[0].id = "table1";
93  collision_objects[0].header.frame_id = "panda_link0";
94 
95  /* Create identity rotation quaternion */
96  tf2::Quaternion zero_orientation;
97  zero_orientation.setRPY(0, 0, 0);
98  geometry_msgs::Quaternion zero_orientation_msg = tf2::toMsg(zero_orientation);
99 
100  /* Define the primitive and its dimensions. */
101  collision_objects[0].primitives.resize(1);
102  collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
103  collision_objects[0].primitives[0].dimensions.resize(3);
104  collision_objects[0].primitives[0].dimensions[0] = 0.2;
105  collision_objects[0].primitives[0].dimensions[1] = 0.4;
106  collision_objects[0].primitives[0].dimensions[2] = 0.4;
107 
108  /* Define the pose of the table. */
109  collision_objects[0].primitive_poses.resize(1);
110  collision_objects[0].primitive_poses[0].position.x = 0.5;
111  collision_objects[0].primitive_poses[0].position.y = 0;
112  collision_objects[0].primitive_poses[0].position.z = 0.2;
113  collision_objects[0].primitive_poses[0].orientation = zero_orientation_msg;
114 
115  collision_objects[0].operation = collision_objects[0].ADD;
116 
117  // Add the second table where we will be placing the cube.
118  collision_objects[1].id = "table2";
119  collision_objects[1].header.frame_id = "panda_link0";
120 
121  /* Define the primitive and its dimensions. */
122  collision_objects[1].primitives.resize(1);
123  collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
124  collision_objects[1].primitives[0].dimensions.resize(3);
125  collision_objects[1].primitives[0].dimensions[0] = 0.4;
126  collision_objects[1].primitives[0].dimensions[1] = 0.2;
127  collision_objects[1].primitives[0].dimensions[2] = 0.4;
128 
129  /* Define the pose of the table. */
130  collision_objects[1].primitive_poses.resize(1);
131  collision_objects[1].primitive_poses[0].position.x = 0;
132  collision_objects[1].primitive_poses[0].position.y = 0.5;
133  collision_objects[1].primitive_poses[0].position.z = 0.2;
134  collision_objects[1].primitive_poses[0].orientation = zero_orientation_msg;
135 
136  collision_objects[1].operation = collision_objects[1].ADD;
137 
138  // Define the object that we will be manipulating
139  collision_objects[2].header.frame_id = "panda_link0";
140  collision_objects[2].id = "object";
141 
142  /* Define the primitive and its dimensions. */
143  collision_objects[2].primitives.resize(1);
144  collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;
145  collision_objects[2].primitives[0].dimensions.resize(3);
146  collision_objects[2].primitives[0].dimensions[0] = 0.02;
147  collision_objects[2].primitives[0].dimensions[1] = 0.02;
148  collision_objects[2].primitives[0].dimensions[2] = 0.2;
149 
150  /* Define the pose of the object. */
151  collision_objects[2].primitive_poses.resize(1);
152  collision_objects[2].primitive_poses[0].position.x = 0.5;
153  collision_objects[2].primitive_poses[0].position.y = 0;
154  collision_objects[2].primitive_poses[0].position.z = 0.5;
155  collision_objects[2].primitive_poses[0].orientation = zero_orientation_msg;
156 
157  collision_objects[2].operation = collision_objects[2].ADD;
158 
159  planning_scene_interface_.applyCollisionObjects(collision_objects);
160 
161  // Create a vector of grasps to be attempted, currently only creating single grasp.
162  // This is essentially useful when using a grasp generator to generate and test multiple grasps.
163  std::vector<moveit_msgs::Grasp> grasps;
164  grasps.resize(1);
165 
166  // Setting grasp pose
167  // ++++++++++++++++++++++
168  // This is the pose of panda_link8. |br|
169  // Make sure that when you set the grasp_pose, you are setting it to be the pose of the last link in
170  // your manipulator which in this case would be `"panda_link8"` You will have to compensate for the
171  // transform from `"panda_link8"` to the palm of the end effector.
172  grasps[0].grasp_pose.header.frame_id = "panda_link0";
173  tf2::Quaternion grasp_orientation;
174  grasp_orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2);
175  grasps[0].grasp_pose.pose.orientation = tf2::toMsg(grasp_orientation);
176  grasps[0].grasp_pose.pose.position.x = 0.415;
177  grasps[0].grasp_pose.pose.position.y = 0;
178  grasps[0].grasp_pose.pose.position.z = 0.5;
179 
180  // Setting pre-grasp approach
181  // ++++++++++++++++++++++++++
182  /* Defined with respect to frame_id */
183  grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
184  /* Direction is set as positive x axis */
185  grasps[0].pre_grasp_approach.direction.vector.x = 1.0;
186  grasps[0].pre_grasp_approach.min_distance = 0.095;
187  grasps[0].pre_grasp_approach.desired_distance = 0.115;
188 
189  // Setting post-grasp retreat
190  // ++++++++++++++++++++++++++
191  /* Defined with respect to frame_id */
192  grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
193  /* Direction is set as positive z axis */
194  grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
195  grasps[0].post_grasp_retreat.min_distance = 0.1;
196  grasps[0].post_grasp_retreat.desired_distance = 0.25;
197 
198  // Setting posture of eef before grasp
199  // +++++++++++++++++++++++++++++++++++
200  /* Add both finger joints of panda robot. */
201  grasps[0].pre_grasp_posture.joint_names.resize(2);
202  grasps[0].pre_grasp_posture.joint_names[0] = "panda_finger_joint1";
203  grasps[0].pre_grasp_posture.joint_names[1] = "panda_finger_joint2";
204 
205  /* Set them as open, wide enough for the object to fit. */
206  grasps[0].pre_grasp_posture.points.resize(1);
207  grasps[0].pre_grasp_posture.points[0].positions.resize(2);
208  grasps[0].pre_grasp_posture.points[0].positions[0] = 0.04;
209  grasps[0].pre_grasp_posture.points[0].positions[1] = 0.04;
210  grasps[0].pre_grasp_posture.points[0].time_from_start = ros::Duration(0.5);
211 
212  // Setting posture of eef during grasp
213  // +++++++++++++++++++++++++++++++++++
214  /* Add both finger joints of panda robot and set them as closed. */
215  grasps[0].grasp_posture = grasps[0].pre_grasp_posture;
216  grasps[0].grasp_posture.points[0].positions[0] = 0.00;
217  grasps[0].grasp_posture.points[0].positions[1] = 0.00;
218 
219  // Set support surface as table1.
220  move_group_->setSupportSurfaceName("table1");
221  // Call pick to pick up the object using the grasps given
222  ASSERT_EQ(move_group_->pick("object", grasps), moveit::core::MoveItErrorCode::SUCCESS);
223 
224  // Ideally, you would create a vector of place locations to be attempted although in this example, we only create
225  // a single place location.
226  std::vector<moveit_msgs::PlaceLocation> place_location;
227  place_location.resize(1);
228 
229  // Setting place location pose
230  // +++++++++++++++++++++++++++
231  place_location[0].place_pose.header.frame_id = "panda_link0";
232  tf2::Quaternion place_orientation;
233  place_orientation.setRPY(0, 0, M_PI / 2);
234  place_location[0].place_pose.pose.orientation = tf2::toMsg(place_orientation);
235 
236  /* For place location, we set the value to the exact location of the center of the object. */
237  place_location[0].place_pose.pose.position.x = 0;
238  place_location[0].place_pose.pose.position.y = 0.5;
239  place_location[0].place_pose.pose.position.z = 0.5;
240 
241  // Setting pre-place approach
242  // ++++++++++++++++++++++++++
243  /* Defined with respect to frame_id */
244  place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
245  /* Direction is set as negative z axis */
246  place_location[0].pre_place_approach.direction.vector.z = -1.0;
247  place_location[0].pre_place_approach.min_distance = 0.095;
248  place_location[0].pre_place_approach.desired_distance = 0.115;
249 
250  // Setting post-grasp retreat
251  // ++++++++++++++++++++++++++
252  /* Defined with respect to frame_id */
253  place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
254  /* Direction is set as negative y axis */
255  place_location[0].post_place_retreat.direction.vector.y = -1.0;
256  place_location[0].post_place_retreat.min_distance = 0.1;
257  place_location[0].post_place_retreat.desired_distance = 0.25;
258 
259  // Setting posture of eef after placing object
260  // +++++++++++++++++++++++++++++++++++++++++++
261  /* Similar to the pick case */
262  /* Add both finger joints of panda robot. */
263  place_location[0].post_place_posture.joint_names.resize(2);
264  place_location[0].post_place_posture.joint_names[0] = "panda_finger_joint1";
265  place_location[0].post_place_posture.joint_names[1] = "panda_finger_joint2";
266 
267  /* Set them as open, wide enough for the object to fit. */
268  place_location[0].post_place_posture.points.resize(1);
269  place_location[0].post_place_posture.points[0].positions.resize(2);
270  place_location[0].post_place_posture.points[0].positions[0] = 0.04;
271  place_location[0].post_place_posture.points[0].positions[1] = 0.04;
272  place_location[0].post_place_posture.points[0].time_from_start = ros::Duration(0.5);
273 
274  // Set support surface as table2.
275  move_group_->setSupportSurfaceName("table2");
276  // Call place to place the object using the place locations given.
277  ASSERT_EQ(move_group_->place("object", place_location), moveit::core::MoveItErrorCode::SUCCESS);
278 }
279 
280 int main(int argc, char** argv)
281 {
282  ros::init(argc, argv, "move_group_pick_place_test");
283  testing::InitGoogleTest(&argc, argv);
284 
285  ros::AsyncSpinner spinner(1);
286  spinner.start();
287 
288  int result = RUN_ALL_TESTS();
289  return result;
290 }
moveit::planning_interface::PlanningSceneInterface planning_scene_interface_
moveit::planning_interface::MoveGroupInterfacePtr move_group_
constexpr double PLANNING_TIME_S
constexpr double MAX_ACCELERATION_SCALE
int main(int argc, char **argv)
TEST_F(PickPlaceTestFixture, PickPlaceTest)
constexpr double MAX_VELOCITY_SCALE