44 constraint_samplers::ConstraintSamplerPtr cs)
45 : ob::StateSampler(pc->getOMPLStateSpace().
get())
46 , planning_context_(pc)
47 , default_(space_->allocDefaultStateSampler())
48 , constraint_sampler_(
std::move(cs))
49 , work_state_(pc->getCompleteInitialRobotState())
50 , constrained_success_(0)
51 , constrained_failure_(0)
53 inv_dim_ = space_->getDimension() > 0 ? 1.0 / (double)space_->getDimension() : 1.0;
58 if (constrained_success_ == 0)
61 return (
double)constrained_success_ / (double)(constrained_success_ + constrained_failure_);
68 if (constraint_sampler_->sample(work_state_, planning_context_->getCompleteInitialRobotState(),
69 planning_context_->getMaximumStateSamplingAttempts()))
71 planning_context_->getOMPLStateSpace()->copyToOMPLState(state, work_state_);
72 if (space_->satisfiesBounds(state))
74 ++constrained_success_;
78 ++constrained_failure_;
84 if (!sampleC(state) && !sampleC(state) && !sampleC(state))
85 default_->sampleUniform(state);
89 const double distance)
91 if (sampleC(state) || sampleC(state) || sampleC(state))
93 double total_d = space_->distance(state, near);
96 double dist = pow(rng_.uniform01(), inv_dim_) *
distance;
97 space_->interpolate(near, state, dist / total_d, state);
101 default_->sampleUniformNear(state, near,
distance);
106 if (sampleC(state) || sampleC(state) || sampleC(state))
108 double total_d = space_->distance(state, mean);
109 double distance = rng_.gaussian(0.0, stdDev);
112 double dist = pow(rng_.uniform01(), inv_dim_) *
distance;
113 space_->interpolate(mean, state, dist / total_d, state);
117 default_->sampleGaussian(state, mean, stdDev);