3 #include <gb_grasp/MapConfigLoader.h>
4 #include <moveit/robot_state/conversions.h>
10 ROS_INFO(
"%s connected...", robot->
name().c_str());
16 double min_quality, max_candidates;
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);
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);
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);
37 std::vector<double> rot_mapspace(3,0);
39 tf2::Matrix3x3 m(tf.getRotation());
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()}));
50 XmlRpc::XmlRpcValue data;
51 nh_->getParam(
"/", data);
53 std::regex rx(
"panda_arm([0-9]+)");
55 std::regex_match(robot->
name(), match, rx);
56 data[
"base_link"]=
"world";
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";
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;
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;
67 data[robot->
map().at(
"eef_name").c_str()][
"grasp_depth_resolution"] = 0.005;
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;
73 data[robot->
map().at(
"eef_name").c_str()][
"grasp_padding_on_approach"] = 0.005;
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;
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;
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();
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;
96 data[robot->
map().at(
"eef_name").c_str()][
"max_grasp_width"] = 0.08;
98 data[robot->
map().at(
"eef_name").c_str()][
"gripper_finger_width"] = 0.015;
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;
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);
109 double& y_width,
double& z_height)
118 rviz_visual_tools::RandomPoseBounds pose_bounds(xmin, xmax, ymin, ymax, zmin, zmax);
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);
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);
136 success = rate.cycleTime().toSec() < timeout;
147 map_config.SetObjects(cbd);
148 float object_offset = 0.02f;
150 tf2::Vector3 middle_pt(0,0,0);
151 std::vector<double> x;
152 std::vector<double> y;
155 tf2::Vector3 pos = r->
tf().getOrigin();
156 pos.setZ((pos.getZ()*2) +object_offset);
157 tf2::Transform man_tf(r->
tf().getRotation(), pos);
161 ROS_INFO(
"X: %f, Y: %f, Z: %f", pos.getX(), pos.getY(), pos.getZ());
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());
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);
187 ROS_INFO(
"X: %f, Y: %f, Z: %f", pos.getX(), pos.getY(), pos.getZ());
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());
204 ROS_INFO_STREAM(
"number of maps: "<<
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);
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))
233 ROS_INFO(
"Failed to add random cuboid ot planning scene");
238 std::vector<moveit_grasps::GraspCandidatePtr> grasp_candidates;
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;
249 auto grasp_generator_ =std::make_shared<moveit_grasps::TwoFingerGraspGenerator>(
visual_tools_);
252 std::vector<double> ideal_grasp_rpy = { 3.14, 0.0, 0.0 };
253 grasp_generator_->setIdealTCPGraspPoseRPY(ideal_grasp_rpy);
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;
264 grasp_score_weights->depth_score_weight_ = 2.0;
265 grasp_score_weights->width_score_weight_ = 2.0;
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"))
271 ROS_INFO(
"Failed to load Grasp Data parameters.");
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))
279 ROS_INFO(
"Grasp generator failed to generate any valid 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()))
289 ROS_INFO(
"The ideal seed state is not reachable. Using start state as seed.");
295 bool filter_pregrasps =
true;
296 auto grasp_filter_ = std::make_shared<moveit_grasps::TwoFingerGraspFilter>(
visual_tools_->getSharedRobotState(),
visual_tools_);
300 ROS_INFO(
"Filter grasps failed");
302 if (!grasp_filter_->removeInvalidAndFilter(grasp_candidates))
304 ROS_INFO(
"Grasp filtering removed all grasps");
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))
312 ROS_INFO(
"Failed to plan grasp motions");
319 const moveit_msgs::MotionPlanResponse& pre_approach_plan)
321 EigenSTL::vector_Isometry3d waypoints;
322 moveit_grasps::GraspGenerator::getGraspWaypoints(valid_grasp_candidate, waypoints);
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))
342 visual_tools_->publishRobotState(grasp_state, rviz_visual_tools::YELLOW);
344 if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 1 &&
345 valid_grasp_candidate->segmented_cartesian_traj_[1].size())
348 visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[1].back(),
349 rviz_visual_tools::BLUE);
351 if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 2 &&
352 valid_grasp_candidate->segmented_cartesian_traj_[2].size())
355 visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[2].back(),
356 rviz_visual_tools::PURPLE);
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();
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();
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();
383 moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
384 moveit_msgs::MotionPlanResponse& pre_approach_plan,
const std::string& object_name)
386 moveit::core::RobotStatePtr current_state;
388 boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
390 current_state = std::make_shared<robot_state::RobotState>((*ls)->getCurrentState());
393 std::shared_ptr<moveit_grasps::GraspPlanner>grasp_planner_ = std::make_shared<moveit_grasps::GraspPlanner>(
visual_tools_);
398 bool success =
false;
399 for (; !grasp_candidates.empty(); grasp_candidates.erase(grasp_candidates.begin()))
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,
406 ROS_INFO(
"failed to plan approach lift retreat");
410 robot_state::RobotStatePtr pre_grasp_state =
411 valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH].front();
414 ROS_INFO(
"failed to plan to pregrasp_state");
426 visual_tools->prompt(prompt);
431 planning_interface::MotionPlanRequest req;
432 planning_interface::MotionPlanResponse res;
434 double tolerance_above = 0.01;
435 double tolerance_below = 0.01;
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);
443 req.goal_constraints.push_back(goal);
444 boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
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" };
455 for (std::size_t i = 0; i < joint_names.size(); ++i)
457 rs.setJointPositions(joint_names[i], &starting_joint_values[i]);
460 robot_state::robotStateToRobotStateMsg(rs, req.start_state);
464 if (res.error_code_.val != res.error_code_.SUCCESS)
466 ROS_INFO(
"Failed to plan approach successfully");
470 res.getMessage(pre_approach_plan);
475 const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, robot_state::RobotState* robot_state,
476 const robot_model::JointModelGroup* group,
const double* ik_solution)
478 robot_state->setJointGroupPositions(group, ik_solution);
479 robot_state->update();
480 return !planning_scene->isStateColliding(*robot_state, group->getName());
484 robot_state::RobotState& solution,
const std::string& link_name)
486 boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
489 moveit::core::GroupStateValidityCallbackFn constraint_fn = boost::bind(
493 solution = (*ls)->getCurrentState();
497 const double timeout = 0.1;
498 return solution.setFromIK(arm_jmg, target_pose, link_name, timeout, constraint_fn);
507 ROS_INFO(
"%li die cuboids", cbd.size());
508 ROS_INFO(
"%li cuboids", cod.size());
512 psi_->addCollisionObjects({o.getCoMsg()});
517 psi_->addCollisionObjects({o.getCoMsg()});
531 visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
573 for (
auto* r :
robots_)r->notify();