00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/pick_place/pick_place.h>
00038 #include <moveit/pick_place/approach_and_translate_stage.h>
00039 #include <moveit/trajectory_processing/trajectory_tools.h>
00040 #include <eigen_conversions/eigen_msg.h>
00041 #include <ros/console.h>
00042
00043 namespace pick_place
00044 {
00045 ApproachAndTranslateStage::ApproachAndTranslateStage(
00046 const planning_scene::PlanningSceneConstPtr& scene,
00047 const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix)
00048 : ManipulationStage("approach & translate"), planning_scene_(scene), collision_matrix_(collision_matrix)
00049 {
00050 max_goal_count_ = GetGlobalPickPlaceParams().max_goal_count_;
00051 max_fail_ = GetGlobalPickPlaceParams().max_fail_;
00052 max_step_ = GetGlobalPickPlaceParams().max_step_;
00053 jump_factor_ = GetGlobalPickPlaceParams().jump_factor_;
00054 }
00055
00056 namespace
00057 {
00058 bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene,
00059 const collision_detection::AllowedCollisionMatrix* collision_matrix, bool verbose,
00060 const trajectory_msgs::JointTrajectory* grasp_posture, robot_state::RobotState* state,
00061 const robot_state::JointModelGroup* group, const double* joint_group_variable_values)
00062 {
00063 state->setJointGroupPositions(group, joint_group_variable_values);
00064
00065 collision_detection::CollisionRequest req;
00066 req.verbose = verbose;
00067 req.group_name = group->getName();
00068
00069 if (grasp_posture->joint_names.size() > 0)
00070 {
00071
00072
00073 for (std::size_t i = 0; i < grasp_posture->points.size(); ++i)
00074 {
00075 state->setVariablePositions(grasp_posture->joint_names, grasp_posture->points[i].positions);
00076 collision_detection::CollisionResult res;
00077 planning_scene->checkCollision(req, res, *state, *collision_matrix);
00078 if (res.collision)
00079 return false;
00080 }
00081 }
00082 else
00083 {
00084 collision_detection::CollisionResult res;
00085 planning_scene->checkCollision(req, res, *state, *collision_matrix);
00086 if (res.collision)
00087 return false;
00088 }
00089 return planning_scene->isStateFeasible(*state);
00090 }
00091
00092 bool samplePossibleGoalStates(const ManipulationPlanPtr& plan, const robot_state::RobotState& reference_state,
00093 double min_distance, unsigned int attempts)
00094 {
00095
00096 robot_state::RobotStatePtr token_state(new robot_state::RobotState(reference_state));
00097 for (unsigned int j = 0; j < attempts; ++j)
00098 {
00099 double min_d = std::numeric_limits<double>::infinity();
00100
00101
00102 if (plan->goal_sampler_->sample(*token_state, plan->shared_data_->max_goal_sampling_attempts_))
00103 {
00104
00105 for (std::size_t i = 0; i < plan->possible_goal_states_.size(); ++i)
00106 {
00107 double d = plan->possible_goal_states_[i]->distance(*token_state, plan->shared_data_->planning_group_);
00108 if (d < min_d)
00109 min_d = d;
00110 }
00111 if (min_d >= min_distance)
00112 {
00113 plan->possible_goal_states_.push_back(token_state);
00114 return true;
00115 }
00116 }
00117 }
00118 return false;
00119 }
00120
00121
00122
00123 bool executeAttachObject(const ManipulationPlanSharedDataConstPtr& shared_plan_data,
00124 const trajectory_msgs::JointTrajectory& detach_posture,
00125 const plan_execution::ExecutableMotionPlan* motion_plan)
00126 {
00127 if (shared_plan_data->diff_attached_object_.object.id.empty())
00128 {
00129
00130 return true;
00131 }
00132
00133 ROS_DEBUG_NAMED("manipulation", "Applying attached object diff to maintained planning scene (attaching/detaching "
00134 "object to end effector)");
00135 bool ok = false;
00136 {
00137 planning_scene_monitor::LockedPlanningSceneRW ps(motion_plan->planning_scene_monitor_);
00138 moveit_msgs::AttachedCollisionObject msg = shared_plan_data->diff_attached_object_;
00139
00140
00141 msg.detach_posture = detach_posture;
00142 ok = ps->processAttachedCollisionObjectMsg(msg);
00143 }
00144 motion_plan->planning_scene_monitor_->triggerSceneUpdateEvent(
00145 (planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType)(
00146 planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY +
00147 planning_scene_monitor::PlanningSceneMonitor::UPDATE_STATE));
00148 return ok;
00149 }
00150
00151
00152 void addGripperTrajectory(const ManipulationPlanPtr& plan,
00153 const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix,
00154 const std::string& name)
00155 {
00156
00157 if (!plan->retreat_posture_.joint_names.empty())
00158 {
00159 robot_state::RobotStatePtr ee_closed_state(
00160 new robot_state::RobotState(plan->trajectories_.back().trajectory_->getLastWayPoint()));
00161
00162 robot_trajectory::RobotTrajectoryPtr ee_closed_traj(new robot_trajectory::RobotTrajectory(
00163 ee_closed_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName()));
00164 ee_closed_traj->setRobotTrajectoryMsg(*ee_closed_state, plan->retreat_posture_);
00165
00166
00167 if (plan->retreat_posture_.points.size() > 0 &&
00168 plan->retreat_posture_.points.back().time_from_start > ros::Duration(0.0))
00169 {
00170 ee_closed_traj->addPrefixWayPoint(ee_closed_state, 0.0);
00171 }
00172 else
00173 {
00174 ee_closed_traj->addPrefixWayPoint(ee_closed_state, PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION);
00175 }
00176
00177 plan_execution::ExecutableTrajectory et(ee_closed_traj, name);
00178
00179
00180 et.effect_on_success_ = boost::bind(&executeAttachObject, plan->shared_data_, plan->approach_posture_, _1);
00181 et.allowed_collision_matrix_ = collision_matrix;
00182 plan->trajectories_.push_back(et);
00183 }
00184 else
00185 {
00186 ROS_WARN_NAMED("manipulation", "No joint states of grasp postures have been defined in the pick place action.");
00187 }
00188 }
00189
00190 }
00191
00192 bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const
00193 {
00194 const robot_model::JointModelGroup* jmg = plan->shared_data_->planning_group_;
00195
00196
00197
00198 const double min_distance = 0.01 * jmg->getMaximumExtent();
00199
00200
00201 Eigen::Vector3d approach_direction, retreat_direction;
00202 tf::vectorMsgToEigen(plan->approach_.direction.vector, approach_direction);
00203 tf::vectorMsgToEigen(plan->retreat_.direction.vector, retreat_direction);
00204
00205
00206
00207 bool approach_direction_is_global_frame = !robot_state::Transforms::sameFrame(
00208 plan->approach_.direction.header.frame_id, plan->shared_data_->ik_link_->getName());
00209 bool retreat_direction_is_global_frame = !robot_state::Transforms::sameFrame(plan->retreat_.direction.header.frame_id,
00210 plan->shared_data_->ik_link_->getName());
00211
00212
00213 if (approach_direction_is_global_frame)
00214 approach_direction =
00215 planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).rotation() * approach_direction;
00216 if (retreat_direction_is_global_frame)
00217 retreat_direction =
00218 planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).rotation() * retreat_direction;
00219
00220
00221 robot_state::GroupStateValidityCallbackFn approach_validCallback =
00222 boost::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_,
00223 &plan->approach_posture_, _1, _2, _3);
00224 plan->goal_sampler_->setVerbose(verbose_);
00225 std::size_t attempted_possible_goal_states = 0;
00226 do
00227 {
00228 for (std::size_t i = attempted_possible_goal_states; i < plan->possible_goal_states_.size() && !signal_stop_;
00229 ++i, ++attempted_possible_goal_states)
00230 {
00231
00232 if (plan->shared_data_->minimize_object_distance_)
00233 {
00234 static const double MAX_CLOSE_UP_DIST = 1.0;
00235 robot_state::RobotStatePtr close_up_state(new robot_state::RobotState(*plan->possible_goal_states_[i]));
00236 std::vector<robot_state::RobotStatePtr> close_up_states;
00237 double d_close_up = close_up_state->computeCartesianPath(
00238 plan->shared_data_->planning_group_, close_up_states, plan->shared_data_->ik_link_, approach_direction,
00239 approach_direction_is_global_frame, MAX_CLOSE_UP_DIST, max_step_, jump_factor_, approach_validCallback);
00240
00241 if (d_close_up > 0.0 && close_up_states.size() > 1)
00242 *plan->possible_goal_states_[i] = *close_up_states[close_up_states.size() - 2];
00243 }
00244
00245
00246 robot_state::RobotStatePtr first_approach_state(new robot_state::RobotState(*plan->possible_goal_states_[i]));
00247
00248 std::vector<robot_state::RobotStatePtr> approach_states;
00249 double d_approach = first_approach_state->computeCartesianPath(
00250 plan->shared_data_->planning_group_, approach_states, plan->shared_data_->ik_link_, -approach_direction,
00251 approach_direction_is_global_frame, plan->approach_.desired_distance, max_step_, jump_factor_,
00252 approach_validCallback);
00253
00254
00255 if (d_approach > plan->approach_.min_distance && !signal_stop_)
00256 {
00257 if (plan->retreat_.desired_distance > 0.0)
00258 {
00259
00260 planning_scene::PlanningScenePtr planning_scene_after_approach = planning_scene_->diff();
00261
00262
00263 planning_scene_after_approach->getCurrentStateNonConst() = *plan->possible_goal_states_[i];
00264
00265
00266 planning_scene_after_approach->processAttachedCollisionObjectMsg(plan->shared_data_->diff_attached_object_);
00267
00268
00269
00270 robot_state::GroupStateValidityCallbackFn retreat_validCallback =
00271 boost::bind(&isStateCollisionFree, planning_scene_after_approach.get(), collision_matrix_.get(), verbose_,
00272 &plan->retreat_posture_, _1, _2, _3);
00273
00274
00275 robot_state::RobotStatePtr last_retreat_state(
00276 new robot_state::RobotState(planning_scene_after_approach->getCurrentState()));
00277 std::vector<robot_state::RobotStatePtr> retreat_states;
00278 double d_retreat = last_retreat_state->computeCartesianPath(
00279 plan->shared_data_->planning_group_, retreat_states, plan->shared_data_->ik_link_, retreat_direction,
00280 retreat_direction_is_global_frame, plan->retreat_.desired_distance, max_step_, jump_factor_,
00281 retreat_validCallback);
00282
00283
00284
00285 if (d_retreat > plan->retreat_.min_distance && !signal_stop_)
00286 {
00287
00288 std::reverse(approach_states.begin(), approach_states.end());
00289 robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory(
00290 planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
00291 for (std::size_t k = 0; k < approach_states.size(); ++k)
00292 approach_traj->addSuffixWayPoint(approach_states[k], 0.0);
00293
00294
00295 robot_trajectory::RobotTrajectoryPtr retreat_traj(new robot_trajectory::RobotTrajectory(
00296 planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
00297 for (std::size_t k = 0; k < retreat_states.size(); ++k)
00298 retreat_traj->addSuffixWayPoint(retreat_states[k], 0.0);
00299
00300
00301 time_param_.computeTimeStamps(*approach_traj);
00302 time_param_.computeTimeStamps(*retreat_traj);
00303
00304
00305 plan_execution::ExecutableTrajectory et_approach(approach_traj, "approach");
00306 et_approach.allowed_collision_matrix_ = collision_matrix_;
00307 plan->trajectories_.push_back(et_approach);
00308
00309
00310 addGripperTrajectory(plan, collision_matrix_, "grasp");
00311
00312
00313 plan_execution::ExecutableTrajectory et_retreat(retreat_traj, "retreat");
00314 et_retreat.allowed_collision_matrix_ = collision_matrix_;
00315 plan->trajectories_.push_back(et_retreat);
00316
00317 plan->approach_state_ = approach_states.front();
00318 return true;
00319 }
00320 }
00321 else
00322 {
00323
00324 plan->approach_state_.swap(first_approach_state);
00325
00326
00327 std::reverse(approach_states.begin(), approach_states.end());
00328 robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory(
00329 planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
00330 for (std::size_t k = 0; k < approach_states.size(); ++k)
00331 approach_traj->addSuffixWayPoint(approach_states[k], 0.0);
00332
00333
00334 time_param_.computeTimeStamps(*approach_traj);
00335
00336
00337 plan_execution::ExecutableTrajectory et_approach(approach_traj, "approach");
00338 et_approach.allowed_collision_matrix_ = collision_matrix_;
00339 plan->trajectories_.push_back(et_approach);
00340
00341
00342 addGripperTrajectory(plan, collision_matrix_, "grasp");
00343
00344 plan->approach_state_ = approach_states.front();
00345
00346 return true;
00347 }
00348 }
00349 }
00350 } while (plan->possible_goal_states_.size() < max_goal_count_ && !signal_stop_ &&
00351 samplePossibleGoalStates(plan, planning_scene_->getCurrentState(), min_distance, max_fail_));
00352
00353 plan->error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
00354
00355 return false;
00356 }
00357 }