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