// create the BspCurv representation of the basis Func
BspCurv<double> BspCurvBasisFunc::CreateBspCurv() const
{
int dim = ComputeDim();
Vector<double> cpts(dim,0.0);
// create KnotSet object
KnotSet kset(*kts,ord,ord+1);
int num = kset.GetNumDistinct();
// create temporary Vectors storing multiplicities and distinct knots
Vector<double> dts(kset.GetDistinctKnots());
Vector<int> mult(kset.GetMult());
// std::set multiplicities at the end equal to ord
mult[0]=ord;
mult[num-1]=ord;
// compute offstd::set
int start=kset.GetMult()[0];
// create KnotSet object for curve
KnotSet kset1(dts,mult,ord,num);
// assign 1.0 to approriate control point
cpts[ord-start]=1.0;
// create and return curve
return BspCurv<double>(cpts,kset1.GetKnots(),ord,dim);
}
// create the BezCurv representation of the basis function
BezCurv<double> BezCurvBasisFunc::CreateBezCurv() const
{
Vector<double> cpts(ord,0.0);
// create the knot set
KnotSet kset(*kts,ord,ord+1);
int num = kset.GetNumDistinct();
// create Vectors of distinct knots and multiplicities
Vector<double> dts(kset.GetDistinctKnots());
Vector<int> mult(kset.GetMult());
// set multiplicity to be ord at two ends
mult[0]=ord;
mult[num-1]=ord;
// find offset
int start=kset.GetMult()[0];
// create knot set for curve
KnotSet kset1(dts,mult,ord,num);
// assign 1.0 to appropriate control point (rest=0.0)
cpts[ord-start]=1.0;
// create and return the BezCurv
return BezCurv<double>(cpts,kset1.GetKnots(),ord);
}
// compute the dimension of the basis Func knot std::set
int BspCurvBasisFunc::ComputeDim() const
{
// what about the case when num = 1?
// create KnotSet object
KnotSet kset(*kts,ord,ord+1);
int num = kset.GetNumDistinct();
int num_cond=0;
// compute dimension
for (int i=1; i<num-1; i++) num_cond = num_cond + ord - kset.GetMult()[i];
return ((num-1)*ord-num_cond);
}
void BiEncoder<Key>::compile(){
if (decodemap.size() == 0){return;}
Key unk_value = decodemap[0];
set<Key> kset(decodemap.begin(),decodemap.end());
this->decodemap.assign(kset.begin(),kset.end());
if (has_unk){
for(int i = 0; i < this->decodemap.size();++i){
if (this->decodemap[i] == unk_value){
this->decodemap[i] = this->decodemap[0];
this->decodemap[0] = unk_value;
}
}
}
for(int i = 0; i < decodemap.size();++i){
codemap[decodemap[i]] = i;
}
is_checked = true;
}
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;
}
TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler)
{
moveit_msgs::Constraints c;
moveit_msgs::PositionConstraint pcm;
pcm.link_name = "l_wrist_roll_link";
pcm.target_point_offset.x = 0;
pcm.target_point_offset.y = 0;
pcm.target_point_offset.z = 0;
pcm.constraint_region.primitives.resize(1);
pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
pcm.constraint_region.primitives[0].dimensions.resize(1);
pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
pcm.header.frame_id = kmodel->getModelFrame();
pcm.constraint_region.primitive_poses.resize(1);
pcm.constraint_region.primitive_poses[0].position.x = 0.55;
pcm.constraint_region.primitive_poses[0].position.y = 0.2;
pcm.constraint_region.primitive_poses[0].position.z = 1.25;
pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
pcm.weight = 1.0;
c.position_constraints.push_back(pcm);
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "l_wrist_roll_link";
ocm.header.frame_id = kmodel->getModelFrame();
ocm.orientation.x = 0.0;
ocm.orientation.y = 0.0;
ocm.orientation.z = 0.0;
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.2;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.4;
ocm.weight = 1.0;
c.orientation_constraints.push_back(ocm);
ocm.link_name = "r_wrist_roll_link";
ocm.header.frame_id = kmodel->getModelFrame();
ocm.orientation.x = 0.0;
ocm.orientation.y = 0.0;
ocm.orientation.z = 0.0;
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.01;
ocm.absolute_y_axis_tolerance = 0.01;
ocm.absolute_z_axis_tolerance = 0.01;
ocm.weight = 1.0;
c.orientation_constraints.push_back(ocm);
robot_state::Transforms &tf = ps->getTransformsNonConst();
constraint_samplers::ConstraintSamplerPtr s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "arms", c);
EXPECT_TRUE(s);
constraint_samplers::UnionConstraintSampler* ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
EXPECT_TRUE(ucs);
kinematic_constraints::KinematicConstraintSet kset(kmodel);
kset.add(c, tf);
robot_state::RobotState ks(kmodel);
ks.setToDefaultValues();
ks.update();
robot_state::RobotState ks_const(kmodel);
ks_const.setToDefaultValues();
ks_const.update();
static const int NT = 100;
int succ = 0;
for (int t = 0 ; t < NT ; ++t)
{
EXPECT_TRUE(s->sample(ks, ks_const, 1000));
EXPECT_TRUE(kset.decide(ks).satisfied);
if (s->sample(ks, ks_const, 1))
succ++;
}
logInform("Success rate for IK Constraint Sampler with position & orientation constraints for both arms: %lf", (double)succ / (double)NT);
}
