37#include "ompl/tools/config/MagicConstants.h"
38#include "ompl/base/spaces/constraint/ConstrainedStateSpace.h"
39#include "ompl/util/Exception.h"
57 return ss_.getConstraint()->isSatisfied(s2) && ss_.discreteGeodesic(s1, s2,
false);
61 std::pair<State *, double> &lastValid)
const
64 std::vector<ompl::base::State *> stateList;
65 bool reached = ss_.discreteGeodesic(s1, s2,
false, &stateList);
69 if (stateList.empty())
71 if (lastValid.first !=
nullptr)
72 ss_.copyState(lastValid.first, s1);
78 double distanceTraveled = 0;
79 for (std::size_t i = 0; i < stateList.size() - 1; i++)
82 distanceTraveled += ss_.distance(stateList[i], stateList[i + 1]);
83 ss_.freeState(stateList[i]);
86 if (!reached && (lastValid.first !=
nullptr))
90 ss_.copyState(lastValid.first, stateList.back());
94 double approxDistanceRemaining = ss_.distance(lastValid.first, s2);
95 lastValid.second = distanceTraveled / (distanceTraveled + approxDistanceRemaining);
98 ss_.freeState(stateList.back());
99 return ss_.getConstraint()->isSatisfied(s2) && reached;
104 , constraint_(constraint)
105 , n_(space->getDimension())
106 , k_(constraint_->getManifoldDimension())
108 setDelta(magic::CONSTRAINED_STATE_SPACE_DELTA);
113 auto *s1 = allocState()->as<
StateType>();
114 State *s2 = allocState();
117 bool isTraversable =
false;
121 bool satisfyGeodesics =
false;
122 bool continuityGeodesics =
false;
124 ss->sampleUniform(s1);
128 if (flags & CONSTRAINED_STATESPACE_JACOBIAN)
130 Eigen::MatrixXd j_a(n_ - k_, n_), j_n(n_ - k_, n_);
132 constraint_->jacobian(*s1, j_a);
133 constraint_->Constraint::jacobian(*s1, j_n);
135 if ((j_a - j_n).norm() > constraint_->getTolerance())
136 throw Exception(
"Constraint Jacobian deviates from numerical approximation.");
139 ss->sampleUniformNear(s2, s1, 10 * delta_);
142 if (flags & CONSTRAINED_STATESPACE_SAMPLERS && (!constraint_->isSatisfied(s1) || !constraint_->isSatisfied(s2)))
143 throw Exception(
"State samplers generate constraint unsatisfying states.");
145 std::vector<State *> geodesic;
147 if ((isTraversable |= discreteGeodesic(s1, s2,
true, &geodesic)))
150 if (flags & CONSTRAINED_STATESPACE_GEODESIC_INTERPOLATE &&
151 !constraint_->isSatisfied(geodesicInterpolate(geodesic, 0.5)))
152 throw Exception(
"Geodesic interpolate returns unsatisfying configurations.");
154 State *prev =
nullptr;
155 for (
auto s : geodesic)
158 if (flags & CONSTRAINED_STATESPACE_GEODESIC_SATISFY)
159 satisfyGeodesics |= !constraint_->isSatisfied(s);
162 if (flags & CONSTRAINED_STATESPACE_GEODESIC_CONTINUITY && prev !=
nullptr)
163 continuityGeodesics |= distance(prev, s) > lambda_ * delta_;
168 for (
auto s : geodesic)
172 if (satisfyGeodesics)
173 throw Exception(
"Discrete geodesic computation generates invalid states.");
175 if (continuityGeodesics)
176 throw Exception(
"Discrete geodesic computation generates non-continuous states.");
183 throw Exception(
"Unable to compute discrete geodesic on constraint.");
188 constrainedSanityChecks(~0);
190 double zero = std::numeric_limits<double>::epsilon();
191 double eps = std::numeric_limits<double>::epsilon();
192 unsigned int flags = STATESPACE_DISTANCE_DIFFERENT_STATES | STATESPACE_DISTANCE_SYMMETRIC |
193 STATESPACE_DISTANCE_BOUND | STATESPACE_RESPECT_BOUNDS | STATESPACE_ENFORCE_BOUNDS_NO_OP;
202 throw ompl::Exception(
"ompl::base::ConstrainedStateSpace::setSpaceInformation(): "
204 if (si->getStateSpace().get() !=
this)
205 throw ompl::Exception(
"ompl::base::ConstrainedStateSpace::setSpaceInformation(): "
206 "si for ConstrainedStateSpace must be constructed from the same state space object.");
214 throw ompl::Exception(
"ompl::base::ConstrainedStateSpace::setDelta(): "
215 "delta must be positive.");
220 setLongestValidSegmentFraction(delta_ / getMaximumExtent());
221 si_->setStateValidityCheckingResolution(delta_);
232 "Must associate a SpaceInformation object to the ConstrainedStateSpace via"
233 "setStateInformation() before use.");
247 auto *state = space_->allocState();
249 for (
unsigned int i = 1; i < space_->getDimension() && flag; ++i)
251 std::size_t newStride = space_->getValueAddressAtIndex(state, i) - space_->getValueAddressAtIndex(state, i - 1);
252 flag = newStride == 1;
254 space_->freeState(state);
258 "Stride length of member variables != 1, cannot translate into dense vector.");
274 std::vector<State *> geodesic;
278 if (discreteGeodesic(from, to,
true, &geodesic))
279 temp = geodesicInterpolate(geodesic, t);
281 copyState(state, temp);
283 for (
auto s : geodesic)
288 const double t)
const
290 unsigned int n = geodesic.size();
291 auto *d =
new double[n];
295 for (
unsigned int i = 1; i < n; ++i)
296 d[i] = d[i - 1] + distance(geodesic[i - 1], geodesic[i]);
299 const double last = d[n - 1];
300 if (last <= std::numeric_limits<double>::epsilon())
308 while (i < (n - 1) && (d[i] / last) <= t)
311 const double t1 = d[i] / last - t;
312 const double t2 = (i <= n - 2) ? d[i + 1] / last - t : 1;
315 assert((t1 < t2 || std::abs(t1 - t2) < std::numeric_limits<double>::epsilon()) ? (i < geodesic.size()) : (i + 1 < geodesic.size()));
316 return (t1 < t2 || std::abs(t1 - t2) < std::numeric_limits<double>::epsilon()) ? geodesic[i] : geodesic[i + 1];
The exception type for ompl.
bool checkMotion(const State *s1, const State *s2) const override
Return whether we can step from s1 to s2 along the manifold without collision.
ConstrainedMotionValidator(SpaceInformation *si)
Constructor.
A State in a ConstrainedStateSpace, represented as a dense real vector of values. For convenience and...
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
virtual State * geodesicInterpolate(const std::vector< State * > &geodesic, double t) const
Like interpolate(...), but interpolates between intermediate states already supplied in stateList fro...
void setSpaceInformation(SpaceInformation *si)
Sets the space information for this state space. Required for collision checking in manifold traversa...
virtual void clear()
Clear any allocated memory from the state space.
State * allocState() const override
Allocate a new state in this space.
void interpolate(const State *from, const State *to, double t, State *state) const override
Find a state between from and to around time t, where t = 0 is from, and t = 1 is the final state rea...
void constrainedSanityChecks(unsigned int flags) const
Do some sanity checks relating to discrete geodesic computation and constraint satisfaction....
ConstrainedStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint)
Construct a constrained space from an ambientSpace and a constraint.
void sanityChecks() const override
Perform both constrained and regular sanity checks.
void setDelta(double delta)
Set delta, the step size for traversing the manifold and collision checking. Default defined by ompl:...
void setup() override
Final setup for the space.
A shared pointer wrapper for ompl::base::Constraint.
Abstract definition for a class checking the validity of motions – path segments between states....
A shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::StateSpace.
virtual void sanityChecks() const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
Definition of an abstract state.
State space wrapper that transparently passes state space operations through to the underlying space....
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...