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