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 ROS_DEBUG_NAMED("manipulation", "Applying attached object diff to maintained planning scene (attaching/detaching "
00128 "object to end effector)");
00129 bool ok = false;
00130 {
00131 planning_scene_monitor::LockedPlanningSceneRW ps(motion_plan->planning_scene_monitor_);
00132 moveit_msgs::AttachedCollisionObject msg = shared_plan_data->diff_attached_object_;
00133
00134
00135 msg.detach_posture = detach_posture;
00136 ok = ps->processAttachedCollisionObjectMsg(msg);
00137 }
00138 motion_plan->planning_scene_monitor_->triggerSceneUpdateEvent(
00139 (planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType)(
00140 planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY +
00141 planning_scene_monitor::PlanningSceneMonitor::UPDATE_STATE));
00142 return ok;
00143 }
00144
00145
00146 void addGripperTrajectory(const ManipulationPlanPtr& plan,
00147 const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix,
00148 const std::string& name)
00149 {
00150
00151 if (!plan->retreat_posture_.joint_names.empty())
00152 {
00153 robot_state::RobotStatePtr ee_closed_state(
00154 new robot_state::RobotState(plan->trajectories_.back().trajectory_->getLastWayPoint()));
00155
00156 robot_trajectory::RobotTrajectoryPtr ee_closed_traj(new robot_trajectory::RobotTrajectory(
00157 ee_closed_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName()));
00158 ee_closed_traj->setRobotTrajectoryMsg(*ee_closed_state, plan->retreat_posture_);
00159
00160
00161 if (plan->retreat_posture_.points.size() > 0 &&
00162 plan->retreat_posture_.points.back().time_from_start > ros::Duration(0.0))
00163 {
00164 ee_closed_traj->addPrefixWayPoint(ee_closed_state, 0.0);
00165 }
00166 else
00167 {
00168 ee_closed_traj->addPrefixWayPoint(ee_closed_state, PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION);
00169 }
00170
00171 plan_execution::ExecutableTrajectory et(ee_closed_traj, name);
00172
00173
00174 et.effect_on_success_ = boost::bind(&executeAttachObject, plan->shared_data_, plan->approach_posture_, _1);
00175 et.allowed_collision_matrix_ = collision_matrix;
00176 plan->trajectories_.push_back(et);
00177 }
00178 else
00179 {
00180 ROS_WARN_NAMED("manipulation", "No joint states of grasp postures have been defined in the pick place action.");
00181 }
00182 }
00183
00184 }
00185
00186 bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const
00187 {
00188 const robot_model::JointModelGroup* jmg = plan->shared_data_->planning_group_;
00189
00190
00191
00192 const double min_distance = 0.01 * jmg->getMaximumExtent();
00193
00194
00195 Eigen::Vector3d approach_direction, retreat_direction;
00196 tf::vectorMsgToEigen(plan->approach_.direction.vector, approach_direction);
00197 tf::vectorMsgToEigen(plan->retreat_.direction.vector, retreat_direction);
00198
00199
00200
00201 bool approach_direction_is_global_frame = !robot_state::Transforms::sameFrame(
00202 plan->approach_.direction.header.frame_id, plan->shared_data_->ik_link_->getName());
00203 bool retreat_direction_is_global_frame = !robot_state::Transforms::sameFrame(plan->retreat_.direction.header.frame_id,
00204 plan->shared_data_->ik_link_->getName());
00205
00206
00207 if (approach_direction_is_global_frame)
00208 approach_direction =
00209 planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).rotation() * approach_direction;
00210 if (retreat_direction_is_global_frame)
00211 retreat_direction =
00212 planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).rotation() * retreat_direction;
00213
00214
00215 robot_state::GroupStateValidityCallbackFn approach_validCallback =
00216 boost::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_,
00217 &plan->approach_posture_, _1, _2, _3);
00218 plan->goal_sampler_->setVerbose(verbose_);
00219 std::size_t attempted_possible_goal_states = 0;
00220 do
00221 {
00222 for (std::size_t i = attempted_possible_goal_states; i < plan->possible_goal_states_.size() && !signal_stop_;
00223 ++i, ++attempted_possible_goal_states)
00224 {
00225
00226 if (plan->shared_data_->minimize_object_distance_)
00227 {
00228 static const double MAX_CLOSE_UP_DIST = 1.0;
00229 robot_state::RobotStatePtr close_up_state(new robot_state::RobotState(*plan->possible_goal_states_[i]));
00230 std::vector<robot_state::RobotStatePtr> close_up_states;
00231 double d_close_up = close_up_state->computeCartesianPath(
00232 plan->shared_data_->planning_group_, close_up_states, plan->shared_data_->ik_link_, approach_direction,
00233 approach_direction_is_global_frame, MAX_CLOSE_UP_DIST, max_step_, jump_factor_, approach_validCallback);
00234
00235 if (d_close_up > 0.0 && close_up_states.size() > 1)
00236 *plan->possible_goal_states_[i] = *close_up_states[close_up_states.size() - 2];
00237 }
00238
00239
00240 robot_state::RobotStatePtr first_approach_state(new robot_state::RobotState(*plan->possible_goal_states_[i]));
00241
00242 std::vector<robot_state::RobotStatePtr> approach_states;
00243 double d_approach = first_approach_state->computeCartesianPath(
00244 plan->shared_data_->planning_group_, approach_states, plan->shared_data_->ik_link_, -approach_direction,
00245 approach_direction_is_global_frame, plan->approach_.desired_distance, max_step_, jump_factor_,
00246 approach_validCallback);
00247
00248
00249 if (d_approach > plan->approach_.min_distance && !signal_stop_)
00250 {
00251 if (plan->retreat_.desired_distance > 0.0)
00252 {
00253
00254 planning_scene::PlanningScenePtr planning_scene_after_approach = planning_scene_->diff();
00255
00256
00257 planning_scene_after_approach->getCurrentStateNonConst() = *plan->possible_goal_states_[i];
00258
00259
00260 planning_scene_after_approach->processAttachedCollisionObjectMsg(plan->shared_data_->diff_attached_object_);
00261
00262
00263
00264 robot_state::GroupStateValidityCallbackFn retreat_validCallback =
00265 boost::bind(&isStateCollisionFree, planning_scene_after_approach.get(), collision_matrix_.get(), verbose_,
00266 &plan->retreat_posture_, _1, _2, _3);
00267
00268
00269 robot_state::RobotStatePtr last_retreat_state(
00270 new robot_state::RobotState(planning_scene_after_approach->getCurrentState()));
00271 std::vector<robot_state::RobotStatePtr> retreat_states;
00272 double d_retreat = last_retreat_state->computeCartesianPath(
00273 plan->shared_data_->planning_group_, retreat_states, plan->shared_data_->ik_link_, retreat_direction,
00274 retreat_direction_is_global_frame, plan->retreat_.desired_distance, max_step_, jump_factor_,
00275 retreat_validCallback);
00276
00277
00278
00279 if (d_retreat > plan->retreat_.min_distance && !signal_stop_)
00280 {
00281
00282 std::reverse(approach_states.begin(), approach_states.end());
00283 robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory(
00284 planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
00285 for (std::size_t k = 0; k < approach_states.size(); ++k)
00286 approach_traj->addSuffixWayPoint(approach_states[k], 0.0);
00287
00288
00289 robot_trajectory::RobotTrajectoryPtr retreat_traj(new robot_trajectory::RobotTrajectory(
00290 planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
00291 for (std::size_t k = 0; k < retreat_states.size(); ++k)
00292 retreat_traj->addSuffixWayPoint(retreat_states[k], 0.0);
00293
00294
00295 time_param_.computeTimeStamps(*approach_traj);
00296 time_param_.computeTimeStamps(*retreat_traj);
00297
00298
00299 plan_execution::ExecutableTrajectory et_approach(approach_traj, "approach");
00300 et_approach.allowed_collision_matrix_ = collision_matrix_;
00301 plan->trajectories_.push_back(et_approach);
00302
00303
00304 addGripperTrajectory(plan, collision_matrix_, "grasp");
00305
00306
00307 plan_execution::ExecutableTrajectory et_retreat(retreat_traj, "retreat");
00308 et_retreat.allowed_collision_matrix_ = collision_matrix_;
00309 plan->trajectories_.push_back(et_retreat);
00310
00311 plan->approach_state_ = approach_states.front();
00312 return true;
00313 }
00314 }
00315 else
00316 {
00317
00318 plan->approach_state_.swap(first_approach_state);
00319
00320
00321 std::reverse(approach_states.begin(), approach_states.end());
00322 robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory(
00323 planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
00324 for (std::size_t k = 0; k < approach_states.size(); ++k)
00325 approach_traj->addSuffixWayPoint(approach_states[k], 0.0);
00326
00327
00328 time_param_.computeTimeStamps(*approach_traj);
00329
00330
00331 plan_execution::ExecutableTrajectory et_approach(approach_traj, "approach");
00332 et_approach.allowed_collision_matrix_ = collision_matrix_;
00333 plan->trajectories_.push_back(et_approach);
00334
00335
00336 addGripperTrajectory(plan, collision_matrix_, "grasp");
00337
00338 plan->approach_state_ = approach_states.front();
00339
00340 return true;
00341 }
00342 }
00343 }
00344 } while (plan->possible_goal_states_.size() < max_goal_count_ && !signal_stop_ &&
00345 samplePossibleGoalStates(plan, planning_scene_->getCurrentState(), min_distance, max_fail_));
00346
00347 plan->error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
00348
00349 return false;
00350 }
00351 }