moveit_grasp_mediator.cpp
Go to the documentation of this file.
2 #include <regex>
3 #include <gb_grasp/MapConfigLoader.h>
4 #include <moveit/robot_state/conversions.h>
6 
7 
9  robots_.push_back(robot);
10  ROS_INFO("%s connected...", robot->name().c_str());
11  auto mr = dynamic_cast<Moveit_robot*>(robot);
12 
14 
15  auto map_gen = std::make_shared<MapGenerator>(*nh_, visual_tools_, planning_scene_monitor_, mr->map().at("eef_name"), mr->name().c_str(), robot_model_->getJointModelGroup(mr->name().c_str()), mgi().get()->getCurrentState(), voxel_manager_);
16  double min_quality, max_candidates;
17  bool top_only;
18 
19  rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"min_quality",min_quality);
20  rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"max_candidate_count",max_candidates);
21  rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"top_grasps_only",top_only);
22 
23  ROS_INFO("min_quality: %f", min_quality);
24  ROS_INFO("max_candidate_count: %f", max_candidates);
25  ROS_INFO("top_grasps_only: %f", top_only);
26 
27 
28  map_gen->SetQuality(min_quality);
29  map_gen->SetMaxGrasp(max_candidates);
30  map_gen->SetZGraspOnly(top_only);
31  mr->set_grasp_map_generator(map_gen);
32 
33 }
34 
35 void Moveit_grasp_mediator::manipulate_mapspace(tf2::Transform& tf, tf2::Vector3& size){
36  double r,p,y;
37  std::vector<double> rot_mapspace(3,0);
38 
39  tf2::Matrix3x3 m(tf.getRotation());
40  m.getRPY(r,p, y);
41  nh_->setParam("/mapspace/rot", std::vector<double>({r,p,y}));
42  nh_->setParam("/mapspace/pos", std::vector<double>({tf.getOrigin().getX(), tf.getOrigin().getY(), 0.91f}));
43  nh_->setParam("/mapspace/dim", std::vector<double>({size.getX(), size.getY(), size.getZ()}));
44  //nh_->setParam("/voxel_space/dim", std::vector<float>({0.3f,0.3f,0.2f}));
45  //nh_->setParam("/voxel_space/pos", std::vector<double>({tf.getOrigin().getX(), tf.getOrigin().getY(), tf.getOrigin().getZ()}));
46  //nh_->setParam("/voxel_space/rot", std::vector<double>({r,p,y}));
47 }
48 
50  XmlRpc::XmlRpcValue data;
51  nh_->getParam("/", data);
52 
53  std::regex rx("panda_arm([0-9]+)");
54  std::smatch match;
55  std::regex_match(robot->name(), match, rx);
56  data["base_link"]= "world";
57 
58  data[robot->map().at("eef_name").c_str()]["end_effector_name"] = robot->map().at("eef_name").c_str();
59  data[robot->map().at("eef_name").c_str()]["end_effector_type"] = "finger";
60 
61  data[robot->map().at("eef_name").c_str()]["grasp_resolution"] = 0.001;
62  data[robot->map().at("eef_name").c_str()]["angle_resolution"] = 45;
63 
64  data[robot->map().at("eef_name").c_str()]["grasp_max_depth"] = 0.035;
65  data[robot->map().at("eef_name").c_str()]["grasp_min_depth"] = 0.01;
66 
67  data[robot->map().at("eef_name").c_str()]["grasp_depth_resolution"] = 0.005;
68 
69  data[robot->map().at("eef_name").c_str()]["approach_distance_desired"] = 0.05;
70  data[robot->map().at("eef_name").c_str()]["retreat_distance_desired"] = 0.05;
71  data[robot->map().at("eef_name").c_str()]["lift_distance_desired"] = 0.02;
72 
73  data[robot->map().at("eef_name").c_str()]["grasp_padding_on_approach"] = 0.005;
74 
75  std::stringstream ss;
76  ss << "panda_" << match[1] << "_tool_center_point";
77  data[robot->map().at("eef_name").c_str()]["define_tcp_by_name"] = false;
78  data[robot->map().at("eef_name").c_str()]["tcp_name"] = ss.str().c_str();
79  data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][0] = 0;
80  data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][1] = 0;
81  data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][2] = -0.105;
82  data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][3] = 0;
83  data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][4] = 0;
84  data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][5] = -0.7853;
85 
86  data[robot->map().at("eef_name").c_str()]["pregrasp_time_from_start"] = 0;
87  data[robot->map().at("eef_name").c_str()]["grasp_time_from_start"] = 0;
88 
89  data[robot->map().at("eef_name").c_str()]["pregrasp_posture"][0] = 0.04;
90  data[robot->map().at("eef_name").c_str()]["grasp_posture"][0] = 0;
91  data[robot->map().at("eef_name").c_str()]["joints"][0] = robot->mgi_hand()->getJoints()[1].c_str();
92 
93  data[robot->map().at("eef_name").c_str()]["max_finger_width"] = 0.085;
94  data[robot->map().at("eef_name").c_str()]["min_finger_width"] = 0.06;
95 
96  data[robot->map().at("eef_name").c_str()]["max_grasp_width"] = 0.08;
97 
98  data[robot->map().at("eef_name").c_str()]["gripper_finger_width"] = 0.015;
99 
100  data[robot->map().at("eef_name").c_str()]["active_suction_range_x"] = 0.022;
101  data[robot->map().at("eef_name").c_str()]["active_suction_range_y"] = 0.012;
102 
103  data[robot->map().at("eef_name").c_str()]["suction_rows_count"] = 2;
104  data[robot->map().at("eef_name").c_str()]["suction_cols_count"] = 2;
105  nh_->setParam("/", data);
106 }
107 
108 bool Moveit_grasp_mediator::generateRandomCuboid(std::string& object_name, geometry_msgs::Pose& object_pose, double& x_depth,
109  double& y_width, double& z_height)
110  {
111  // Generate random cuboid
112  double xmin = 0.1;
113  double xmax = 0.7;
114  double ymin = -0.25;
115  double ymax = 0.25;
116  double zmin = 0.89;
117  double zmax = 1.2;
118  rviz_visual_tools::RandomPoseBounds pose_bounds(xmin, xmax, ymin, ymax, zmin, zmax);
119 
120  double cuboid_size_min = 0.01;
121  double cuboid_size_max = 0.03;
122  rviz_visual_tools::RandomCuboidBounds cuboid_bounds(cuboid_size_min, cuboid_size_max);
123 
124  object_name = "pick_target";
125  visual_tools_->generateRandomCuboid(object_pose, x_depth, y_width, z_height, pose_bounds, cuboid_bounds);
126  visual_tools_->publishCollisionCuboid(object_pose, x_depth, y_width, z_height, object_name, rviz_visual_tools::RED);
127  visual_tools_->publishAxis(object_pose, rviz_visual_tools::MEDIUM);
128  visual_tools_->trigger();
129 
130  bool success = true;
131  double timeout = 5; // seconds
132  ros::Rate rate(100);
133  while (success && !planning_scene_monitor_->getPlanningScene()->knowsFrameTransform(object_name))
134  {
135  rate.sleep();
136  success = rate.cycleTime().toSec() < timeout;
137  }
138  return success;
139 }
140 
142  visual_tools_->deleteAllMarkers();
143 
144  ROS_INFO("ns is %s", grasp_ns_.c_str());
145  MapConfigLoader map_config(*nh_, grasp_ns_.c_str());
146  auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
147  map_config.SetObjects(cbd);
148  float object_offset = 0.02f;
149 
150  tf2::Vector3 middle_pt(0,0,0);
151  std::vector<double> x;
152  std::vector<double> y;
153  for(auto* ar: robots_){
154  Moveit_robot* r = dynamic_cast<Moveit_robot*>(ar);
155  tf2::Vector3 pos = r->tf().getOrigin();
156  pos.setZ((pos.getZ()*2) +object_offset);
157  tf2::Transform man_tf(r->tf().getRotation(), pos);
158  manipulate_mapspace(man_tf, r->size());
159  map_config.Create(grasp_maps_);
160  middle_pt+= pos;
161  ROS_INFO("X: %f, Y: %f, Z: %f", pos.getX(), pos.getY(), pos.getZ());
162 
163  tf2::Transform A = r->tf() * r->bounds()[0];
164  tf2::Transform B = r->tf() * r->bounds()[1];
165  tf2::Transform C = r->tf() * r->bounds()[2];
166  tf2::Transform D = r->tf() * r->bounds()[3];
167  x.push_back(A.getOrigin().getX());
168  x.push_back(B.getOrigin().getX());
169  x.push_back(C.getOrigin().getX());
170  x.push_back(D.getOrigin().getX());
171  y.push_back(A.getOrigin().getY());
172  y.push_back(B.getOrigin().getY());
173  y.push_back(C.getOrigin().getY());
174  y.push_back(D.getOrigin().getY());
175 
176  r->grasp_map_generator()->SampleMap(grasp_maps_.front());
177  for (auto* are: r->observers()){
178  auto wmd = dynamic_cast<Wing_moveit_decorator*>(are);
179  auto w = dynamic_cast<Wing*>(wmd->wing());
180  pos.setX(w->world_tf().getOrigin().getX());
181  pos.setY(w->world_tf().getOrigin().getY());
182  man_tf.setRotation(w->world_tf().getRotation());
183  man_tf.setOrigin(pos);
184  manipulate_mapspace(man_tf, w->size());
185  map_config.Create(grasp_maps_);
186  middle_pt+= pos;
187  ROS_INFO("X: %f, Y: %f, Z: %f", pos.getX(), pos.getY(), pos.getZ());
188 
189  A = w->world_tf() * w->bounds()[0];
190  B = w->world_tf() * w->bounds()[1];
191  C = w->world_tf() * w->bounds()[2];
192  D = w->world_tf() * w->bounds()[3];
193  x.push_back(A.getOrigin().getX());
194  x.push_back(B.getOrigin().getX());
195  x.push_back(C.getOrigin().getX());
196  x.push_back(D.getOrigin().getX());
197  y.push_back(A.getOrigin().getY());
198  y.push_back(B.getOrigin().getY());
199  y.push_back(C.getOrigin().getY());
200  y.push_back(D.getOrigin().getY());
201  r->grasp_map_generator()->SampleMap(grasp_maps_.front());
202  }
203  }
204  ROS_INFO_STREAM("number of maps: "<< grasp_maps_.size());
205  middle_pt/= grasp_maps_.size();
206  ROS_INFO("X: %f, Y: %f, Z: %f", middle_pt.getX(), middle_pt.getY(), middle_pt.getZ());
207  auto x_min = *std::min_element(x.begin(),x.end());
208  auto x_max = *std::max_element(x.begin(),x.end());
209  auto y_min = *std::min_element(y.begin(),y.end());
210  auto y_max = *std::max_element(y.begin(),y.end());
211  ROS_INFO("width: %f", x_max-x_min);
212  ROS_INFO("height: %f", y_max-y_min);
213 
214  visual_tools_->setAlpha(0.1);
215  voxel_manager_->Voxelize();
216 
217  auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();
218  for(auto&o:cod){
219  voxel_manager_->GetCuboidCollisions(o,voxel_environment_);
220  }
221  voxel_manager_->Voxelize(true);
222 
223 
224  // -----------------------------------
225  // Generate random object to grasp
226  geometry_msgs::Pose object_pose;
227  double object_x_depth;
228  double object_y_width;
229  double object_z_height;
230  std::string object_name;
231  if (!generateRandomCuboid(object_name, object_pose, object_x_depth, object_y_width, object_z_height))
232  {
233  ROS_INFO("Failed to add random cuboid ot planning scene");
234  }
235 
236  // -----------------------------------
237  // Generate grasp candidates
238  std::vector<moveit_grasps::GraspCandidatePtr> grasp_candidates;
239 
240  // Configure the desired types of grasps
241  moveit_grasps::TwoFingerGraspCandidateConfig grasp_generator_config =
242  moveit_grasps::TwoFingerGraspCandidateConfig();
243  grasp_generator_config.disableAll();
244  grasp_generator_config.enable_face_grasps_ = true;
245  grasp_generator_config.generate_y_axis_grasps_ = true;
246  grasp_generator_config.generate_x_axis_grasps_ = true;
247  grasp_generator_config.generate_z_axis_grasps_ = true;
248 
249  auto grasp_generator_ =std::make_shared<moveit_grasps::TwoFingerGraspGenerator>(visual_tools_);
250 
251  // Set the ideal grasp orientation for scoring
252  std::vector<double> ideal_grasp_rpy = { 3.14, 0.0, 0.0 };
253  grasp_generator_->setIdealTCPGraspPoseRPY(ideal_grasp_rpy);
254 
255  // Set custom grasp score weights
256  auto grasp_score_weights = std::make_shared<moveit_grasps::TwoFingerGraspScoreWeights>();
257  grasp_score_weights->orientation_x_score_weight_ = 2.0;
258  grasp_score_weights->orientation_y_score_weight_ = 2.0;
259  grasp_score_weights->orientation_z_score_weight_ = 2.0;
260  grasp_score_weights->translation_x_score_weight_ = 1.0;
261  grasp_score_weights->translation_y_score_weight_ = 1.0;
262  grasp_score_weights->translation_z_score_weight_ = 1.0;
263  // Finger gripper specific weights.
264  grasp_score_weights->depth_score_weight_ = 2.0;
265  grasp_score_weights->width_score_weight_ = 2.0;
266  // Assign the grasp score weights in the grasp_generator
267  grasp_generator_->setGraspScoreWeights(grasp_score_weights);
268  auto grasp_data_ = std::make_shared<moveit_grasps::TwoFingerGraspData>(*nh_, "hand_1", visual_tools_->getRobotModel());
269  if (!grasp_data_->loadGraspData(*nh_, "hand_1"))
270  {
271  ROS_INFO("Failed to load Grasp Data parameters.");
272  exit(-1);
273  }
274 
275  grasp_generator_->setGraspCandidateConfig(grasp_generator_config);
276  if (!grasp_generator_->generateGrasps(visual_tools_->convertPose(object_pose), object_x_depth, object_y_width,
277  object_z_height, grasp_data_, grasp_candidates))
278  {
279  ROS_INFO("Grasp generator failed to generate any valid grasps");
280  }
281 
282  // --------------------------------------------
283  // Generating a seed state for filtering grasps
284  auto seed_state = std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
285  Eigen::Isometry3d eef_mount_grasp_pose =
286  visual_tools_->convertPose(object_pose) * grasp_data_->tcp_to_eef_mount_.inverse();
287  if (!getIKSolution(robot_model_->getJointModelGroup("panda_arm1"), eef_mount_grasp_pose, *seed_state, grasp_data_->parent_link_->getName()))
288  {
289  ROS_INFO("The ideal seed state is not reachable. Using start state as seed.");
290  }
291 
292  // --------------------------------------------
293  // Filtering grasps
294  // Note: This step also solves for the grasp and pre-grasp states and stores them in grasp candidates)
295  bool filter_pregrasps = true;
296  auto grasp_filter_ = std::make_shared<moveit_grasps::TwoFingerGraspFilter>(visual_tools_->getSharedRobotState(), visual_tools_);
297  if (!grasp_filter_->filterGrasps(grasp_candidates, planning_scene_monitor_, robot_model_->getJointModelGroup("panda_arm1"), seed_state, filter_pregrasps,
298  object_name))
299  {
300  ROS_INFO("Filter grasps failed");
301  }
302  if (!grasp_filter_->removeInvalidAndFilter(grasp_candidates))
303  {
304  ROS_INFO("Grasp filtering removed all grasps");
305  }
306 
307  // Plan free-space approach, cartesian approach, lift and retreat trajectories
308  moveit_grasps::GraspCandidatePtr selected_grasp_candidate;
309  moveit_msgs::MotionPlanResponse pre_approach_plan;
310  if (!planFullGrasp(grasp_candidates, selected_grasp_candidate, pre_approach_plan, object_name))
311  {
312  ROS_INFO("Failed to plan grasp motions");
313  }
314 
315  visualizePick(selected_grasp_candidate, pre_approach_plan);
316 }
317 
318  void Moveit_grasp_mediator::visualizePick(const moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
319  const moveit_msgs::MotionPlanResponse& pre_approach_plan)
320  {
321  EigenSTL::vector_Isometry3d waypoints;
322  moveit_grasps::GraspGenerator::getGraspWaypoints(valid_grasp_candidate, waypoints);
323 
324  // Visualize waypoints
325  visual_tools_->publishAxisLabeled(waypoints[0], "pregrasp");
326  visual_tools_->publishAxisLabeled(waypoints[1], "grasp");
327  visual_tools_->publishAxisLabeled(waypoints[2], "lifted");
328  visual_tools_->publishAxisLabeled(waypoints[3], "retreat");
329  visual_tools_->trigger();
330 
331  // Get the pre and post grasp states
332  visual_tools_->prompt("pre_grasp");
333  robot_state::RobotStatePtr pre_grasp_state =
334  std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
335  valid_grasp_candidate->getPreGraspState(pre_grasp_state);
336  visual_tools_->publishRobotState(pre_grasp_state, rviz_visual_tools::ORANGE);
337  robot_state::RobotStatePtr grasp_state =
338  std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
339  if (valid_grasp_candidate->getGraspStateClosed(grasp_state))
340  {
341  visual_tools_->prompt("grasp");
342  visual_tools_->publishRobotState(grasp_state, rviz_visual_tools::YELLOW);
343  }
344  if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 1 &&
345  valid_grasp_candidate->segmented_cartesian_traj_[1].size())
346  {
347  visual_tools_->prompt("lift");
348  visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[1].back(),
349  rviz_visual_tools::BLUE);
350  }
351  if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 2 &&
352  valid_grasp_candidate->segmented_cartesian_traj_[2].size())
353  {
354  visual_tools_->prompt("retreat");
355  visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[2].back(),
356  rviz_visual_tools::PURPLE);
357  }
358 
359  visual_tools_->prompt("show free space approach");
360  visual_tools_->hideRobot();
361  visual_tools_->trigger();
362 
363  bool wait_for_animation = true;
364  visual_tools_->publishTrajectoryPath(pre_approach_plan.trajectory, pre_grasp_state, wait_for_animation);
365  ros::Duration(0.25).sleep();
366  if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::APPROACH)
367  visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH],
368  valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
369  ros::Duration(0.25).sleep();
370 
371  if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::LIFT)
372  visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::LIFT],
373  valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
374  ros::Duration(0.25).sleep();
375 
376  if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::RETREAT)
377  visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::RETREAT],
378  valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
379  ros::Duration(0.25).sleep();
380 }
381 
382 bool Moveit_grasp_mediator::planFullGrasp(std::vector<moveit_grasps::GraspCandidatePtr>& grasp_candidates,
383  moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
384  moveit_msgs::MotionPlanResponse& pre_approach_plan, const std::string& object_name)
385  {
386  moveit::core::RobotStatePtr current_state;
387  {
388  boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
389  new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));
390  current_state = std::make_shared<robot_state::RobotState>((*ls)->getCurrentState());
391  }
392 
393  std::shared_ptr<moveit_grasps::GraspPlanner>grasp_planner_ = std::make_shared<moveit_grasps::GraspPlanner>(visual_tools_);
394 
395  // MoveIt Grasps allows for a manual breakpoint debugging tool to be optionally passed in
396  grasp_planner_->setWaitForNextStepCallback(boost::bind(&Moveit_grasp_mediator::waitForNextStep, this, visual_tools_, _1));
397 
398  bool success = false;
399  for (; !grasp_candidates.empty(); grasp_candidates.erase(grasp_candidates.begin()))
400  {
401  valid_grasp_candidate = grasp_candidates.front();
402  valid_grasp_candidate->getPreGraspState(current_state);
403  if (!grasp_planner_->planApproachLiftRetreat(valid_grasp_candidate, current_state, planning_scene_monitor_, false,
404  object_name))
405  {
406  ROS_INFO("failed to plan approach lift retreat");
407  continue;
408  }
409 
410  robot_state::RobotStatePtr pre_grasp_state =
411  valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH].front();
412  if (!planPreApproach(*pre_grasp_state, pre_approach_plan))
413  {
414  ROS_INFO("failed to plan to pregrasp_state");
415  continue;
416  }
417 
418  success = true;
419  break;
420  }
421  return success;
422 }
423 
424 void Moveit_grasp_mediator::waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt)
425 {
426  visual_tools->prompt(prompt);
427 }
428 
429 bool Moveit_grasp_mediator::planPreApproach(const robot_state::RobotState& goal_state, moveit_msgs::MotionPlanResponse& pre_approach_plan)
430  {
431  planning_interface::MotionPlanRequest req;
432  planning_interface::MotionPlanResponse res;
433 
434  double tolerance_above = 0.01;
435  double tolerance_below = 0.01;
436  // req.planner_id = "RRTConnectkConfigDefault";
437  req.group_name = "panda_1_arm1";
438  req.num_planning_attempts = 5;
439  req.allowed_planning_time = 1.5;
440  moveit_msgs::Constraints goal =
441  kinematic_constraints::constructGoalConstraints(goal_state, robot_model_->getJointModelGroup("panda_arm1"), tolerance_below, tolerance_above);
442 
443  req.goal_constraints.push_back(goal);
444  boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
445  new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));
446 
447  // ---------------------------------
448  // Change the robot current state
449  // NOTE: We have to do this since Panda start configuration is in self collision.
450  robot_state::RobotState rs = (*ls)->getCurrentState();
451  std::vector<double> starting_joint_values = { 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 };
452  std::vector<std::string> joint_names = { "panda_1_joint1", "panda_1_joint2", "panda_1_joint3", "panda_1_joint4",
453  "panda_1_joint5", "panda_1_joint6", "panda_1_joint7" };
454  // arm_jmg_->getActiveJointModelNames();
455  for (std::size_t i = 0; i < joint_names.size(); ++i)
456  {
457  rs.setJointPositions(joint_names[i], &starting_joint_values[i]);
458  }
459  rs.update();
460  robot_state::robotStateToRobotStateMsg(rs, req.start_state);
461  // ---------------------------
462 
463  planning_pipeline_->generatePlan(*ls, req, res);
464  if (res.error_code_.val != res.error_code_.SUCCESS)
465  {
466  ROS_INFO( "Failed to plan approach successfully");
467  return false;
468  }
469 
470  res.getMessage(pre_approach_plan);
471  return true;
472 }
473 
474 bool Moveit_grasp_mediator::isStateValid(const planning_scene::PlanningScene* planning_scene,
475  const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, robot_state::RobotState* robot_state,
476  const robot_model::JointModelGroup* group, const double* ik_solution)
477 {
478  robot_state->setJointGroupPositions(group, ik_solution);
479  robot_state->update();
480  return !planning_scene->isStateColliding(*robot_state, group->getName());
481 }
482 
483 bool Moveit_grasp_mediator::getIKSolution(const moveit::core::JointModelGroup* arm_jmg, const Eigen::Isometry3d& target_pose,
484  robot_state::RobotState& solution, const std::string& link_name)
485  {
486  boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
487  new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));
488 
489  moveit::core::GroupStateValidityCallbackFn constraint_fn = boost::bind(
490  &Moveit_grasp_mediator::isStateValid,this, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get(), visual_tools_, _1, _2, _3);
491 
492  // seed IK call with current state
493  solution = (*ls)->getCurrentState();
494 
495  // Solve IK problem for arm
496  // disable explicit restarts to guarantee close solution if one exists
497  const double timeout = 0.1;
498  return solution.setFromIK(arm_jmg, target_pose, link_name, timeout, constraint_fn);
499 }
500 
502  for(Abstract_robot* ar: robots_) ar->notify();
503  visual_tools_->setAlpha(1.0);
504  auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
505  auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();
506 
507  ROS_INFO("%li die cuboids", cbd.size());
508  ROS_INFO("%li cuboids", cod.size());
509 
510  for(auto&o:cod){
511  o.publishCOwait(visual_tools_,planning_scene_monitor_,rviz_visual_tools::GREY);
512  psi_->addCollisionObjects({o.getCoMsg()});
513  }
514 
515  for(auto&o:cbd){
516  //o.publish(visual_tools_,rviz_visual_tools::GREEN);
517  psi_->addCollisionObjects({o.getCoMsg()});
518  }
519 
520  visual_tools_->publishRobotState(mgi_->getCurrentState());
521  visual_tools_->trigger();
522 
523 }
524 
525 Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh)
526  : Moveit_mediator(objects, pub, nh)
527  , robot_reader_(std::make_unique<Robot_reader>(nh))
528  , wing_reader_(std::make_unique<Wing_reader>(nh))
529  , cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) {
530  // , job_reader_(std::make_unique<Job_reader>(nh)) {
531  visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
532  robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_);
533  visual_tools_->loadMarkerPub();
534  visual_tools_->loadRemoteControl();
535  visual_tools_->setPlanningSceneMonitor(planning_scene_monitor_);
536 
537  visual_tools_->loadRobotStatePub("/display_robot_state");
538  visual_tools_->setRobotStateTopic("/robot_state_publisher");
539  visual_tools_->loadTrajectoryPub("/display_planned_path");
540 
541  visual_tools_->loadSharedRobotState();
542  visual_tools_->enableBatchPublishing();
543  visual_tools_->deleteAllMarkers();
544  visual_tools_->removeAllCollisionObjects();
545  visual_tools_->trigger();
546 
547  nh_->getParam("/grasps_ns", grasp_ns_);
548  grasp_ns_ = "grasps_ns";
549  ROS_INFO("ns is %s", grasp_ns_.c_str());
550 
551  planning_pipeline_ = std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, *nh_, "planning_plugin", "request_adapter");
552  voxel_manager_ = std::make_shared<VoxelManager>(*nh_, grasp_ns_.c_str() ,visual_tools_,planning_scene_monitor_);
553 
554  auto rd = robot_reader_->robot_data();
555  for (int i = 0; i < rd.size() ;i++) connect_robots(new Moveit_robot(rd[i].name_, rd[i].pose_, rd[i].size_));
556  /*
557  auto wd = static_cast<Wing_reader*>(wing_reader_.get())->wing_data();
558  for (int i = 0; i < robots_.size(); i++){
559  for(object_data& w : wd[i].first){
560  w.pose_ = robots_[i]->tf().inverse() * w.pose_;
561  }
562  robots_[i]->set_observer_mask(static_cast<wing_config>(wd[i].second));
563  }
564 
565  set_wings(wd);
566 
567  for (int i = 0; i < robots_.size(); i++){
568  std::bitset<3> bs = wd[i].second;
569  build_wings(bs, i);
570  }
571  */
572 
573  for (auto* r : robots_)r->notify();
574 
575  publish_tables();
576  scene_setup();
577  };
Moveit_grasp_mediator::generateRandomCuboid
bool generateRandomCuboid(std::string &object_name, geometry_msgs::Pose &object_pose, double &x_depth, double &y_width, double &z_height)
Definition: moveit_grasp_mediator.cpp:108
Moveit_grasp_mediator::connect_robots
void connect_robots(Abstract_robot *robot) override
Definition: moveit_grasp_mediator.cpp:8
Moveit_robot::map
std::map< std::string, std::string > & map()
Definition: moveit_robot.h:48
Moveit_robot::mgi_hand
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > mgi_hand()
Definition: moveit_robot.h:44
Robot::observers
std::vector< Abstract_robot_element * > & observers()
Definition: robot.h:29
Wing_moveit_decorator
Definition: wing_moveit_decorator.h:13
Moveit_mediator::publish_tables
void publish_tables()
Definition: moveit_mediator.cpp:24
Moveit_grasp_mediator::mediate
void mediate() override
Definition: moveit_grasp_mediator.cpp:141
Wing_reader
Wing reader.
Definition: wing_reader.h:14
Moveit_mediator::mgi_
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > mgi_
Definition: moveit_mediator.h:61
Moveit_grasp_mediator::planning_pipeline_
std::shared_ptr< planning_pipeline::PlanningPipeline > planning_pipeline_
Definition: moveit_grasp_mediator.h:25
Moveit_grasp_mediator::grasp_ns_
std::string grasp_ns_
Definition: moveit_grasp_mediator.h:27
Moveit_grasp_mediator::voxel_environment_
boost::dynamic_bitset voxel_environment_
Definition: moveit_grasp_mediator.h:28
Cuboid_reader
Cuboid reader.
Definition: cuboid_reader.h:17
Moveit_mediator::nh_
std::shared_ptr< ros::NodeHandle > nh_
Definition: moveit_mediator.h:56
Moveit_grasp_mediator::cuboid_reader_
std::unique_ptr< Abstract_param_reader > cuboid_reader_
Definition: moveit_grasp_mediator.h:35
Moveit_grasp_mediator::manipulate_mapspace
void manipulate_mapspace(tf2::Transform &tf, tf2::Vector3 &size)
Definition: moveit_grasp_mediator.cpp:35
Moveit_grasp_mediator::robot_reader_
std::unique_ptr< Robot_reader > robot_reader_
Definition: moveit_grasp_mediator.h:33
moveit_grasp_mediator.h
Moveit_grasp_mediator::Moveit_grasp_mediator
Moveit_grasp_mediator(std::vector< std::vector< tf2::Transform >> objects, ros::Publisher *pub, std::shared_ptr< ros::NodeHandle > const &nh)
Definition: moveit_grasp_mediator.cpp:525
wing_moveit_decorator.h
Moveit_grasp_mediator::grasp_maps_
std::vector< GraspMap > grasp_maps_
Definition: moveit_grasp_mediator.h:29
Moveit_mediator::psi_
std::unique_ptr< moveit::planning_interface::PlanningSceneInterface > psi_
Definition: moveit_mediator.h:62
Abstract_robot::name
std::string & name()
Definition: impl/abstract_robot.h:57
Abstract_robot::size
tf2::Vector3 & size()
Definition: impl/abstract_robot.h:59
Moveit_grasp_mediator::waitForNextStep
void waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr &visual_tools, const std::string &prompt)
Definition: moveit_grasp_mediator.cpp:424
Moveit_grasp_mediator::planFullGrasp
bool planFullGrasp(std::vector< moveit_grasps::GraspCandidatePtr > &grasp_candidates, moveit_grasps::GraspCandidatePtr &valid_grasp_candidate, moveit_msgs::MotionPlanResponse &pre_approach_plan, const std::string &object_name)
Definition: moveit_grasp_mediator.cpp:382
Abstract_robot::tf
tf2::Transform & tf()
Definition: impl/abstract_robot.h:58
Moveit_grasp_mediator::visualizePick
void visualizePick(const moveit_grasps::GraspCandidatePtr &valid_grasp_candidate, const moveit_msgs::MotionPlanResponse &pre_approach_plan)
Definition: moveit_grasp_mediator.cpp:318
Moveit_robot
Definition: moveit_robot.h:12
Moveit_grasp_mediator::isStateValid
bool isStateValid(const planning_scene::PlanningScene *planning_scene, const moveit_visual_tools::MoveItVisualToolsPtr &visual_tools, robot_state::RobotState *robot_state, const robot_model::JointModelGroup *group, const double *ik_solution)
Definition: moveit_grasp_mediator.cpp:474
Moveit_grasp_mediator::manipulate_grasp_data
void manipulate_grasp_data(Moveit_robot *robot)
Definition: moveit_grasp_mediator.cpp:49
Moveit_grasp_mediator::voxel_manager_
std::shared_ptr< VoxelManager > voxel_manager_
Definition: moveit_grasp_mediator.h:26
Moveit_mediator
Definition: moveit_mediator.h:54
Moveit_mediator::planning_scene_monitor_
std::shared_ptr< planning_scene_monitor::PlanningSceneMonitor > planning_scene_monitor_
Definition: moveit_mediator.h:65
Abstract_mediator::robots_
std::vector< Abstract_robot * > robots_
Definition: impl/abstract_mediator.h:35
Moveit_grasp_mediator::getIKSolution
bool getIKSolution(const moveit::core::JointModelGroup *arm_jmg, const Eigen::Isometry3d &target_pose, robot_state::RobotState &solution, const std::string &link_name)
Definition: moveit_grasp_mediator.cpp:483
Abstract_robot::bounds
std::vector< tf2::Transform > & bounds()
Definition: impl/abstract_robot.h:61
Moveit_robot::grasp_map_generator
std::shared_ptr< MapGenerator > grasp_map_generator()
Definition: moveit_robot.h:45
Moveit_mediator::robot_model_
std::shared_ptr< moveit::core::RobotModel > robot_model_
Definition: moveit_mediator.h:57
Wing
Definition: wing.h:7
Moveit_mediator::mgi
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > mgi()
Definition: moveit_mediator.h:87
Moveit_grasp_mediator::planPreApproach
bool planPreApproach(const robot_state::RobotState &goal_state, moveit_msgs::MotionPlanResponse &pre_approach_plan)
Definition: moveit_grasp_mediator.cpp:429
Robot_reader
Robot reader.
Definition: robot_reader.h:15
Abstract_robot
Definition: impl/abstract_robot.h:25
Moveit_grasp_mediator::scene_setup
void scene_setup()
Definition: moveit_grasp_mediator.cpp:501
Moveit_mediator::visual_tools_
std::shared_ptr< moveit_visual_tools::MoveItVisualTools > visual_tools_
Definition: moveit_mediator.h:58


multi_cell_builder
Author(s): MA
autogenerated on Thu Jan 12 2023 23:45:43