model_based_planning_context.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/ompl_interface/model_based_planning_context.h>
00038 #include <moveit/ompl_interface/detail/state_validity_checker.h>
00039 #include <moveit/ompl_interface/detail/constrained_sampler.h>
00040 #include <moveit/ompl_interface/detail/constrained_goal_sampler.h>
00041 #include <moveit/ompl_interface/detail/goal_union.h>
00042 #include <moveit/ompl_interface/detail/projection_evaluators.h>
00043 #include <moveit/ompl_interface/constraints_library.h>
00044 #include <moveit/kinematic_constraints/utils.h>
00045 #include <moveit/profiler/profiler.h>
00046 #include <eigen_conversions/eigen_msg.h>
00047 
00048 #include <ompl/base/samplers/UniformValidStateSampler.h>
00049 #include <ompl/base/goals/GoalLazySamples.h>
00050 #include <ompl/tools/config/SelfConfig.h>
00051 #include <ompl/base/spaces/SE3StateSpace.h>
00052 #include <ompl/datastructures/PDF.h>
00053 
00054 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
00055 #include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h"
00056 #include "ompl/base/objectives/MinimaxObjective.h"
00057 #include "ompl/base/objectives/StateCostIntegralObjective.h"
00058 #include "ompl/base/objectives/MaximizeMinClearanceObjective.h"
00059 
00060 ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std::string& name,
00061                                                                      const ModelBasedPlanningContextSpecification& spec)
00062   : planning_interface::PlanningContext(name, spec.state_space_->getJointModelGroup()->getName())
00063   , spec_(spec)
00064   , complete_initial_robot_state_(spec.state_space_->getRobotModel())
00065   , ompl_simple_setup_(spec.ompl_simple_setup_)
00066   , ompl_benchmark_(*ompl_simple_setup_)
00067   , ompl_parallel_plan_(ompl_simple_setup_->getProblemDefinition())
00068   , ptc_(NULL)
00069   , last_plan_time_(0.0)
00070   , last_simplify_time_(0.0)
00071   , max_goal_samples_(0)
00072   , max_state_sampling_attempts_(0)
00073   , max_goal_sampling_attempts_(0)
00074   , max_planning_threads_(0)
00075   , max_solution_segment_length_(0.0)
00076   , minimum_waypoint_count_(0)
00077   , use_state_validity_cache_(true)
00078   , simplify_solutions_(true)
00079 {
00080   complete_initial_robot_state_.update();
00081   ompl_simple_setup_->getStateSpace()->computeSignature(space_signature_);
00082   ompl_simple_setup_->getStateSpace()->setStateSamplerAllocator(
00083       boost::bind(&ModelBasedPlanningContext::allocPathConstrainedSampler, this, _1));
00084 }
00085 
00086 void ompl_interface::ModelBasedPlanningContext::setProjectionEvaluator(const std::string& peval)
00087 {
00088   if (!spec_.state_space_)
00089   {
00090     logError("No state space is configured yet");
00091     return;
00092   }
00093   ob::ProjectionEvaluatorPtr pe = getProjectionEvaluator(peval);
00094   if (pe)
00095     spec_.state_space_->registerDefaultProjection(pe);
00096 }
00097 
00098 ompl::base::ProjectionEvaluatorPtr
00099 ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::string& peval) const
00100 {
00101   if (peval.find_first_of("link(") == 0 && peval[peval.length() - 1] == ')')
00102   {
00103     std::string link_name = peval.substr(5, peval.length() - 6);
00104     if (getRobotModel()->hasLinkModel(link_name))
00105       return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorLinkPose(this, link_name));
00106     else
00107       logError("Attempted to set projection evaluator with respect to position of link '%s', but that link is not "
00108                "known to the kinematic model.",
00109                link_name.c_str());
00110   }
00111   else if (peval.find_first_of("joints(") == 0 && peval[peval.length() - 1] == ')')
00112   {
00113     std::string joints = peval.substr(7, peval.length() - 8);
00114     boost::replace_all(joints, ",", " ");
00115     std::vector<unsigned int> j;
00116     std::stringstream ss(joints);
00117     while (ss.good() && !ss.eof())
00118     {
00119       std::string v;
00120       ss >> v >> std::ws;
00121       if (getJointModelGroup()->hasJointModel(v))
00122       {
00123         unsigned int vc = getJointModelGroup()->getJointModel(v)->getVariableCount();
00124         if (vc > 0)
00125         {
00126           int idx = getJointModelGroup()->getVariableGroupIndex(v);
00127           for (int q = 0; q < vc; ++q)
00128             j.push_back(idx + q);
00129         }
00130         else
00131           logWarn("%s: Ignoring joint '%s' in projection since it has 0 DOF", name_.c_str(), v.c_str());
00132       }
00133       else
00134         logError("%s: Attempted to set projection evaluator with respect to value of joint '%s', but that joint is not "
00135                  "known to the group '%s'.",
00136                  name_.c_str(), v.c_str(), getGroupName().c_str());
00137     }
00138     if (j.empty())
00139       logError("%s: No valid joints specified for joint projection", name_.c_str());
00140     else
00141       return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorJointValue(this, j));
00142   }
00143   else
00144     logError("Unable to allocate projection evaluator based on description: '%s'", peval.c_str());
00145   return ob::ProjectionEvaluatorPtr();
00146 }
00147 
00148 ompl::base::StateSamplerPtr
00149 ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const ompl::base::StateSpace* ss) const
00150 {
00151   if (spec_.state_space_.get() != ss)
00152   {
00153     logError("%s: Attempted to allocate a state sampler for an unknown state space", name_.c_str());
00154     return ompl::base::StateSamplerPtr();
00155   }
00156 
00157   logDebug("%s: Allocating a new state sampler (attempts to use path constraints)", name_.c_str());
00158 
00159   if (path_constraints_)
00160   {
00161     if (spec_.constraints_library_)
00162     {
00163       const ConstraintApproximationPtr& ca =
00164           spec_.constraints_library_->getConstraintApproximation(path_constraints_msg_);
00165       if (ca)
00166       {
00167         ompl::base::StateSamplerAllocator c_ssa = ca->getStateSamplerAllocator(path_constraints_msg_);
00168         if (c_ssa)
00169         {
00170           ompl::base::StateSamplerPtr res = c_ssa(ss);
00171           if (res)
00172           {
00173             logInform("%s: Using precomputed state sampler (approximated constraint space) for constraint '%s'",
00174                       name_.c_str(), path_constraints_msg_.name.c_str());
00175             return res;
00176           }
00177         }
00178       }
00179     }
00180 
00181     constraint_samplers::ConstraintSamplerPtr cs;
00182     if (spec_.constraint_sampler_manager_)
00183       cs = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(),
00184                                                             path_constraints_->getAllConstraints());
00185 
00186     if (cs)
00187     {
00188       logInform("%s: Allocating specialized state sampler for state space", name_.c_str());
00189       return ob::StateSamplerPtr(new ConstrainedSampler(this, cs));
00190     }
00191   }
00192   logDebug("%s: Allocating default state sampler for state space", name_.c_str());
00193   return ss->allocDefaultStateSampler();
00194 }
00195 
00196 void ompl_interface::ModelBasedPlanningContext::configure()
00197 {
00198   // convert the input state to the corresponding OMPL state
00199   ompl::base::ScopedState<> ompl_start_state(spec_.state_space_);
00200   spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState());
00201   ompl_simple_setup_->setStartState(ompl_start_state);
00202   ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr(new StateValidityChecker(this)));
00203 
00204   if (path_constraints_ && spec_.constraints_library_)
00205   {
00206     const ConstraintApproximationPtr& ca =
00207         spec_.constraints_library_->getConstraintApproximation(path_constraints_msg_);
00208     if (ca)
00209     {
00210       getOMPLStateSpace()->setInterpolationFunction(ca->getInterpolationFunction());
00211       logInform("Using precomputed interpolation states");
00212     }
00213   }
00214 
00215   useConfig();
00216   if (ompl_simple_setup_->getGoal())
00217     ompl_simple_setup_->setup();
00218 }
00219 
00220 void ompl_interface::ModelBasedPlanningContext::useConfig()
00221 {
00222   const std::map<std::string, std::string>& config = spec_.config_;
00223   if (config.empty())
00224     return;
00225   std::map<std::string, std::string> cfg = config;
00226 
00227   // set the projection evaluator
00228   std::map<std::string, std::string>::iterator it = cfg.find("projection_evaluator");
00229   if (it != cfg.end())
00230   {
00231     setProjectionEvaluator(boost::trim_copy(it->second));
00232     cfg.erase(it);
00233   }
00234 
00235   if (cfg.empty())
00236     return;
00237 
00238   std::string optimizer;
00239   ompl::base::OptimizationObjectivePtr objective;
00240   it = cfg.find("optimization_objective");
00241   if (it == cfg.end())
00242   {
00243     optimizer = "PathLengthOptimizationObjective";
00244     logInform("No optimization objective specified, defaulting to %s", optimizer.c_str());
00245   }
00246   else
00247   {
00248     optimizer = it->second;
00249     cfg.erase(it);
00250   }
00251 
00252   if (optimizer == "PathLengthOptimizationObjective")
00253   {
00254     objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation()));
00255   }
00256   else if (optimizer == "MinimaxObjective")
00257   {
00258     objective.reset(new ompl::base::MinimaxObjective(ompl_simple_setup_->getSpaceInformation()));
00259   }
00260   else if (optimizer == "StateCostIntegralObjective")
00261   {
00262     objective.reset(new ompl::base::StateCostIntegralObjective(ompl_simple_setup_->getSpaceInformation()));
00263   }
00264   else if (optimizer == "MechanicalWorkOptimizationObjective")
00265   {
00266     objective.reset(new ompl::base::MechanicalWorkOptimizationObjective(ompl_simple_setup_->getSpaceInformation()));
00267   }
00268   else if (optimizer == "MaximizeMinClearanceObjective")
00269   {
00270     objective.reset(new ompl::base::MaximizeMinClearanceObjective(ompl_simple_setup_->getSpaceInformation()));
00271   }
00272   else
00273   {
00274     objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation()));
00275   }
00276 
00277   ompl_simple_setup_->setOptimizationObjective(objective);
00278 
00279   // remove the 'type' parameter; the rest are parameters for the planner itself
00280   it = cfg.find("type");
00281   if (it == cfg.end())
00282   {
00283     if (name_ != getGroupName())
00284       logWarn("%s: Attribute 'type' not specified in planner configuration", name_.c_str());
00285   }
00286   else
00287   {
00288     std::string type = it->second;
00289     cfg.erase(it);
00290     ompl_simple_setup_->setPlannerAllocator(
00291         boost::bind(spec_.planner_selector_(type), _1, name_ != getGroupName() ? name_ : "", spec_));
00292     logInform("Planner configuration '%s' will use planner '%s'. Additional configuration parameters will be set when "
00293               "the planner is constructed.",
00294               name_.c_str(), type.c_str());
00295   }
00296 
00297   // call the setParams() after setup(), so we know what the params are
00298   ompl_simple_setup_->getSpaceInformation()->setup();
00299   ompl_simple_setup_->getSpaceInformation()->params().setParams(cfg, true);
00300   // call setup() again for possibly new param values
00301   ompl_simple_setup_->getSpaceInformation()->setup();
00302 }
00303 
00304 void ompl_interface::ModelBasedPlanningContext::setPlanningVolume(const moveit_msgs::WorkspaceParameters& wparams)
00305 {
00306   if (wparams.min_corner.x == wparams.max_corner.x && wparams.min_corner.x == 0.0 &&
00307       wparams.min_corner.y == wparams.max_corner.y && wparams.min_corner.y == 0.0 &&
00308       wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0)
00309     logWarn("It looks like the planning volume was not specified.");
00310 
00311   logDebug("%s: Setting planning volume (affects SE2 & SE3 joints only) to x = [%f, %f], y = [%f, %f], z = [%f, %f]",
00312            name_.c_str(), wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y, wparams.max_corner.y,
00313            wparams.min_corner.z, wparams.max_corner.z);
00314 
00315   spec_.state_space_->setPlanningVolume(wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y,
00316                                         wparams.max_corner.y, wparams.min_corner.z, wparams.max_corner.z);
00317 }
00318 
00319 void ompl_interface::ModelBasedPlanningContext::simplifySolution(double timeout)
00320 {
00321   ompl_simple_setup_->simplifySolution(timeout);
00322   last_simplify_time_ = ompl_simple_setup_->getLastSimplificationTime();
00323 }
00324 
00325 void ompl_interface::ModelBasedPlanningContext::interpolateSolution()
00326 {
00327   if (ompl_simple_setup_->haveSolutionPath())
00328   {
00329     og::PathGeometric& pg = ompl_simple_setup_->getSolutionPath();
00330     pg.interpolate(
00331         std::max((unsigned int)floor(0.5 + pg.length() / max_solution_segment_length_), minimum_waypoint_count_));
00332   }
00333 }
00334 
00335 void ompl_interface::ModelBasedPlanningContext::convertPath(const ompl::geometric::PathGeometric& pg,
00336                                                             robot_trajectory::RobotTrajectory& traj) const
00337 {
00338   robot_state::RobotState ks = complete_initial_robot_state_;
00339   for (std::size_t i = 0; i < pg.getStateCount(); ++i)
00340   {
00341     spec_.state_space_->copyToRobotState(ks, pg.getState(i));
00342     traj.addSuffixWayPoint(ks, 0.0);
00343   }
00344 }
00345 
00346 bool ompl_interface::ModelBasedPlanningContext::getSolutionPath(robot_trajectory::RobotTrajectory& traj) const
00347 {
00348   traj.clear();
00349   if (!ompl_simple_setup_->haveSolutionPath())
00350     return false;
00351   convertPath(ompl_simple_setup_->getSolutionPath(), traj);
00352   return true;
00353 }
00354 
00355 void ompl_interface::ModelBasedPlanningContext::setVerboseStateValidityChecks(bool flag)
00356 {
00357   if (ompl_simple_setup_->getStateValidityChecker())
00358     static_cast<StateValidityChecker*>(ompl_simple_setup_->getStateValidityChecker().get())->setVerbose(flag);
00359 }
00360 
00361 ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal()
00362 {
00363   // ******************* set up the goal representation, based on goal constraints
00364 
00365   std::vector<ob::GoalPtr> goals;
00366   for (std::size_t i = 0; i < goal_constraints_.size(); ++i)
00367   {
00368     constraint_samplers::ConstraintSamplerPtr cs;
00369     if (spec_.constraint_sampler_manager_)
00370       cs = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(),
00371                                                             goal_constraints_[i]->getAllConstraints());
00372     if (cs)
00373     {
00374       ob::GoalPtr g = ob::GoalPtr(new ConstrainedGoalSampler(this, goal_constraints_[i], cs));
00375       goals.push_back(g);
00376     }
00377   }
00378 
00379   if (!goals.empty())
00380     return goals.size() == 1 ? goals[0] : ompl::base::GoalPtr(new GoalSampleableRegionMux(goals));
00381   else
00382     logError("Unable to construct goal representation");
00383 
00384   return ob::GoalPtr();
00385 }
00386 
00387 void ompl_interface::ModelBasedPlanningContext::setCompleteInitialState(
00388     const robot_state::RobotState& complete_initial_robot_state)
00389 {
00390   complete_initial_robot_state_ = complete_initial_robot_state;
00391   complete_initial_robot_state_.update();
00392 }
00393 
00394 void ompl_interface::ModelBasedPlanningContext::clear()
00395 {
00396   ompl_simple_setup_->clear();
00397   ompl_simple_setup_->clearStartStates();
00398   ompl_simple_setup_->setGoal(ob::GoalPtr());
00399   ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr());
00400   path_constraints_.reset();
00401   goal_constraints_.clear();
00402   getOMPLStateSpace()->setInterpolationFunction(InterpolationFunction());
00403 }
00404 
00405 bool ompl_interface::ModelBasedPlanningContext::setPathConstraints(const moveit_msgs::Constraints& path_constraints,
00406                                                                    moveit_msgs::MoveItErrorCodes* error)
00407 {
00408   // ******************* set the path constraints to use
00409   path_constraints_.reset(new kinematic_constraints::KinematicConstraintSet(getRobotModel()));
00410   path_constraints_->add(path_constraints, getPlanningScene()->getTransforms());
00411   path_constraints_msg_ = path_constraints;
00412 
00413   return true;
00414 }
00415 
00416 bool ompl_interface::ModelBasedPlanningContext::setGoalConstraints(
00417     const std::vector<moveit_msgs::Constraints>& goal_constraints, const moveit_msgs::Constraints& path_constraints,
00418     moveit_msgs::MoveItErrorCodes* error)
00419 {
00420   // ******************* check if the input is correct
00421   goal_constraints_.clear();
00422   for (std::size_t i = 0; i < goal_constraints.size(); ++i)
00423   {
00424     moveit_msgs::Constraints constr = kinematic_constraints::mergeConstraints(goal_constraints[i], path_constraints);
00425     kinematic_constraints::KinematicConstraintSetPtr kset(
00426         new kinematic_constraints::KinematicConstraintSet(getRobotModel()));
00427     kset->add(constr, getPlanningScene()->getTransforms());
00428     if (!kset->empty())
00429       goal_constraints_.push_back(kset);
00430   }
00431 
00432   if (goal_constraints_.empty())
00433   {
00434     logWarn("%s: No goal constraints specified. There is no problem to solve.", name_.c_str());
00435     if (error)
00436       error->val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
00437     return false;
00438   }
00439 
00440   ob::GoalPtr goal = constructGoal();
00441   ompl_simple_setup_->setGoal(goal);
00442   if (goal)
00443     return true;
00444   else
00445     return false;
00446 }
00447 
00448 bool ompl_interface::ModelBasedPlanningContext::benchmark(double timeout, unsigned int count,
00449                                                           const std::string& filename)
00450 {
00451   ompl_benchmark_.clearPlanners();
00452   ompl_simple_setup_->setup();
00453   ompl_benchmark_.addPlanner(ompl_simple_setup_->getPlanner());
00454   ompl_benchmark_.setExperimentName(getRobotModel()->getName() + "_" + getGroupName() + "_" +
00455                                     getPlanningScene()->getName() + "_" + name_);
00456 
00457   ot::Benchmark::Request req;
00458   req.maxTime = timeout;
00459   req.runCount = count;
00460   req.displayProgress = true;
00461   req.saveConsoleOutput = false;
00462   ompl_benchmark_.benchmark(req);
00463   return filename.empty() ? ompl_benchmark_.saveResultsToFile() : ompl_benchmark_.saveResultsToFile(filename.c_str());
00464 }
00465 
00466 void ompl_interface::ModelBasedPlanningContext::startSampling()
00467 {
00468   bool gls = ompl_simple_setup_->getGoal()->hasType(ob::GOAL_LAZY_SAMPLES);
00469   if (gls)
00470     static_cast<ob::GoalLazySamples*>(ompl_simple_setup_->getGoal().get())->startSampling();
00471   else
00472     // we know this is a GoalSampleableMux by elimination
00473     static_cast<GoalSampleableRegionMux*>(ompl_simple_setup_->getGoal().get())->startSampling();
00474 }
00475 
00476 void ompl_interface::ModelBasedPlanningContext::stopSampling()
00477 {
00478   bool gls = ompl_simple_setup_->getGoal()->hasType(ob::GOAL_LAZY_SAMPLES);
00479   if (gls)
00480     static_cast<ob::GoalLazySamples*>(ompl_simple_setup_->getGoal().get())->stopSampling();
00481   else
00482     // we know this is a GoalSampleableMux by elimination
00483     static_cast<GoalSampleableRegionMux*>(ompl_simple_setup_->getGoal().get())->stopSampling();
00484 }
00485 
00486 void ompl_interface::ModelBasedPlanningContext::preSolve()
00487 {
00488   // clear previously computed solutions
00489   ompl_simple_setup_->getProblemDefinition()->clearSolutionPaths();
00490   const ob::PlannerPtr planner = ompl_simple_setup_->getPlanner();
00491   if (planner)
00492     planner->clear();
00493   startSampling();
00494   ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
00495 }
00496 
00497 void ompl_interface::ModelBasedPlanningContext::postSolve()
00498 {
00499   stopSampling();
00500   int v = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getValidMotionCount();
00501   int iv = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getInvalidMotionCount();
00502   logDebug("There were %d valid motions and %d invalid motions.", v, iv);
00503 
00504   if (ompl_simple_setup_->getProblemDefinition()->hasApproximateSolution())
00505     logWarn("Computed solution is approximate");
00506 }
00507 
00508 bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res)
00509 {
00510   if (solve(request_.allowed_planning_time, request_.num_planning_attempts))
00511   {
00512     double ptime = getLastPlanTime();
00513     if (simplify_solutions_ && ptime < request_.allowed_planning_time)
00514     {
00515       simplifySolution(request_.allowed_planning_time - ptime);
00516       ptime += getLastSimplifyTime();
00517     }
00518     interpolateSolution();
00519 
00520     // fill the response
00521     logDebug("%s: Returning successful solution with %lu states", getName().c_str(),
00522              getOMPLSimpleSetup()->getSolutionPath().getStateCount());
00523 
00524     res.trajectory_.reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName()));
00525     getSolutionPath(*res.trajectory_);
00526     res.planning_time_ = ptime;
00527     return true;
00528   }
00529   else
00530   {
00531     logInform("Unable to solve the planning problem");
00532     res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
00533     return false;
00534   }
00535 }
00536 
00537 bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res)
00538 {
00539   if (solve(request_.allowed_planning_time, request_.num_planning_attempts))
00540   {
00541     res.trajectory_.reserve(3);
00542 
00543     // add info about planned solution
00544     double ptime = getLastPlanTime();
00545     res.processing_time_.push_back(ptime);
00546     res.description_.push_back("plan");
00547     res.trajectory_.resize(res.trajectory_.size() + 1);
00548     res.trajectory_.back().reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName()));
00549     getSolutionPath(*res.trajectory_.back());
00550 
00551     // simplify solution if time remains
00552     if (simplify_solutions_ && ptime < request_.allowed_planning_time)
00553     {
00554       simplifySolution(request_.allowed_planning_time - ptime);
00555       res.processing_time_.push_back(getLastSimplifyTime());
00556       res.description_.push_back("simplify");
00557       res.trajectory_.resize(res.trajectory_.size() + 1);
00558       res.trajectory_.back().reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName()));
00559       getSolutionPath(*res.trajectory_.back());
00560     }
00561 
00562     ompl::time::point start_interpolate = ompl::time::now();
00563     interpolateSolution();
00564     res.processing_time_.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate));
00565     res.description_.push_back("interpolate");
00566     res.trajectory_.resize(res.trajectory_.size() + 1);
00567     res.trajectory_.back().reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName()));
00568     getSolutionPath(*res.trajectory_.back());
00569 
00570     // fill the response
00571     logDebug("%s: Returning successful solution with %lu states", getName().c_str(),
00572              getOMPLSimpleSetup()->getSolutionPath().getStateCount());
00573     return true;
00574   }
00575   else
00576   {
00577     logInform("Unable to solve the planning problem");
00578     res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
00579     return false;
00580   }
00581 }
00582 
00583 bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned int count)
00584 {
00585   moveit::tools::Profiler::ScopedBlock sblock("PlanningContext:Solve");
00586   ompl::time::point start = ompl::time::now();
00587   preSolve();
00588 
00589   bool result = false;
00590   if (count <= 1)
00591   {
00592     logDebug("%s: Solving the planning problem once...", name_.c_str());
00593     ob::PlannerTerminationCondition ptc =
00594         ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
00595     registerTerminationCondition(ptc);
00596     result = ompl_simple_setup_->solve(ptc) == ompl::base::PlannerStatus::EXACT_SOLUTION;
00597     last_plan_time_ = ompl_simple_setup_->getLastPlanComputationTime();
00598     unregisterTerminationCondition();
00599   }
00600   else
00601   {
00602     logDebug("%s: Solving the planning problem %u times...", name_.c_str(), count);
00603     ompl_parallel_plan_.clearHybridizationPaths();
00604     if (count <= max_planning_threads_)
00605     {
00606       ompl_parallel_plan_.clearPlanners();
00607       if (ompl_simple_setup_->getPlannerAllocator())
00608         for (unsigned int i = 0; i < count; ++i)
00609           ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator());
00610       else
00611         for (unsigned int i = 0; i < count; ++i)
00612           ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal()));
00613 
00614       ob::PlannerTerminationCondition ptc =
00615           ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
00616       registerTerminationCondition(ptc);
00617       result = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
00618       last_plan_time_ = ompl::time::seconds(ompl::time::now() - start);
00619       unregisterTerminationCondition();
00620     }
00621     else
00622     {
00623       ob::PlannerTerminationCondition ptc =
00624           ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
00625       registerTerminationCondition(ptc);
00626       int n = count / max_planning_threads_;
00627       result = true;
00628       for (int i = 0; i < n && ptc() == false; ++i)
00629       {
00630         ompl_parallel_plan_.clearPlanners();
00631         if (ompl_simple_setup_->getPlannerAllocator())
00632           for (unsigned int i = 0; i < max_planning_threads_; ++i)
00633             ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator());
00634         else
00635           for (unsigned int i = 0; i < max_planning_threads_; ++i)
00636             ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal()));
00637         bool r = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
00638         result = result && r;
00639       }
00640       n = count % max_planning_threads_;
00641       if (n && ptc() == false)
00642       {
00643         ompl_parallel_plan_.clearPlanners();
00644         if (ompl_simple_setup_->getPlannerAllocator())
00645           for (int i = 0; i < n; ++i)
00646             ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator());
00647         else
00648           for (int i = 0; i < n; ++i)
00649             ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal()));
00650         bool r = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
00651         result = result && r;
00652       }
00653       last_plan_time_ = ompl::time::seconds(ompl::time::now() - start);
00654       unregisterTerminationCondition();
00655     }
00656   }
00657 
00658   postSolve();
00659 
00660   return result;
00661 }
00662 
00663 void ompl_interface::ModelBasedPlanningContext::registerTerminationCondition(const ob::PlannerTerminationCondition& ptc)
00664 {
00665   boost::mutex::scoped_lock slock(ptc_lock_);
00666   ptc_ = &ptc;
00667 }
00668 
00669 void ompl_interface::ModelBasedPlanningContext::unregisterTerminationCondition()
00670 {
00671   boost::mutex::scoped_lock slock(ptc_lock_);
00672   ptc_ = NULL;
00673 }
00674 
00675 bool ompl_interface::ModelBasedPlanningContext::terminate()
00676 {
00677   boost::mutex::scoped_lock slock(ptc_lock_);
00678   if (ptc_)
00679     ptc_->terminate();
00680   return true;
00681 }


ompl
Author(s): Ioan Sucan
autogenerated on Wed Jan 17 2018 03:32:20