Loading...
Searching...
No Matches
VFRRT.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2015, Caleb Voss and Wilson Beebe
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Willow Garage nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Authors: Caleb Voss, Wilson Beebe */
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_RRT_VFRRT_
38#define OMPL_GEOMETRIC_PLANNERS_RRT_VFRRT_
39
40#include <limits>
41
42#include <Eigen/Core>
43
44#include <ompl/geometric/planners/rrt/RRT.h>
45
46namespace ompl
47{
48 namespace geometric
49 {
66 class VFRRT : public RRT
67 {
68 public:
69 using VectorField = std::function<Eigen::VectorXd(const base::State *)>;
70
72 VFRRT(const base::SpaceInformationPtr &si, VectorField vf, double exploration, double initial_lambda,
73 unsigned int update_freq);
74
76 ~VFRRT() override;
77
79 void clear() override;
80
82 double determineMeanNorm();
83
85 Eigen::VectorXd getNewDirection(const base::State *qnear, const base::State *qrand);
86
88 double biasedSampling(const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield, double lambdaScale);
89
96 void updateGain();
97
103 Eigen::VectorXd computeAlphaBeta(double omega, const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield);
104
109 Motion *extendTree(Motion *m, base::State *rstate, const Eigen::VectorXd &v);
115
118
119 void setup() override;
120
121 private:
123 const VectorField vf_;
124
126 unsigned int efficientCount_{0u};
127
129 unsigned int inefficientCount_{0u};
130
132 double explorationInefficiency_{0.};
133
135 double explorationSetting_;
136
138 double lambda_;
139
141 unsigned int nth_step_;
142
144 unsigned int step_{0u};
145
147 double meanNorm_{0.};
148
150 unsigned int vfdim_{0u};
151 };
152 }
153}
154#endif
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition of an abstract state.
Definition State.h:50
Representation of a motion.
Definition RRT.h:148
RRT(const base::SpaceInformationPtr &si, bool addIntermediateStates=false)
Constructor.
Definition RRT.cpp:42
double biasedSampling(const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield, double lambdaScale)
Definition VFRRT.cpp:116
VFRRT(const base::SpaceInformationPtr &si, VectorField vf, double exploration, double initial_lambda, unsigned int update_freq)
Definition VFRRT.cpp:51
double determineMeanNorm()
Definition VFRRT.cpp:80
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Definition VFRRT.cpp:194
void clear() override
Definition VFRRT.cpp:65
Eigen::VectorXd getNewDirection(const base::State *qnear, const base::State *qrand)
Definition VFRRT.cpp:93
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition VFRRT.cpp:74
Motion * extendTree(Motion *m, base::State *rstate, const Eigen::VectorXd &v)
Definition VFRRT.cpp:154
void updateExplorationEfficiency(Motion *m)
Definition VFRRT.cpp:184
Eigen::VectorXd computeAlphaBeta(double omega, const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield)
Definition VFRRT.cpp:140
This namespace contains code that is specific to planning under geometric constraints.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()