51 Eigen::VectorXd field(const ob::State *state)
53 const ob::RealVectorStateSpace::StateType &x = *state->as<ob::RealVectorStateSpace::StateType>();
67 ob::RealVectorBounds bounds(2);
74 og::SimpleSetup ss(space);
81 ob::ScopedState<> start(space);
86 ob::ScopedState<> goal(space);
102 ob::PlannerStatus solved = ss.solve(10.0);
106 if (solved == ob::PlannerStatus::EXACT_SOLUTION)
113 ompl::geometric::PathGeometric p = ss.getSolutionPath();
118 std::cout << "Total upstream cost: " << p.cost(upstream) << "\n";
base::Cost cost(const base::OptimizationObjectivePtr &obj) const override
The sum of the costs for the sequence of segments that make up the path, computed using OptimizationO...
virtual void printAsMatrix(std::ostream &out) const
Print the path as a real-valued matrix where the i-th row represents the i-th state along the path...
Create the set of classes typically needed to solve a geometric problem.
const T * as() const
Cast this instance to a desired type.
Define the type of state allocated by this space.
void interpolate(unsigned int count)
Insert a number of states in a path so that the path is made up of exactly count states. States are inserted uniformly (more states on longer segments). Changes are performed only if a path has less than count states.
The planner found an exact solution.
A class to store the exit status of Planner::solve()
A state space representing Rn. The distance function is the L2 norm.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
The lower and upper bounds for an Rn space.