1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  *   https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  *   Redistribution and use in source and binary forms, with or
10  *   without modification, are permitted provided that the following
11  *   conditions are met:
12  *   * Redistributions of source code must retain the above copyright
13  *     notice, this list of conditions and the following disclaimer.
14  *   * Redistributions in binary form must reproduce the above
15  *     copyright notice, this list of conditions and the following
16  *     disclaimer in the documentation and/or other materials provided
17  *     with the distribution.
18  *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  *   POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_PLANNING_PATHSHORTENER_HPP_
34 #define DART_PLANNING_PATHSHORTENER_HPP_
35 
36 #include <list>
37 #include <vector>
38 #include <Eigen/Core>
39 
40 #include "dart/simulation/World.hpp"
41 
42 namespace dart {
43 namespace planning {
44 
45 class PathShortener
46 {
47 public:
48   PathShortener();
49   PathShortener(
50       simulation::WorldPtr world,
51       dynamics::SkeletonPtr robot,
52       const std::vector<std::size_t>& dofs,
53       double stepSize = 0.1);
54   ~PathShortener();
55   virtual void shortenPath(std::list<Eigen::VectorXd>& rawPath);
56   bool segmentCollisionFree(
57       std::list<Eigen::VectorXd>& waypoints,
58       const Eigen::VectorXd& config1,
59       const Eigen::VectorXd& config2);
60 
61 protected:
62   simulation::WorldPtr world;
63   dynamics::SkeletonPtr robot;
64   std::vector<std::size_t> dofs;
65   double stepSize;
66   virtual bool localPlanner(
67       std::list<Eigen::VectorXd>& waypoints,
68       std::list<Eigen::VectorXd>::const_iterator it1,
69       std::list<Eigen::VectorXd>::const_iterator it2);
70 };
71 
72 } // namespace planning
73 } // namespace dart
74 
75 #endif // DART_PLANNING_PATHSHORTENER_HPP_
76