30 #ifndef TIME_INDEXED_RRT_CONNECT_SOLVER_TIME_INDEXED_RRT_CONNECT_H_ 31 #define TIME_INDEXED_RRT_CONNECT_SOLVER_TIME_INDEXED_RRT_CONNECT_H_ 38 #include <ompl/base/SpaceInformation.h> 39 #include <ompl/base/StateSpace.h> 40 #include <ompl/base/StateValidityChecker.h> 41 #include <ompl/base/goals/GoalSampleableRegion.h> 42 #include <ompl/base/spaces/RealVectorStateSpace.h> 43 #include <ompl/base/spaces/TimeStateSpace.h> 44 #include <ompl/geometric/SimpleSetup.h> 45 #include <ompl/tools/config/SelfConfig.h> 47 #include <exotica_time_indexed_rrt_connect_solver/time_indexed_rrt_connect_initializer.h> 54 class StateType :
public ompl::base::CompoundStateSpace::StateType
61 const ompl::base::RealVectorStateSpace::StateType &
getRNSpace()
const 63 return *as<ompl::base::RealVectorStateSpace::StateType>(0);
66 ompl::base::RealVectorStateSpace::StateType &
getRNSpace()
68 return *as<ompl::base::RealVectorStateSpace::StateType>(0);
71 const ompl::base::TimeStateSpace::StateType &
getTime()
const 73 return *as<ompl::base::TimeStateSpace::StateType>(1);
76 ompl::base::TimeStateSpace::StateType &
getTime()
78 return *as<ompl::base::TimeStateSpace::StateType>(1);
84 void ExoticaToOMPLState(
const Eigen::VectorXd &q,
const double &t, ompl::base::State *state)
const;
85 void OMPLToExoticaState(
const ompl::base::State *state, Eigen::VectorXd &q,
double &t)
const;
86 void StateDebug(
const Eigen::VectorXd &q)
const;
96 bool isValid(
const ompl::base::State *state)
const override;
98 bool isValid(
const ompl::base::State *state,
double &dist)
const override;
104 typedef boost::function<ompl::base::PlannerPtr(const ompl::base::SpaceInformationPtr &si, const std::string &name)>
ConfiguredPlannerAllocator;
109 void Instantiate(
const TimeIndexedRRTConnectSolverInitializer &init)
override;
110 void Solve(Eigen::MatrixXd &solution)
override;
112 void SetPlannerTerminationCondition(
const std::shared_ptr<ompl::base::PlannerTerminationCondition> &ptc);
115 template <
typename T>
116 static ompl::base::PlannerPtr
allocatePlanner(
const ompl::base::SpaceInformationPtr &si,
const std::string &new_name)
118 ompl::base::PlannerPtr planner(
new T(si));
119 if (!new_name.empty()) planner->setName(new_name);
123 void SetGoalState(
const Eigen::VectorXd &qT,
const double t,
const double eps = 0);
126 void GetPath(Eigen::MatrixXd &traj, ompl::base::PlannerTerminationCondition &ptc);
133 std::shared_ptr<ompl::base::PlannerTerminationCondition>
ptc_;
136 using namespace ompl;
144 void getPlannerData(base::PlannerData &data)
const override;
146 base::PlannerStatus solve(
const base::PlannerTerminationCondition &ptc)
override;
148 void clear()
override;
156 maxDistance_ = distance;
166 template <
template <
typename T>
class NN>
169 tStart_.reset(
new NN<Motion *>());
170 tGoal_.reset(
new NN<Motion *>());
173 void setup()
override;
182 Motion(
const base::SpaceInformationPtr &si) : state(si->allocState())
188 const base::State *root =
nullptr;
189 base::State *state =
nullptr;
194 typedef std::shared_ptr<NearestNeighbors<Motion *> >
TreeData;
230 Eigen::VectorXd qa, qb;
234 if (tb < ta)
return 1e10;
235 Eigen::VectorXd
diff = (qb - qa).cwiseAbs();
236 double min_dt = (diff.array() / max_vel.array()).maxCoeff();
237 if (fabs(tb - ta) < min_dt)
return 1e10;
246 Eigen::VectorXd qa, qb;
250 if (tb > ta)
return 1e10;
251 Eigen::VectorXd
diff = (qb - qa).cwiseAbs();
252 double min_dt = (diff.array() / max_vel.array()).maxCoeff();
253 if (fabs(tb - ta) < min_dt)
return 1e10;
261 Eigen::VectorXd qa, qb;
264 Eigen::VectorXd
diff = (qb - qa).cwiseAbs();
265 double min_dt = (diff.array() / max_vel.array()).maxCoeff();
266 if (fabs(tb - ta) < min_dt)
268 if (reverse_check_)
return false;
269 tb = ta + (reverse ? -min_dt : min_dt);
304 #endif // TIME_INDEXED_RRT_CONNECT_SOLVER_TIME_INDEXED_RRT_CONNECT_H_ 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 setNearestNeighbors()
Set a different nearest neighbors datastructure.
Representation of a motion.
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
const ompl::base::TimeStateSpace::StateType & getTime() const
progress has been made towards the randomly sampled state
RNG rng_
The random number generator.
bool correctTime(const Motion *a, Motion *b, bool reverse, bool &changed) const
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Information attached to growing a tree of motions (used internally)
TreeData tGoal_
The goal tree.
ompl::geometric::SimpleSetupPtr ompl_simple_setup_
GrowState
The state of the tree after an attempt to extend it.
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
OMPLTimeIndexedRNStateSpace(TimeIndexedSamplingProblemPtr &prob, TimeIndexedRRTConnectSolverInitializer init)
double getRange() const
Get the range the planner is using.
ompl::base::StateSamplerPtr allocDefaultStateSampler() const override
Motion(const base::SpaceInformationPtr &si)
double maxDistance_
The maximum length of a motion to be added to a tree.
boost::function< ompl::base::PlannerPtr(const ompl::base::SpaceInformationPtr &si, const std::string &name)> ConfiguredPlannerAllocator
base::StateSamplerPtr sampler_
State sampler.
void OMPLToExoticaState(const ompl::base::State *state, Eigen::VectorXd &q, double &t) const
TreeData tStart_
The start tree.
TimeIndexedSamplingProblemPtr prob_
ompl::base::TimeStateSpace::StateType & getTime()
ConfiguredPlannerAllocator planner_allocator_
double reverseTimeDistance(const Motion *a, const Motion *b) const
std::shared_ptr< exotica::TimeIndexedSamplingProblem > TimeIndexedSamplingProblemPtr
std::shared_ptr< PlanningProblem > PlanningProblemPtr
const ompl::base::RealVectorStateSpace::StateType & getRNSpace() const
static ompl::base::PlannerPtr allocatePlanner(const ompl::base::SpaceInformationPtr &si, const std::string &new_name)
double forwardTimeDistance(const Motion *a, const Motion *b) const
no progress has been made
ompl::base::StateSpacePtr state_space_
ompl::base::RealVectorStateSpace::StateType & getRNSpace()
TimeIndexedSamplingProblemPtr prob_
std::shared_ptr< NearestNeighbors< Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
void StateDebug(const Eigen::VectorXd &q) const
std::shared_ptr< ompl::base::PlannerTerminationCondition > ptc_