31 #include <ompl/geometric/planners/rrt/RRTConnect.h> 32 #include <ompl/util/RandomNumbers.h> 38 OMPLTimeIndexedRNStateSpace::OMPLTimeIndexedRNStateSpace(
TimeIndexedSamplingProblemPtr &prob, TimeIndexedRRTConnectSolverInitializer init) :
ompl::base::CompoundStateSpace(), prob_(prob)
40 setName(
"OMPLTimeIndexedRNStateSpace");
41 unsigned int dim = prob->N;
42 addSubspace(ompl::base::StateSpacePtr(
new ompl::base::RealVectorStateSpace(dim)), 1.0);
43 ompl::base::RealVectorBounds bounds(dim);
44 for (
unsigned int i = 0; i < dim; ++i)
46 bounds.setHigh(i, prob->GetBounds()[i + dim]);
47 bounds.setLow(i, prob->GetBounds()[i]);
49 getSubspace(0)->as<ompl::base::RealVectorStateSpace>()->setBounds(bounds);
50 addSubspace(ompl::base::StateSpacePtr(
new ompl::base::TimeStateSpace), 1.0);
51 getSubspace(1)->as<ompl::base::TimeStateSpace>()->setBounds(
prob_->GetStartTime(),
prob_->GetGoalTime());
57 return CompoundStateSpace::allocDefaultStateSampler();
62 memcpy(ss->
getRNSpace().values, q.data(),
sizeof(double) * q.rows());
88 Eigen::VectorXd q(
prob_->N);
90 #if ROS_VERSION_MINIMUM(1, 12, 0) // if ROS version >= ROS_KINETIC 93 boost::static_pointer_cast<OMPLTimeIndexedRNStateSpace>(si_->getStateSpace())->OMPLToExoticaState(state, q, t);
96 if (!
prob_->IsValid(q, t))
107 this->parameters_ = init;
108 algorithm_ =
"Exotica_TimeIndexedRRTConnect";
109 planner_allocator_ = boost::bind(&allocatePlanner<OMPLTimeIndexedRRTConnect>, _1, _2);
111 if (this->parameters_.RandomSeed > -1)
113 HIGHLIGHT_NAMED(algorithm_,
"Setting random seed to " << this->parameters_.RandomSeed);
114 ompl::RNG::setSeed(static_cast<long unsigned int>(this->parameters_.RandomSeed));
124 ompl_simple_setup_.reset(
new ompl::geometric::SimpleSetup(state_space_));
126 ompl_simple_setup_->setPlannerAllocator(boost::bind(planner_allocator_, _1,
"Exotica_" + algorithm_));
127 ompl_simple_setup_->getSpaceInformation()->setStateValidityCheckingResolution(this->parameters_.ValidityCheckResolution);
129 ompl_simple_setup_->getSpaceInformation()->setup();
130 ompl_simple_setup_->setup();
131 if (ompl_simple_setup_->getPlanner()->params().hasParam(
"Range")) ompl_simple_setup_->getPlanner()->params().setParam(
"Range", this->parameters_.Range);
137 ompl_simple_setup_->getProblemDefinition()->clearSolutionPaths();
138 const ompl::base::PlannerPtr planner = ompl_simple_setup_->getPlanner();
139 if (planner) planner->clear();
140 ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
141 ompl_simple_setup_->getPlanner()->setProblemDefinition(ompl_simple_setup_->getProblemDefinition());
146 ompl_simple_setup_->clearStartStates();
147 int v = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getValidMotionCount();
148 int iv = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getInvalidMotionCount();
151 if (ompl_simple_setup_->getProblemDefinition()->hasApproximateSolution())
158 ompl::base::ScopedState<> gs(state_space_);
160 if (!ompl_simple_setup_->getStateValidityChecker()->isValid(gs.get()))
165 if (!ompl_simple_setup_->getSpaceInformation()->satisfiesBounds(gs.get()))
170 std::string out_of_bounds_joint_ids =
"";
171 for (
int i = 0; i < qT.rows(); ++i)
172 if (qT(i) <
prob_->GetBounds()[i] || qT(i) >
prob_->GetBounds()[i + qT.rows()]) out_of_bounds_joint_ids +=
"[j" + std::to_string(i) +
"=" + std::to_string(qT(i)) +
", ll=" + std::to_string(
prob_->GetBounds()[i]) +
", ul=" + std::to_string(
prob_->GetBounds()[i + qT.rows()]) +
"]\n";
174 ThrowNamed(
"Invalid goal state [Invalid joint bounds for joint indices: \n" 175 << out_of_bounds_joint_ids <<
"]");
177 ompl_simple_setup_->setGoalState(gs, eps);
182 ompl::geometric::PathSimplifierPtr psf_ = ompl_simple_setup_->getPathSimplifier();
183 const ompl::base::SpaceInformationPtr &si = ompl_simple_setup_->getSpaceInformation();
185 ompl::geometric::PathGeometric pg = ompl_simple_setup_->getSolutionPath();
186 if (this->parameters_.Smooth)
188 bool tryMore =
false;
189 if (ptc ==
false) tryMore = psf_->reduceVertices(pg);
190 if (ptc ==
false) psf_->collapseCloseVertices(pg);
192 while (times < 10 && tryMore && ptc ==
false)
194 tryMore = psf_->reduceVertices(pg);
197 if (si->getStateSpace()->isMetricSpace())
200 tryMore = psf_->shortcutPath(pg);
203 while (times < 10 && tryMore && ptc ==
false)
205 tryMore = psf_->shortcutPath(pg);
210 std::vector<ompl::base::State *> &states = pg.getStates();
211 unsigned int length = 0;
213 if (this->parameters_.TrajectoryPointsPerSecond <= 0)
215 const int n1 = states.size() - 1;
216 for (
int i = 0; i < n1; ++i)
217 length += si->getStateSpace()->validSegmentCount(states[i], states[i + 1]);
221 double tstart, tgoal;
222 Eigen::VectorXd qs, qg;
225 length =
static_cast<int>(std::ceil((tgoal - tstart) * this->parameters_.TrajectoryPointsPerSecond));
227 pg.interpolate(length);
229 traj.resize(pg.getStateCount(), this->parameters_.AddTimeIntoSolution ?
prob_->GetSpaceDim() + 1 :
prob_->GetSpaceDim());
230 Eigen::VectorXd tmp(
prob_->GetSpaceDim());
231 Eigen::VectorXd ts(pg.getStateCount());
232 for (
int i = 0; i < (int)pg.getStateCount(); ++i)
235 traj.row(i).tail(
prob_->GetSpaceDim()) = tmp;
237 if (this->parameters_.AddTimeIntoSolution) traj.col(0) = ts;
245 ompl::base::TimeStateSpace *time_space = ompl_simple_setup_->getStateSpace()->as<ompl::base::CompoundStateSpace>()->getSubspace(1)->as<ompl::base::TimeStateSpace>();
246 time_space->setBounds(
prob_->GetStartTime(),
prob_->GetGoalTime());
249 SetGoalState(
prob_->GetGoalState(),
prob_->GetGoalTime());
252 Eigen::VectorXd q0 =
prob_->ApplyStartState();
253 ompl::base::ScopedState<> ompl_start_state(state_space_);
255 ompl_simple_setup_->setStartState(ompl_start_state);
258 ompl::time::point start = ompl::time::now();
260 ptc_.reset(
new ompl::base::PlannerTerminationCondition(ompl::base::timedPlannerTerminationCondition(this->parameters_.Timeout - ompl::time::seconds(ompl::time::now() - start))));
261 if (ompl_simple_setup_->solve(*ptc_) == ompl::base::PlannerStatus::EXACT_SOLUTION && ompl_simple_setup_->haveSolutionPath())
263 GetPath(solution, *ptc_);
277 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
278 specs_.directed =
true;
283 connectionPoint_ = std::make_pair<base::State *, base::State *>(
nullptr,
nullptr);
294 tools::SelfConfig sc(si_,
getName());
297 #if ROS_VERSION_MINIMUM(1, 12, 0) // if ROS version >= ROS_KINETIC 298 if (!
tStart_)
tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(
this));
299 if (!
tGoal_)
tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(
this));
302 tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(si_->getStateSpace()));
304 tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(si_->getStateSpace()));
312 std::vector<Motion *> motions;
317 for (
unsigned int i = 0; i < motions.size(); ++i)
319 if (motions[i]->state) si_->freeState(motions[i]->state);
327 for (
unsigned int i = 0; i < motions.size(); ++i)
329 if (motions[i]->state) si_->freeState(motions[i]->state);
342 connectionPoint_ = std::make_pair<base::State *, base::State *>(
nullptr,
nullptr);
348 Motion *nmotion = tree->nearest(rmotion);
350 bool changed =
false;
354 bool reach = !changed;
357 base::State *dstate = rmotion->
state;
358 double d = si_->distance(nmotion->
state, rmotion->
state);
368 bool validMotion = tgi.
start ? si_->checkMotion(nmotion->
state, dstate) : si_->getStateValidityChecker()->isValid(dstate) && si_->checkMotion(dstate, nmotion->
state);
374 si_->copyState(motion->
state, dstate);
396 base::GoalSampleableRegion *goal =
dynamic_cast<base::GoalSampleableRegion *
>(pdef_->getGoal().get());
400 OMPL_ERROR(
"%s: Unknown type of goal",
getName().c_str());
401 return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
407 while (
const base::State *st = pis_.nextStart())
410 si_->copyState(motion->
state, st);
417 OMPL_ERROR(
"%s: Motion planning start tree could not be initialized!",
getName().c_str());
418 return base::PlannerStatus::INVALID_START;
421 if (!goal->couldSample())
423 OMPL_ERROR(
"%s: Insufficient states in sampleable goal region",
getName().c_str());
424 return base::PlannerStatus::INVALID_GOAL;
429 OMPL_INFORM(
"%s: Starting planning with %d states already in datastructure",
getName().c_str(), (
int)(
tStart_->size() +
tGoal_->size()));
432 tgi.
xstate = si_->allocState();
435 base::State *rstate = rmotion->
state;
436 bool startTree =
true;
442 tgi.
start = startTree;
443 startTree = !startTree;
446 if (
tGoal_->size() == 0 || pis_.getSampledGoalsCount() <
tGoal_->size() / 2)
448 const base::State *st =
tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
452 si_->copyState(motion->
state, st);
459 OMPL_ERROR(
"%s: Unable to sample any valid states for goal tree",
getName().c_str());
480 tgi.
start = startTree;
483 while (ptc ==
false && gsc ==
ADVANCED)
485 gsc =
growTree(otherTree, tgi, rmotion);
492 if (gsc ==
REACHED && goal->isStartGoalPairValid(startMotion->
root, goalMotion->
root))
498 startMotion = startMotion->
parent;
500 goalMotion = goalMotion->
parent;
505 Motion *solution = startMotion;
506 std::vector<Motion *> mpath1;
507 while (solution !=
nullptr)
509 mpath1.push_back(solution);
510 solution = solution->
parent;
513 solution = goalMotion;
514 std::vector<Motion *> mpath2;
515 while (solution !=
nullptr)
517 mpath2.push_back(solution);
518 solution = solution->
parent;
521 ompl::geometric::PathGeometric *path =
new ompl::geometric::PathGeometric(si_);
522 path->getStates().reserve(mpath1.size() + mpath2.size());
523 for (
int i = mpath1.size() - 1; i >= 0; --i)
524 path->append(mpath1[i]->state);
525 for (
unsigned int i = 0; i < mpath2.size(); ++i)
526 path->append(mpath2[i]->state);
528 pdef_->addSolutionPath(base::PathPtr(path),
false, 0.0,
getName());
535 si_->freeState(tgi.
xstate);
536 si_->freeState(rstate);
540 return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
545 Planner::getPlannerData(data);
547 std::vector<Motion *> motions;
550 for (
unsigned int i = 0; i < motions.size(); ++i)
552 if (motions[i]->parent ==
nullptr)
553 data.addStartVertex(base::PlannerDataVertex(motions[i]->state, 1));
556 data.addEdge(base::PlannerDataVertex(motions[i]->parent->state, 1), base::PlannerDataVertex(motions[i]->state, 1));
563 for (
unsigned int i = 0; i < motions.size(); ++i)
565 if (motions[i]->parent ==
nullptr)
566 data.addGoalVertex(base::PlannerDataVertex(motions[i]->state, 2));
570 data.addEdge(base::PlannerDataVertex(motions[i]->state, 2), base::PlannerDataVertex(motions[i]->parent->state, 2));
#define CONSOLE_BRIDGE_logWarn(fmt,...)
void ExoticaToOMPLState(const Eigen::VectorXd &q, const double &t, ompl::base::State *state) const
TimeIndexedSamplingProblemPtr prob_
void setRange(double distance)
Set the range the planner is supposed to use. This parameter greatly influences the runtime of the al...
void Solve(Eigen::MatrixXd &solution) override
Representation of a motion.
std::string getName(void *handle)
OMPLTimeIndexedStateValidityChecker(const ompl::base::SpaceInformationPtr &si, const TimeIndexedSamplingProblemPtr &prob)
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
const ompl::base::TimeStateSpace::StateType & getTime() const
void GetPath(Eigen::MatrixXd &traj, ompl::base::PlannerTerminationCondition &ptc)
#define CONSOLE_BRIDGE_logDebug(fmt,...)
virtual void SpecifyProblem(PlanningProblemPtr pointer)
progress has been made towards the randomly sampled state
bool correctTime(const Motion *a, Motion *b, bool reverse, bool &changed) const
#define REGISTER_MOTIONSOLVER_TYPE(TYPE, DERIV)
void Instantiate(const TimeIndexedRRTConnectSolverInitializer &init) override
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
OMPLTimeIndexedRRTConnect(const base::SpaceInformationPtr &si)
Information attached to growing a tree of motions (used internally)
TreeData tGoal_
The goal tree.
GrowState
The state of the tree after an attempt to extend it.
void SetGoalState(const Eigen::VectorXd &qT, const double t, const double eps=0)
bool isValid(const ompl::base::State *state) const override
double getRange() const
Get the range the planner is using.
void SpecifyProblem(PlanningProblemPtr pointer) override
ompl::base::StateSamplerPtr allocDefaultStateSampler() const override
virtual ~OMPLTimeIndexedRRTConnect()
double maxDistance_
The maximum length of a motion to be added to a tree.
base::StateSamplerPtr sampler_
State sampler.
void OMPLToExoticaState(const ompl::base::State *state, Eigen::VectorXd &q, double &t) const
void getPlannerData(base::PlannerData &data) const override
TreeData tStart_
The start tree.
TimeIndexedSamplingProblemPtr prob_
#define HIGHLIGHT_NAMED(name, x)
double reverseTimeDistance(const Motion *a, const Motion *b) const
std::shared_ptr< exotica::TimeIndexedSamplingProblem > TimeIndexedSamplingProblemPtr
void SetPlannerTerminationCondition(const std::shared_ptr< ompl::base::PlannerTerminationCondition > &ptc)
double GetDuration() const
std::shared_ptr< PlanningProblem > PlanningProblemPtr
the randomly sampled state was reached
const ompl::base::RealVectorStateSpace::StateType & getRNSpace() const
void freeMemory()
Free the memory allocated by this planner.
double forwardTimeDistance(const Motion *a, const Motion *b) const
no progress has been made
std::shared_ptr< NearestNeighbors< Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion)
Grow a tree towards a random state.
void StateDebug(const Eigen::VectorXd &q) const