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_);
}
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;
}