47 const planning_scene::PlanningSceneConstPtr& scene,
48 const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix)
49 : ManipulationStage(
"approach & translate"), planning_scene_(scene), collision_matrix_(collision_matrix)
70 if (!grasp_posture->joint_names.empty())
74 for (std::size_t i = 0; i < grasp_posture->points.size(); ++i)
86 planning_scene->checkCollision(req, res, *state, *collision_matrix);
94 double min_distance,
unsigned int attempts)
98 for (
unsigned int j = 0; j < attempts; ++j)
100 double min_d = std::numeric_limits<double>::infinity();
103 if (plan->goal_sampler_->sample(*token_state, plan->shared_data_->max_goal_sampling_attempts_))
106 for (std::size_t i = 0; i < plan->possible_goal_states_.size(); ++i)
108 double d = plan->possible_goal_states_[i]->distance(*token_state, plan->shared_data_->planning_group_);
112 if (min_d >= min_distance)
114 plan->possible_goal_states_.push_back(token_state);
124 bool executeAttachObject(
const ManipulationPlanSharedDataConstPtr& shared_plan_data,
125 const trajectory_msgs::JointTrajectory& detach_posture,
128 if (shared_plan_data->diff_attached_object_.object.id.empty())
134 ROS_DEBUG_NAMED(
"manipulation",
"Applying attached object diff to maintained planning scene (attaching/detaching "
135 "object to end effector)");
139 moveit_msgs::AttachedCollisionObject msg = shared_plan_data->diff_attached_object_;
142 msg.detach_posture = detach_posture;
143 ok = ps->processAttachedCollisionObjectMsg(msg);
153 void addGripperTrajectory(
const ManipulationPlanPtr& plan,
154 const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix,
155 const std::string& name)
158 if (!plan->retreat_posture_.joint_names.empty())
160 moveit::core::RobotStatePtr ee_closed_state(
164 ee_closed_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName()));
165 ee_closed_traj->setRobotTrajectoryMsg(*ee_closed_state, plan->retreat_posture_);
168 if (!plan->retreat_posture_.points.empty() &&
169 plan->retreat_posture_.points.back().time_from_start >
ros::Duration(0.0))
171 ee_closed_traj->addPrefixWayPoint(ee_closed_state, 0.0);
175 ROS_INFO_STREAM(
"Adding default duration of " << PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION
176 <<
" seconds to the grasp closure time. Assign time_from_start to "
177 <<
"your trajectory to avoid this.");
178 ee_closed_traj->addPrefixWayPoint(ee_closed_state, PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION);
185 return executeAttachObject(plan->shared_data_, plan->approach_posture_, *motion_plan);
187 et.allowed_collision_matrix_ = collision_matrix;
188 plan->trajectories_.push_back(et);
192 ROS_WARN_NAMED(
"manipulation",
"No joint states of grasp postures have been defined in the pick place action.");
198 bool ApproachAndTranslateStage::evaluate(
const ManipulationPlanPtr& plan)
const
207 Eigen::Vector3d approach_direction, retreat_direction;
208 tf2::fromMsg(plan->approach_.direction.vector, approach_direction);
209 tf2::fromMsg(plan->retreat_.direction.vector, retreat_direction);
214 plan->approach_.direction.header.frame_id, plan->shared_data_->ik_link_->getName());
216 plan->retreat_.direction.header.frame_id, plan->shared_data_->ik_link_->getName());
219 if (approach_direction_is_global_frame)
222 planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).linear() * approach_direction;
223 if (retreat_direction_is_global_frame)
226 planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).linear() * retreat_direction;
230 [scene = planning_scene_.get(), acm = collision_matrix_.get(),
verbose = this->verbose_,
233 const double* joint_group_variable_values) {
234 return isStateCollisionFree(scene, acm,
verbose, approach_posture, robot_state, joint_group,
235 joint_group_variable_values);
237 plan->goal_sampler_->setVerbose(verbose_);
238 std::size_t attempted_possible_goal_states = 0;
241 for (std::size_t i = attempted_possible_goal_states; i < plan->possible_goal_states_.size() && !signal_stop_;
242 ++i, ++attempted_possible_goal_states)
245 if (plan->shared_data_->minimize_object_distance_)
247 static const double MAX_CLOSE_UP_DIST = 1.0;
249 std::vector<moveit::core::RobotStatePtr> close_up_states;
251 close_up_state.get(), plan->shared_data_->planning_group_, close_up_states, plan->shared_data_->ik_link_,
252 approach_direction, approach_direction_is_global_frame, MAX_CLOSE_UP_DIST,
255 if (d_close_up > 0.0 && close_up_states.size() > 1)
256 *plan->possible_goal_states_[i] = *close_up_states[close_up_states.size() - 2];
262 std::vector<moveit::core::RobotStatePtr> approach_states;
264 first_approach_state.get(), plan->shared_data_->planning_group_, approach_states,
265 plan->shared_data_->ik_link_, -approach_direction, approach_direction_is_global_frame,
270 if (d_approach > plan->approach_.min_distance && !signal_stop_)
272 if (plan->retreat_.desired_distance > 0.0)
275 planning_scene::PlanningScenePtr planning_scene_after_approach = planning_scene_->diff();
278 planning_scene_after_approach->getCurrentStateNonConst() = *plan->possible_goal_states_[i];
281 planning_scene_after_approach->processAttachedCollisionObjectMsg(plan->shared_data_->diff_attached_object_);
286 [scene = planning_scene_after_approach.get(), acm = collision_matrix_.get(),
verbose = verbose_,
289 const double* joint_group_variable_values) {
290 return isStateCollisionFree(scene, acm, verbose, retreat_posture, robot_state, joint_group,
291 joint_group_variable_values);
295 moveit::core::RobotStatePtr last_retreat_state(
297 std::vector<moveit::core::RobotStatePtr> retreat_states;
299 last_retreat_state.get(), plan->shared_data_->planning_group_, retreat_states,
300 plan->shared_data_->ik_link_, retreat_direction, retreat_direction_is_global_frame,
306 if (d_retreat > plan->retreat_.min_distance && !signal_stop_)
309 std::reverse(approach_states.begin(), approach_states.end());
311 planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
312 for (
const moveit::core::RobotStatePtr& approach_state : approach_states)
313 approach_traj->addSuffixWayPoint(approach_state, 0.0);
317 planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
318 for (
const moveit::core::RobotStatePtr& retreat_state : retreat_states)
319 retreat_traj->addSuffixWayPoint(retreat_state, 0.0);
322 time_param_.computeTimeStamps(*approach_traj);
323 time_param_.computeTimeStamps(*retreat_traj);
327 et_approach.allowed_collision_matrix_ = collision_matrix_;
328 plan->trajectories_.push_back(et_approach);
331 addGripperTrajectory(plan, collision_matrix_,
"grasp");
335 et_retreat.allowed_collision_matrix_ = collision_matrix_;
336 plan->trajectories_.push_back(et_retreat);
338 plan->approach_state_ = approach_states.front();
345 plan->approach_state_.swap(first_approach_state);
348 std::reverse(approach_states.begin(), approach_states.end());
350 planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
351 for (
const moveit::core::RobotStatePtr& approach_state : approach_states)
352 approach_traj->addSuffixWayPoint(approach_state, 0.0);
355 time_param_.computeTimeStamps(*approach_traj);
359 et_approach.allowed_collision_matrix_ = collision_matrix_;
360 plan->trajectories_.push_back(et_approach);
363 addGripperTrajectory(plan, collision_matrix_,
"grasp");
365 plan->approach_state_ = approach_states.front();
371 }
while (plan->possible_goal_states_.size() < max_goal_count_ && !signal_stop_ &&
372 samplePossibleGoalStates(plan, planning_scene_->getCurrentState(), min_distance, max_fail_));
374 plan->error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;