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