6 std::regex reg(
"\\s+");
7 std::vector<tf2::Transform> _map;
9 ROS_INFO(
"load [map] data...");
10 for (
int i = 0; i <
map.size();i++){
11 std::string str =
map[i];
12 std::sregex_token_iterator to_it(str.begin(), str.end(),reg,-1);
13 std::vector<std::string> temp{to_it, {}};
14 tf2::Vector3 t(std::stof(temp[0]), std::stof(temp[1]), std::stof(temp[2]));
15 tf2::Quaternion so(std::stof(temp[3]), std::stof(temp[4]), std::stof(temp[5]), std::stof(temp[6]));
17 _map.push_back(tf2::Transform(so.normalize(),t));
20 ROS_INFO(
"[map] saved with %li entries...", _map.size());
22 ROS_INFO(
"saving [target] positions...");
23 std::vector<std::vector<tf2::Transform>> task_;
24 for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = task.begin(); it != task.end(); ++it){
25 std::vector<tf2::Transform> task_per_robot;
26 for (
int i = 0; i < it->second.size(); i++){
27 std::string str = it->second[i];
28 std::sregex_token_iterator to_it(str.begin(), str.end(),reg,-1);
29 std::vector<std::string> temp{to_it, {}};
30 task_per_robot.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(std::stof(temp[0]), std::stof(temp[1]), std::stof(temp[2]))));
32 task_.push_back(task_per_robot);
36 for (
long unsigned int i = 0; i < task_.size(); i++) ROS_INFO(
"[target] for Robot %li saved with %li entries...", i, task_[i].size());
41 ROS_INFO(
"calculating target orientation basic set...");
42 std::vector<tf2::Quaternion> basic_rot;
43 tf2::Quaternion x_rot(0,0,0,1);
44 tf2::Quaternion y_rot(0,0,0.707,0.707);
45 tf2::Quaternion z_rot(0,-0.707,0,0.707);
46 basic_rot.push_back(x_rot.normalize());
47 basic_rot.push_back(x_rot.inverse().normalize());
48 basic_rot.push_back(y_rot.normalize());
49 basic_rot.push_back(y_rot.inverse().normalize());
50 basic_rot.push_back(z_rot.inverse().normalize());
52 std::vector<std::vector<std::vector<tf2::Quaternion>>> target_orientation_grasps;
53 for (
long unsigned int i = 0; i <
task_grasps_.size(); i++) {
54 std::vector<std::vector<tf2::Quaternion>> quat;
55 for (
long unsigned int j = 0; j <
task_grasps_[i].size(); j++) {
56 quat.push_back(basic_rot);
58 target_orientation_grasps.push_back(quat);
62 ROS_INFO(
"basic set registered...");
65 ROS_INFO(
"init voxel...");
67 std::vector<std::vector<std::vector<int>>> base_target_map;
69 for(
long unsigned int i = 0; i < base_target_map.size();i++) base_target_map[i].resize(voxelization.size());
71 ROS_INFO(
"forming base clouds...");
75 ROS_INFO(
"start cloud quantization...");
77 for(
long unsigned int j = 0; j <
target_cloud_[i].size();j++){
78 pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.2f);
80 octree.addPointsFromInputCloud();
81 double min_x, min_y, min_z, max_x, max_y, max_z;
82 octree.getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
83 for(
long unsigned int k = 0; k < voxelization.size(); k++) {
84 pcl::PointXYZ p = voxelization[k];
85 bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);
86 if(isInBox && octree.isVoxelOccupiedAtPoint(p)) {
87 std::vector< int > pointIdxVec;
88 if(octree.voxelSearch(p, pointIdxVec))
if(!pointIdxVec.empty()) base_target_map[i][k].push_back(j);
98 std::vector<std::vector<pcl::PointXYZ>> resulting;
99 for(
long unsigned int i = 0; i < base_target_map.size(); i++) {
100 std::vector<pcl::PointXYZ> points_per_robot;
101 for(
int j = 0; j < base_target_map[i].size(); j++){
102 if (base_target_map[i][j].size() ==
task_grasps_[i].size()) {
103 points_per_robot.push_back(voxelization[j]);
106 if (!points_per_robot.empty()) resulting.push_back(points_per_robot);
109 for (
long unsigned int i = 0; i < resulting.size(); i++) {
110 ROS_INFO(
"Robot %li got %li base positions to ckeck", i, resulting[i].size());
117 tf2::Vector3 origin(0,0,0);
118 float resolution = 0.4f;
119 float diameter = 3.0f;
120 unsigned char depth = 16;
121 std::vector<pcl::PointXYZ> box;
122 octomap::OcTree* tree =
new octomap::OcTree(resolution/2);
123 for (
float x = origin.getX() - diameter * 5 ; x <= origin.getX() + diameter * 5 ; x += resolution){
124 for (
float y = origin.getY() - diameter * 5 ; y <= origin.getY() + diameter * 5 ; y += resolution){
125 for (
float z = origin.getZ() - diameter * 1.5 ; z <= origin.getZ() + diameter * 1.5 ; z += resolution){
126 octomap::point3d point(x,y,z);
127 tree->updateNode(point,
true);
132 for (octomap::OcTree::leaf_iterator it = tree->begin_leafs(depth), end = tree->end_leafs(); it != end; ++it){
133 pcl::PointXYZ searchPoint(it.getCoordinate().x(), it.getCoordinate().y(), it.getCoordinate().z());
134 box.push_back(searchPoint);
142 std::ofstream o(ros::package::getPath(
"multi_cell_builder") +
"/mtc_task_file/dummy.yaml");
146 x = target_end.getOrigin().getX();
147 y = target_end.getOrigin().getY();
148 z = target_end.getOrigin().getZ();
150 std::stringstream ss;
151 ss <<
"hand_" << robot->
name().back();
152 std::string hand = ss.str();
153 std::stringstream sss;
154 sss <<
"panda_" << robot->
name().back() <<
"_link8";
155 std::string last_link = sss.str();
161 YAML::Node planner_node;
162 planner_node[
"id"] =
"cartesian";
163 planner_node[
"type"] =
"CartesianPath";
164 node[
"planners"].push_back(planner_node);
165 planner_node.reset();
167 planner_node[
"id"] =
"sampling";
168 planner_node[
"type"] =
"PipelinePlanner";
169 planner_node[
"properties"][
"step_size"] = 0.005f;
170 planner_node[
"properties"][
"goal_joint_tolerance"] =
static_cast<double>(0.00001f);
171 node[
"planners"].push_back(planner_node);
172 planner_node.reset();
174 planner_node[
"id"] =
"interpolation";
175 planner_node[
"type"] =
"JointInterpolationPlanner";
176 node[
"planners"].push_back(planner_node);
177 planner_node.reset();
180 node[
"task"][
"name"] =
"Pick and Place test";
181 node[
"task"][
"properties"][
"group"] = robot->
name();
182 node[
"task"][
"properties"][
"eef"] = hand;
183 node[
"task"][
"properties"][
"hand_grasping_frame"] = last_link;
184 node[
"task"][
"properties"][
"ik_frame"] = last_link;
185 node[
"task"][
"properties"][
"hand"] = hand;
188 stage[
"name"] =
"current";
189 stage[
"type"] =
"CurrentState";
190 node[
"task"][
"stages"].push_back(stage);
193 stage[
"name"] =
"move to ready";
194 stage[
"type"] =
"MoveTo";
195 stage[
"id"] =
"ready";
196 stage[
"planner"] =
"sampling";
197 stage[
"propertiesConfigureInitFrom"][
"source"] =
"PARENT";
198 stage[
"propertiesConfigureInitFrom"][
"values"].push_back(
"group");
199 stage[
"set"][
"goal"] =
"ready";
200 node[
"task"][
"stages"].push_back(stage);
203 stage[
"type"] =
"MoveTo";
204 stage[
"planner"] =
"sampling";
205 stage[
"id"] =
"hand_open";
206 stage[
"properties"][
"group"] = hand;
207 stage[
"set"][
"goal"] =
"open";
208 node[
"task"][
"stages"].push_back(stage);
211 stage[
"type"] =
"Connect";
212 stage[
"group_planner_vector"][robot->
name()] =
"sampling";
213 stage[
"propertiesConfigureInitFrom"][
"source"] =
"PARENT";
214 node[
"task"][
"stages"].push_back(stage);
217 stage[
"type"] =
"SerialContainer";
218 stage[
"name"] =
"grasp";
219 stage[
"properties_exposeTo"][
"source"] =
"task";
220 stage[
"properties_exposeTo"][
"values"] = YAML::Load(
"[eef, hand, group, ik_frame]");
221 stage[
"propertiesConfigureInitFrom"][
"source"] =
"PARENT";
222 stage[
"propertiesConfigureInitFrom"][
"values"] = YAML::Load(
"[eef, hand, group, ik_frame]");
225 YAML::Node stage_in_stage;
226 stage_in_stage[
"type"] =
"MoveRelative";
227 stage_in_stage[
"planner"] =
"cartesian";
228 stage_in_stage[
"properties"][
"link"] = last_link;
229 stage_in_stage[
"properties"][
"min_distance"] = 0.07f;
230 stage_in_stage[
"properties"][
"max_distance"] = 0.2f;
231 stage_in_stage[
"propertiesConfigureInitFrom"][
"source"] =
"PARENT";
232 stage_in_stage[
"propertiesConfigureInitFrom"][
"values"] = YAML::Load(
"[group]");
233 stage_in_stage[
"set"][
"direction"][
"vector"] = YAML::Load(
"{x: 0.0, y: 0.0, z: 1.0, header: { frame_id: " + last_link +
" }}");
234 stage[
"stages"].push_back(stage_in_stage);
235 stage_in_stage.reset();
237 stage_in_stage[
"type"] =
"ComputeIK";
238 stage_in_stage[
"properties"] = YAML::Load(
"{max_ik_solutions: 5}");
239 stage_in_stage[
"set"][
"ik_frame"][
"isometry"][
"translation"] = YAML::Load(
"{ x: 0.1, y: 0.0, z: 0.0 }");
240 stage_in_stage[
"set"][
"ik_frame"][
"isometry"][
"quaternion"] = YAML::Load(
"{ r: 1.571, p: 0.785, y: 1.571 }");
241 stage_in_stage[
"set"][
"ik_frame"][
"link"] = last_link;
243 YAML::Node properties_in_stage_in_stage;
244 properties_in_stage_in_stage[
"source"]=
"PARENT";
245 properties_in_stage_in_stage[
"values"]= YAML::Load(
"[eef, group]");
246 stage_in_stage[
"propertiesConfigureInitFrom"].push_back(properties_in_stage_in_stage);
247 properties_in_stage_in_stage.reset();
249 properties_in_stage_in_stage[
"source"]=
"INTERFACE";
250 properties_in_stage_in_stage[
"values"]= YAML::Load(
"[target_pose]");
251 stage_in_stage[
"propertiesConfigureInitFrom"].push_back(properties_in_stage_in_stage);
252 properties_in_stage_in_stage.reset();
254 stage_in_stage[
"stage"][
"type"] =
"GenerateGraspPose";
255 stage_in_stage[
"stage"][
"propertiesConfigureInitFrom"][
"source"] =
"PARENT";
256 stage_in_stage[
"stage"][
"properties"][
"object"] =
"bottle";
257 stage_in_stage[
"stage"][
"properties"][
"angle_delta"] = 1.571f;
258 stage_in_stage[
"stage"][
"properties"][
"pregrasp"] =
"open";
259 stage_in_stage[
"stage"][
"set"][
"monitored_stage"] =
"ready";
260 stage[
"stages"].push_back(stage_in_stage);
261 stage_in_stage.reset();
263 stage_in_stage[
"type"] =
"ModifyPlanningScene";
264 stage_in_stage[
"set"][
"allow_collisions"][
"first"] =
"bottle";
265 stage_in_stage[
"set"][
"allow_collisions"][
"second"][
"joint_model_group_name"] = hand;
266 stage_in_stage[
"set"][
"allow_collisions"][
"allow"] =
true;
267 stage[
"stages"].push_back(stage_in_stage);
268 stage_in_stage.reset();
270 stage_in_stage[
"type"] =
"MoveTo";
271 stage_in_stage[
"planner"] =
"sampling";
272 stage_in_stage[
"properties"][
"group"] = hand;
273 stage_in_stage[
"set"][
"goal"] =
"close";
274 stage[
"stages"].push_back(stage_in_stage);
275 stage_in_stage.reset();
277 stage_in_stage[
"type"] =
"ModifyPlanningScene";
278 stage_in_stage[
"set"][
"attach_object"][
"object"] =
"bottle";
279 stage_in_stage[
"set"][
"attach_object"][
"link"] = last_link;
280 stage[
"stages"].push_back(stage_in_stage);
281 stage_in_stage.reset();
283 stage_in_stage[
"type"] =
"MoveRelative";
284 stage_in_stage[
"planner"] =
"cartesian";
285 stage_in_stage[
"id"] =
"pick_up";
286 stage_in_stage[
"propertiesConfigureInitFrom"][
"source"] =
"PARENT";
287 stage_in_stage[
"propertiesConfigureInitFrom"][
"values"] = YAML::Load(
"[group]");
288 stage_in_stage[
"properties"][
"min_distance"] = 0.1f;
289 stage_in_stage[
"properties"][
"max_distance"] = 0.2f;
290 stage_in_stage[
"set"][
"ik_frame"][
"link"] = last_link;
291 stage_in_stage[
"set"][
"direction"][
"vector"] = YAML::Load(
"{ x: 0.0, y: 0.0, z: 1.0 }");
292 stage[
"stages"].push_back(stage_in_stage);
293 stage_in_stage.reset();
294 node[
"task"][
"stages"].push_back(stage);
298 stage[
"type"] =
"Connect";
299 stage[
"group_planner_vector"][robot->
name()] =
"sampling";
300 stage[
"propertiesConfigureInitFrom"][
"source"] =
"PARENT";
301 node[
"task"][
"stages"].push_back(stage);
305 stage[
"type"] =
"SerialContainer";
306 stage[
"name"] =
"place";
307 stage[
"properties_exposeTo"][
"source"] =
"task";
308 stage[
"properties_exposeTo"][
"values"] = YAML::Load(
"[eef, hand, group, ik_frame]");
309 stage[
"propertiesConfigureInitFrom"][
"source"] =
"PARENT";
311 stage_in_stage[
"type"] =
"MoveRelative";
312 stage_in_stage[
"planner"] =
"cartesian";
313 stage_in_stage[
"properties"][
"link"] = last_link;
314 stage_in_stage[
"properties"][
"min_distance"] = 0.1f;
315 stage_in_stage[
"properties"][
"max_distance"] = 0.2f;
316 stage_in_stage[
"propertiesConfigureInitFrom"][
"source"] =
"PARENT";
317 stage_in_stage[
"propertiesConfigureInitFrom"][
"values"] = YAML::Load(
"[group]");
318 stage_in_stage[
"set"][
"direction"][
"vector"] = YAML::Load(
"{ x: 0.0, y: 0.0, z: -1.0}");
319 stage[
"stages"].push_back(stage_in_stage);
320 stage_in_stage.reset();
322 stage_in_stage[
"type"] =
"ComputeIK";
323 stage_in_stage[
"properties"] = YAML::Load(
"{ max_ik_solutions: 5 }");
324 stage_in_stage[
"set"][
"ik_frame"][
"isometry"][
"translation"] = YAML::Load(
"{ x: 0.1, y: 0.0, z: 0.0 }");
325 stage_in_stage[
"set"][
"ik_frame"][
"isometry"][
"quaternion"] = YAML::Load(
"{ r: 1.571, p: 0.785, y: 1.571 }");
326 stage_in_stage[
"set"][
"ik_frame"][
"link"] = last_link;
328 properties_in_stage_in_stage[
"source"]=
"PARENT";
329 properties_in_stage_in_stage[
"values"]= YAML::Load(
"[eef, group]");
330 stage_in_stage[
"propertiesConfigureInitFrom"].push_back(properties_in_stage_in_stage);
331 properties_in_stage_in_stage.reset();
333 properties_in_stage_in_stage[
"source"]=
"INTERFACE";
334 properties_in_stage_in_stage[
"values"]= YAML::Load(
"[target_pose]");
335 stage_in_stage[
"propertiesConfigureInitFrom"].push_back(properties_in_stage_in_stage);
336 properties_in_stage_in_stage.reset();
338 stage_in_stage[
"stage"][
"type"] =
"GeneratePose";
339 stage_in_stage[
"stage"][
"set"][
"monitored_stage"] =
"pick_up";
340 stage_in_stage[
"stage"][
"set"][
"pose"][
"point"] = YAML::Load(
"{ x: " + std::to_string(x) +
", y: " + std::to_string(y) +
", z: 0.9305 }");
341 stage_in_stage[
"stage"][
"set"][
"pose"][
"orientation"] = YAML::Load(
"{ x: 0.0, y: 0.0, z: 0.0, w: 1.0}");
342 stage[
"stages"].push_back(stage_in_stage);
343 stage_in_stage.reset();
345 stage_in_stage[
"type"] =
"MoveTo";
346 stage_in_stage[
"planner"] =
"sampling";
347 stage_in_stage[
"properties"][
"group"] = hand;
348 stage_in_stage[
"set"][
"goal"] =
"open";
349 stage[
"stages"].push_back(stage_in_stage);
350 stage_in_stage.reset();
352 stage_in_stage[
"type"] =
"ModifyPlanningScene";
353 stage_in_stage[
"set"][
"detach_object"][
"object"]=
"bottle";
354 stage_in_stage[
"set"][
"detach_object"][
"link"]= last_link;
355 stage_in_stage[
"set"][
"allow_collisions"][
"first"]=
"bottle";
356 stage_in_stage[
"set"][
"allow_collisions"][
"second"][
"joint_model_group_name"]= hand;
357 stage_in_stage[
"set"][
"allow_collisions"][
"allow"] =
false;
358 stage[
"stages"].push_back(stage_in_stage);
359 stage_in_stage.reset();
361 stage_in_stage[
"type"] =
"MoveRelative";
362 stage_in_stage[
"planner"] =
"cartesian";
363 stage_in_stage[
"properties"][
"link"] = last_link;
364 stage_in_stage[
"properties"][
"min_distance"] = 0.07f;
365 stage_in_stage[
"properties"][
"max_distance"] = 0.2f;
366 stage_in_stage[
"propertiesConfigureInitFrom"][
"source"] =
"PARENT";
367 stage_in_stage[
"propertiesConfigureInitFrom"][
"values"] = YAML::Load(
"[group]");
368 stage_in_stage[
"set"][
"direction"][
"vector"] = YAML::Load(
"{x: 0.0, y: 0.0, z: -1.0, header: { frame_id: " + last_link+
" }}");
369 stage[
"stages"].push_back(stage_in_stage);
370 stage_in_stage.reset();
372 stage_in_stage[
"type"] =
"MoveTo";
373 stage_in_stage[
"planner"] =
"sampling";
374 stage_in_stage[
"properties"][
"group"] = hand;
375 stage_in_stage[
"set"][
"goal"] =
"close";
376 stage[
"stages"].push_back(stage_in_stage);
377 stage_in_stage.reset();
379 stage_in_stage[
"name"] =
"move to ready";
380 stage_in_stage[
"type"] =
"MoveTo";
381 stage_in_stage[
"planner"] =
"sampling";
382 stage_in_stage[
"propertiesConfigureInitFrom"][
"source"]=
"PARENT";
383 stage_in_stage[
"propertiesConfigureInitFrom"][
"values"].push_back(
"group");
384 stage_in_stage[
"set"][
"goal"] =
"ready";
385 stage[
"stages"].push_back(stage_in_stage);
386 stage_in_stage.reset();
387 node[
"task"][
"stages"].push_back(stage);
390 node[
"max_planning_solutions"] = 10;