Exemple #1
0
void ompl_interface::OMPLInterface::configureContext(const ModelBasedPlanningContextPtr &context) const
{
  if (use_constraints_approximations_)
    context->setConstraintsApproximations(constraints_library_);
  else
    context->setConstraintsApproximations(ConstraintsLibraryPtr());
  context->simplifySolutions(simplify_solutions_);
}
Exemple #2
0
bool ompl_interface::OMPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                      const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanDetailedResponse &res) const
{
  moveit::tools::Profiler::ScopedStart pslv;
  moveit::tools::Profiler::ScopedBlock sblock("OMPLInterface:Solve");

  ModelBasedPlanningContextPtr context = getPlanningContext(planning_scene, req);
  if (context)
  {
    context->clear();
    return context->solve(res);
  }
  else
    return false;
}
ompl_interface::ConstraintApproximationConstructionResults
ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs::Constraints &constr_sampling, const moveit_msgs::Constraints &constr_hard,
                                                               const std::string &group, const std::string &state_space_parameterization,
                                                               const planning_scene::PlanningSceneConstPtr &scene, unsigned int samples, unsigned int edges_per_sample)
{ 
  ConstraintApproximationConstructionResults res;
  ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, state_space_parameterization);
  if (pc)
  {                                             
    pc->clear();
    pc->setPlanningScene(scene);
    pc->setCompleteInitialState(scene->getCurrentState());

    std::map<std::string, ConstraintApproximationFactoryPtr>::const_iterator it = constraint_factories_.find(constr_hard.name);
    ConstraintApproximationFactory *fct = NULL;
    ConstraintStateStorageOrderFn order;
    if (it != constraint_factories_.end())
    {
      fct = it->second.get();
      order = fct->getOrderFunction();
    }
    
    ros::WallTime start = ros::WallTime::now();
    ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, order, samples, edges_per_sample, res);
    logInform("Spent %lf seconds constructing the database", (ros::WallTime::now() - start).toSec());
    if (ss)
    {
      ConstraintApproximationPtr ca;
      if (fct)
        ca = fct->allocApproximation(context_manager_.getRobotModel(), group, state_space_parameterization, constr_hard, group + "_" + 
                                     boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + ".ompldb", ss);
      else
        ca.reset(new ConstraintApproximation(context_manager_.getRobotModel(), group, state_space_parameterization, constr_hard, group + "_" + 
                                             boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + ".ompldb", ss));
      if (ca)
      {
        if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end())
          logWarn("Overwriting constraint approximation named '%s'", ca->getName().c_str());
        constraint_approximations_[ca->getName()] = ca;
        res.approx = ca;
      }
    }
    else
      logError("Unable to construct constraint approximation for group '%s'", group.c_str());
  }
  return res;
}
ompl_interface::ConstraintApproximationConstructionResults
ompl_interface::ConstraintsLibrary::addConstraintApproximation(
    const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard,
    const std::string& group, const planning_scene::PlanningSceneConstPtr& scene,
    const ConstraintApproximationConstructionOptions& options)
{
  ConstraintApproximationConstructionResults res;
  ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, options.state_space_parameterization);
  if (pc)
  {
    pc->clear();
    pc->setPlanningScene(scene);
    pc->setCompleteInitialState(scene->getCurrentState());

    ros::WallTime start = ros::WallTime::now();
    ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, options, res);
    ROS_INFO_NAMED("constraints_library", "Spent %lf seconds constructing the database",
                   (ros::WallTime::now() - start).toSec());
    if (ss)
    {
      ConstraintApproximationPtr ca(new ConstraintApproximation(
          group, options.state_space_parameterization, options.explicit_motions, constr_hard,
          group + "_" + boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) +
              ".ompldb",
          ss, res.milestones));
      if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end())
        ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", ca->getName().c_str());
      constraint_approximations_[ca->getName()] = ca;
      res.approx = ca;
    }
    else
      ROS_ERROR_NAMED("constraints_library", "Unable to construct constraint approximation for group '%s'",
                      group.c_str());
  }
  return res;
}
ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstraintApproximation(
    const ModelBasedPlanningContextPtr& pcontext, const moveit_msgs::Constraints& constr_sampling,
    const moveit_msgs::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options,
    ConstraintApproximationConstructionResults& result)
{
  // state storage structure
  ConstraintApproximationStateStorage* cass = new ConstraintApproximationStateStorage(pcontext->getOMPLStateSpace());
  ob::StateStoragePtr sstor(cass);

  // construct a sampler for the sampling constraints
  kinematic_constraints::KinematicConstraintSet kset(pcontext->getRobotModel());
  robot_state::Transforms no_transforms(pcontext->getRobotModel()->getModelFrame());
  kset.add(constr_hard, no_transforms);

  const robot_state::RobotState& default_state = pcontext->getCompleteInitialRobotState();

  unsigned int attempts = 0;

  double bounds_val = std::numeric_limits<double>::max() / 2.0 - 1.0;
  pcontext->getOMPLStateSpace()->setPlanningVolume(-bounds_val, bounds_val, -bounds_val, bounds_val, -bounds_val,
                                                   bounds_val);
  pcontext->getOMPLStateSpace()->setup();

  // construct the constrained states

  robot_state::RobotState robot_state(default_state);
  const constraint_samplers::ConstraintSamplerManagerPtr& csmng = pcontext->getConstraintSamplerManager();
  ConstrainedSampler* csmp = nullptr;
  if (csmng)
  {
    constraint_samplers::ConstraintSamplerPtr cs =
        csmng->selectSampler(pcontext->getPlanningScene(), pcontext->getJointModelGroup()->getName(), constr_sampling);
    if (cs)
      csmp = new ConstrainedSampler(pcontext.get(), cs);
  }

  ob::StateSamplerPtr ss(csmp ? ob::StateSamplerPtr(csmp) : pcontext->getOMPLStateSpace()->allocDefaultStateSampler());

  ompl::base::ScopedState<> temp(pcontext->getOMPLStateSpace());
  int done = -1;
  bool slow_warn = false;
  ompl::time::point start = ompl::time::now();
  while (sstor->size() < options.samples)
  {
    ++attempts;
    int done_now = 100 * sstor->size() / options.samples;
    if (done != done_now)
    {
      done = done_now;
      ROS_INFO_NAMED("constraints_library", "%d%% complete (kept %0.1lf%% sampled states)", done,
                     100.0 * (double)sstor->size() / (double)attempts);
    }

    if (!slow_warn && attempts > 10 && attempts > sstor->size() * 100)
    {
      slow_warn = true;
      ROS_WARN_NAMED("constraints_library", "Computation of valid state database is very slow...");
    }

    if (attempts > options.samples && sstor->size() == 0)
    {
      ROS_ERROR_NAMED("constraints_library", "Unable to generate any samples");
      break;
    }

    ss->sampleUniform(temp.get());
    pcontext->getOMPLStateSpace()->copyToRobotState(robot_state, temp.get());
    if (kset.decide(robot_state).satisfied)
    {
      if (sstor->size() < options.samples)
      {
        temp->as<ModelBasedStateSpace::StateType>()->tag = sstor->size();
        sstor->addState(temp.get());
      }
    }
  }

  result.state_sampling_time = ompl::time::seconds(ompl::time::now() - start);
  ROS_INFO_NAMED("constraints_library", "Generated %u states in %lf seconds", (unsigned int)sstor->size(),
                 result.state_sampling_time);
  if (csmp)
  {
    result.sampling_success_rate = csmp->getConstrainedSamplingRate();
    ROS_INFO_NAMED("constraints_library", "Constrained sampling rate: %lf", result.sampling_success_rate);
  }

  result.milestones = sstor->size();
  if (options.edges_per_sample > 0)
  {
    ROS_INFO_NAMED("constraints_library", "Computing graph connections (max %u edges per sample) ...",
                   options.edges_per_sample);

    // construct connexions
    const ob::StateSpacePtr& space = pcontext->getOMPLSimpleSetup()->getStateSpace();
    unsigned int milestones = sstor->size();
    std::vector<ob::State*> int_states(options.max_explicit_points, nullptr);
    pcontext->getOMPLSimpleSetup()->getSpaceInformation()->allocStates(int_states);

    ompl::time::point start = ompl::time::now();
    int good = 0;
    int done = -1;

    for (std::size_t j = 0; j < milestones; ++j)
    {
      int done_now = 100 * j / milestones;
      if (done != done_now)
      {
        done = done_now;
        ROS_INFO_NAMED("constraints_library", "%d%% complete", done);
      }
      if (cass->getMetadata(j).first.size() >= options.edges_per_sample)
        continue;

      const ob::State* sj = sstor->getState(j);

      for (std::size_t i = j + 1; i < milestones; ++i)
      {
        if (cass->getMetadata(i).first.size() >= options.edges_per_sample)
          continue;
        double d = space->distance(sstor->getState(i), sj);
        if (d >= options.max_edge_length)
          continue;
        unsigned int isteps =
            std::min<unsigned int>(options.max_explicit_points, d / options.explicit_points_resolution);
        double step = 1.0 / (double)isteps;
        bool ok = true;
        space->interpolate(sstor->getState(i), sj, step, int_states[0]);
        for (unsigned int k = 1; k < isteps; ++k)
        {
          double this_step = step / (1.0 - (k - 1) * step);
          space->interpolate(int_states[k - 1], sj, this_step, int_states[k]);
          pcontext->getOMPLStateSpace()->copyToRobotState(robot_state, int_states[k]);
          if (!kset.decide(robot_state).satisfied)
          {
            ok = false;
            break;
          }
        }

        if (ok)
        {
          cass->getMetadata(i).first.push_back(j);
          cass->getMetadata(j).first.push_back(i);

          if (options.explicit_motions)
          {
            cass->getMetadata(i).second[j].first = sstor->size();
            for (unsigned int k = 0; k < isteps; ++k)
            {
              int_states[k]->as<ModelBasedStateSpace::StateType>()->tag = -1;
              sstor->addState(int_states[k]);
            }
            cass->getMetadata(i).second[j].second = sstor->size();
            cass->getMetadata(j).second[i] = cass->getMetadata(i).second[j];
          }

          good++;
          if (cass->getMetadata(j).first.size() >= options.edges_per_sample)
            break;
        }
      }
    }

    result.state_connection_time = ompl::time::seconds(ompl::time::now() - start);
    ROS_INFO_NAMED("constraints_library", "Computed possible connexions in %lf seconds. Added %d connexions",
                   result.state_connection_time, good);
    pcontext->getOMPLSimpleSetup()->getSpaceInformation()->freeStates(int_states);

    return sstor;
  }

  // TODO(davetcoleman): this function did not originally return a value, causing compiler warnings in ROS Melodic
  // Update with more intelligent logic as needed
  ROS_ERROR_NAMED("constraints_library", "No StateStoragePtr found - implement better solution here.");
  return sstor;
}
ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstraintApproximation(const ModelBasedPlanningContextPtr &pcontext,
                                                                                                 const moveit_msgs::Constraints &constr_sampling,
                                                                                                 const moveit_msgs::Constraints &constr_hard,
                                                                                                 const ConstraintStateStorageOrderFn &order,
                                                                                                 unsigned int samples, unsigned int edges_per_sample,
                                                                                                 ConstraintApproximationConstructionResults &result)
{
  // state storage structure
  ConstraintApproximationStateStorage *cass = new ConstraintApproximationStateStorage(pcontext->getOMPLStateSpace());
  ob::StateStoragePtr sstor(cass);
  
  // construct a sampler for the sampling constraints
  kinematic_constraints::KinematicConstraintSet kset(pcontext->getRobotModel(), robot_state::TransformsConstPtr(new robot_state::Transforms(pcontext->getRobotModel()->getModelFrame())));
  kset.add(constr_hard);

  const robot_state::RobotState &default_state = pcontext->getCompleteInitialRobotState();
  
  int nthreads = 0;
  unsigned int attempts = 0;
  
  double bounds_val = std::numeric_limits<double>::max() / 2.0 - 1.0;
  pcontext->getOMPLStateSpace()->setPlanningVolume(-bounds_val, bounds_val, -bounds_val, bounds_val, -bounds_val, bounds_val);
  pcontext->getOMPLStateSpace()->setup();
  
  // construct the constrained states
#pragma omp parallel
  { 
#pragma omp master
    {
	nthreads = omp_get_num_threads();    
    }
    
    robot_state::RobotState kstate(default_state);
    const constraint_samplers::ConstraintSamplerManagerPtr &csmng = pcontext->getConstraintSamplerManager();
    ConstrainedSampler *csmp = NULL;
    if (csmng)
    {
      constraint_samplers::ConstraintSamplerPtr cs = csmng->selectSampler(pcontext->getPlanningScene(), pcontext->getJointModelGroup()->getName(), constr_sampling);
      if (cs)
        csmp = new ConstrainedSampler(pcontext.get(), cs);
    }
    
    ob::StateSamplerPtr ss(csmp ? ob::StateSamplerPtr(csmp) : pcontext->getOMPLStateSpace()->allocDefaultStateSampler());
    
    ompl::base::ScopedState<> temp(pcontext->getOMPLStateSpace());
    int done = -1;
    bool slow_warn = false;
    ompl::time::point start = ompl::time::now();
    while (sstor->size() < samples)
    {
      ++attempts;
#pragma omp master
      {      
	int done_now = 100 * sstor->size() / samples;
	if (done != done_now)
	{
	  done = done_now;
	  logInform("%d%% complete (kept %0.1lf%% sampled states)", done, 100.0 * (double)sstor->size() / (double)attempts);
	}

	if (!slow_warn && attempts > 10 && attempts > sstor->size() * 100)
	{
	  slow_warn = true;
	  logWarn("Computation of valid state database is very slow...");
	}
      }

      if (attempts > samples && sstor->size() == 0)
      {
	logError("Unable to generate any samples");
	break;
      }
      
      ss->sampleUniform(temp.get());
      pcontext->getOMPLStateSpace()->copyToRobotState(kstate, temp.get());
      if (kset.decide(kstate).satisfied)
      {
#pragma omp critical
	{
	  if (sstor->size() < samples)
	  {
	    temp->as<ModelBasedStateSpace::StateType>()->tag = sstor->size();
	    sstor->addState(temp.get());
	  }
	}
      }
    }
#pragma omp master
    {
      result.state_sampling_time = ompl::time::seconds(ompl::time::now() - start);
      logInform("Generated %u states in %lf seconds", (unsigned int)sstor->size(), result.state_sampling_time); 
      if (csmp)
      {
        result.sampling_success_rate = csmp->getConstrainedSamplingRate();
        logInform("Constrained sampling rate: %lf", result.sampling_success_rate);
      }
    }
  }
  if (order)
  {
    logInform("Sorting states...");
    sstor->sort(order);
  }
  
  if (edges_per_sample > 0)
  {
    logInform("Computing graph connections (max %u edges per sample) ...", edges_per_sample);
    
    ompl::tools::SelfConfig sc(pcontext->getOMPLSimpleSetup().getSpaceInformation());
    double range = 0.0;
    sc.configurePlannerRange(range);
    
    // construct connexions
    const ob::StateSpacePtr &space = pcontext->getOMPLSimpleSetup().getStateSpace();
    std::vector<robot_state::RobotState> kstates(nthreads, default_state);
    const std::vector<const ompl::base::State*> &states = sstor->getStates();
    std::vector<ompl::base::ScopedState<> > temps(nthreads, ompl::base::ScopedState<>(space));
    
    ompl::time::point start = ompl::time::now();
    int good = 0;
    int done = -1;
    
#pragma omp parallel for schedule(dynamic) 
    for (std::size_t j = 0 ; j < sstor->size() ; ++j)
    {
      int threadid = omp_get_thread_num();
      robot_state::RobotState &kstate = kstates[threadid];
      robot_state::JointStateGroup *jsg = kstate.getJointStateGroup(pcontext->getJointModelGroup()->getName());
      ompl::base::State *temp = temps[threadid].get();
      int done_now = 100 * j / sstor->size();
      if (done != done_now)
      {
	done = done_now;
        logInform("%d%% complete", done);
      }
      
      for (std::size_t i = j + 1 ; i < sstor->size() ; ++i)
      {
	double d = space->distance(states[j], states[i]);

        if (d > range * 3.0 || d < range / 100.0)
          continue;
        
        space->interpolate(states[j], states[i], 0.5, temp);
        pcontext->getOMPLStateSpace()->copyToRobotState(kstate, temp);
        if (kset.decide(kstate).satisfied)
        {
	  space->interpolate(states[j], states[i], 0.25, temp);
	  pcontext->getOMPLStateSpace()->copyToRobotState(kstate, temp);
	  if (kset.decide(kstate).satisfied)
	  {
            space->interpolate(states[j], states[i], 0.75, temp);
            pcontext->getOMPLStateSpace()->copyToRobotState(kstate, temp);
            if (kset.decide(kstate).satisfied)
            {
#pragma omp critical
              {
                cass->getMetadata(i).push_back(j);
                cass->getMetadata(j).push_back(i);
                good++;
              }
              if (cass->getMetadata(j).size() >= edges_per_sample)
                break;
            }
	  }
        }
      }
    }
    result.state_connection_time = ompl::time::seconds(ompl::time::now() - start);
    logInform("Computed possible connexions in %lf seconds. Added %d connexions", result.state_connection_time, good);
  }
  
  return sstor;
}