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