/
pick_and_place_opencv.cpp
222 lines (154 loc) · 9.99 KB
/
pick_and_place_opencv.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include "opencv_services/box_and_target_position.h"
int main(int argc, char** argv)
{
ros::init(argc, argv, "pick_and_place_opencv");
ros::NodeHandle n;
// ROS spinning must be running for the MoveGroupInterface to get information
// about the robot's state. One way to do this is to start an AsyncSpinner
// beforehand.
ros::AsyncSpinner spinner(1);
spinner.start();
// MoveIt operates on sets of joints called "planning groups" and stores them in an object called
// the `JointModelGroup`. Throughout MoveIt the terms "planning group" and "joint model group"
// are used interchangably.
static const std::string PLANNING_GROUP_ARM = "ur5_arm";
static const std::string PLANNING_GROUP_GRIPPER = "gripper";
// The :planning_interface:`MoveGroupInterface` class can be easily
// setup using just the name of the planning group you would like to control and plan for.
moveit::planning_interface::MoveGroupInterface move_group_interface_arm(PLANNING_GROUP_ARM);
moveit::planning_interface::MoveGroupInterface move_group_interface_gripper(PLANNING_GROUP_GRIPPER);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
robot_model_loader::RobotModelLoaderPtr robot_model_loader(new robot_model_loader::RobotModelLoader("robot_description"));
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(new planning_scene_monitor::PlanningSceneMonitor(robot_model_loader));
// Allow collisions between the gripper and the box to be able to grasp it
planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor);
collision_detection::AllowedCollisionMatrix& acm = ls->getAllowedCollisionMatrixNonConst();
acm.setEntry("blue_box", "robotiq_85_left_finger_tip_link", true);
acm.setEntry("blue_box", "robotiq_85_right_finger_tip_link", true);
acm.setEntry("blue_box", "robotiq_85_right_inner_knuckle_link", true);
acm.setEntry("blue_box","robotiq_85_left_inner_knuckle_link", true);
acm.setEntry("blue_box","robotiq_85_base_link", true);
acm.setEntry("blue_box", "robotiq_85_left_finger_link", true);
acm.setEntry("blue_box", "robotiq_85_right_finger_link", true);
acm.setEntry("blue_box", "robotiq_85_left_knuckle_link", true);
acm.setEntry("blue_box", "robotiq_85_right_knuckle_link", true);
std::cout << "\nAllowedCollisionMatrix:\n";
acm.print(std::cout);
moveit_msgs::PlanningScene diff_scene;
ls->getPlanningSceneDiffMsg(diff_scene);
planning_scene_interface.applyPlanningScene(diff_scene);
ros::Duration(0.1).sleep();
// We can get a list of all the groups in the robot:
ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
std::copy(move_group_interface_arm.getJointModelGroupNames().begin(),
move_group_interface_arm.getJointModelGroupNames().end(), std::ostream_iterator<std::string>(std::cout, ", "));
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
// 1. Move to home position
move_group_interface_arm.setJointValueTarget(move_group_interface_arm.getNamedTargetValues("home"));
bool success = (move_group_interface_arm.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
move_group_interface_arm.move();
// Get the box and the target position from the opencv node
ros::ServiceClient box_and_target_position_srv_client = n.serviceClient<opencv_services::box_and_target_position>("box_and_target_position");
opencv_services::box_and_target_position srv;
if(box_and_target_position_srv_client.call(srv)) {
ROS_INFO_STREAM("3d target position camera frame: x " << srv.response.target_position.x << " y " << srv.response.target_position.y << " z " << srv.response.target_position.z);
ROS_INFO_STREAM("3d box position camera frame: x " << srv.response.box_position.x << " y " << srv.response.box_position.y << " z " << srv.response.box_position.z);
} else {
ROS_INFO_STREAM("Failed to call box and target position service");
}
// Add the object to be grasped (the suqare box) to the planning scene
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group_interface_arm.getPlanningFrame();
collision_object.id = "blue_box";
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.06;
primitive.dimensions[1] = 0.06;
primitive.dimensions[2] = 0.06;
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = srv.response.box_position.x;
box_pose.position.y = srv.response.box_position.y;
box_pose.position.z = srv.response.box_position.z;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
planning_scene_interface.applyCollisionObjects(collision_objects);
ROS_INFO_NAMED("tutorial", "Add an object into the world");
ros::Duration(0.1).sleep();
// 2. Place the TCP (Tool Center Point, the tip of the robot) above the blue box
ROS_INFO_NAMED("tutorial", "// 2. Place the TCP (Tool Center Point, the tip of the robot) above the blue box");
geometry_msgs::PoseStamped current_pose;
current_pose = move_group_interface_arm.getCurrentPose("ee_link");
geometry_msgs::Pose target_pose1;
target_pose1.orientation = current_pose.pose.orientation;
target_pose1.position.x = srv.response.box_position.x;
target_pose1.position.y = srv.response.box_position.y;
target_pose1.position.z = srv.response.box_position.z + 0.34;
move_group_interface_arm.setPoseTarget(target_pose1);
success = (move_group_interface_arm.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Goal position x:%f y:%f z:%f", target_pose1.position.x, target_pose1.position.y, target_pose1.position.z);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
move_group_interface_arm.move();
moveit::planning_interface::MoveGroupInterface::Plan my_plan_gripper;
// 3. Open the gripper
ROS_INFO_NAMED("tutorial", "// 3. Open the gripper");
move_group_interface_gripper.setJointValueTarget(move_group_interface_gripper.getNamedTargetValues("open"));
success = (move_group_interface_gripper.plan(my_plan_gripper) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
move_group_interface_gripper.move();
// 4. Move the TCP close to the object
ROS_INFO_NAMED("tutorial", "// 4. Move the TCP close to the object");
target_pose1.position.z = target_pose1.position.z - 0.2;
move_group_interface_arm.setPoseTarget(target_pose1);
success = (move_group_interface_arm.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Goal position x:%f y:%f z:%f", target_pose1.position.x, target_pose1.position.y, target_pose1.position.z);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
move_group_interface_arm.move();
// 5. Close the gripper
move_group_interface_gripper.setJointValueTarget(move_group_interface_gripper.getNamedTargetValues("closed"));
success = (move_group_interface_gripper.plan(my_plan_gripper) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
move_group_interface_gripper.move();
// Attach the box to the gripper after it was grasped
moveit_msgs::AttachedCollisionObject aco;
aco.object.id = collision_object.id;
aco.link_name = "robotiq_85_right_finger_tip_link";
aco.touch_links.push_back("robotiq_85_left_finger_tip_link");
aco.object.operation = moveit_msgs::CollisionObject::ADD;
planning_scene_interface.applyAttachedCollisionObject(aco);
// 6. Move the TCP above the plate
target_pose1.position.z = srv.response.target_position.z + 0.5;
target_pose1.position.x = srv.response.target_position.x;
target_pose1.position.y = srv.response.target_position.y;
move_group_interface_arm.setPoseTarget(target_pose1);
success = (move_group_interface_arm.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
move_group_interface_arm.move();
// 7. Lower the TCP above the plate
target_pose1.position.z = target_pose1.position.z - 0.14;
move_group_interface_arm.setPoseTarget(target_pose1);
success = (move_group_interface_arm.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
move_group_interface_arm.move();
// 8. Open the gripper
move_group_interface_gripper.setJointValueTarget(move_group_interface_gripper.getNamedTargetValues("open"));
success = (move_group_interface_gripper.plan(my_plan_gripper) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
move_group_interface_gripper.move();
ROS_INFO_NAMED("tutorial", "Remove the object from the world");
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);
planning_scene_interface.removeCollisionObjects(object_ids);
ros::shutdown();
return 0;
}