GCC Code Coverage Report

Line	Exec	Source
1		//
2		// Copyright (c) 2015 CNRS
3		// Authors: Joseph Mirabel
4		//
5
6		// Redistribution and use in source and binary forms, with or without
7		// modification, are permitted provided that the following conditions are
8		// met:
9		//
10		// 1. Redistributions of source code must retain the above copyright
11		// notice, this list of conditions and the following disclaimer.
12		//
13		// 2. Redistributions in binary form must reproduce the above copyright
14		// notice, this list of conditions and the following disclaimer in the
15		// documentation and/or other materials provided with the distribution.
16		//
17		// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18		// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19		// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20		// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21		// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22		// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23		// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24		// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25		// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26		// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27		// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28		// DAMAGE.
29
30		#ifndef HPP_CORE_PATH_OPTIMIZATION_PARTIAL_SHORTCUT_HH
31		#define HPP_CORE_PATH_OPTIMIZATION_PARTIAL_SHORTCUT_HH
32
33		#include <hpp/core/path-optimizer.hh>
34
35		namespace hpp {
36		namespace core {
37		namespace pathOptimization {
38
39		typedef std::vector<JointConstPtr_t> JointStdVector_t;
40
41		/// \addtogroup path_optimization
42		/// \{
43
44		/// Partial shortcut
45		///
46		/// The algorithm has 3 steps:
47		/// \li find a suitable set of joints that can be optimized.
48		/// \li try a direct path for each of this joints. If this step fails for
49		/// a joint, then the joint is inserted in a input set of next step.
50		/// \li try to find random shortcut on each joint in the set.
51		///
52		/// See Parameters for information on how to tune the algorithm.
53		///
54		/// \note The optimizer assumes that the input path is a vector of optimal
55		/// paths for the distance function.
56		struct PartialShortcutTraits {
57	✗	static bool removeLockedJoints() { return true; }
58	✗	static bool onlyFullShortcut() { return false; }
59	✗	static std::size_t numberOfConsecutiveFailurePerJoints() { return 5; }
60	✗	static value_type progressionMargin() { return 1e-3; }
61		};
62
63		class HPP_CORE_DLLAPI PartialShortcut : public PathOptimizer {
64		public:
65		/// Return shared pointer to new object.
66		template <typename Traits>
67		static PartialShortcutPtr_t createWithTraits(
68		const ProblemConstPtr_t& problem);
69
70		/// Return shared pointer to new object.
71		static PartialShortcutPtr_t create(const ProblemConstPtr_t& problem);
72
73		/// Optimize path
74		virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path);
75
76		struct Parameters {
77		/// Whether of not the joint that are locked by the constraints
78		/// in the path should not be optimized.
79		/// This is safe if you have the same constraints along the path.
80		/// Defaults to true
81		bool removeLockedJoints;
82
83		/// Set it to true if you want to skip step 3
84		/// Defaults to false
85		bool onlyFullShortcut;
86
87		/// The optimization will stop after a number of consecutive failure
88		/// on each joint.
89		/// Defaults to 5
90		std::size_t numberOfConsecutiveFailurePerJoints;
91
92		/// An iteration will be considered as a failure is the path length
93		/// did not decrease more than progressionMargin.
94		/// Defaults is 1e-3
95		value_type progressionMargin;
96
97		Parameters();
98		} parameters;
99
100		protected:
101		PartialShortcut(const ProblemConstPtr_t& problem);
102
103		private:
104		PathVectorPtr_t generatePath(PathVectorPtr_t path, JointConstPtr_t joint,
105		const value_type t1, ConfigurationIn_t q1,
106		const value_type t2, ConfigurationIn_t q2) const;
107
108		JointStdVector_t generateJointVector(const PathVectorPtr_t& pv) const;
109
110		/// try direct path on each joint in jvIn.
111		/// \param jvIn contains the joints on which optimization should be
112		/// tried
113		/// \param jvOut contains the joints of jvIn on which optimization
114		/// failed.
115		/// \return the optimized path
116		PathVectorPtr_t optimizeFullPath(const PathVectorPtr_t& pv,
117		const JointStdVector_t& jvIn,
118		JointStdVector_t& jvOut) const;
119
120		/// optimize each joint in jvIn.
121		/// \param jvIn contains the joints on which optimization should be
122		/// tried
123		/// \return the optimized path
124		PathVectorPtr_t optimizeRandom(const PathVectorPtr_t& pv,
125		const JointStdVector_t& jv) const;
126		}; // class RandomShortcut
127		/// \}
128
129		template <typename Traits>
130	✗	PartialShortcutPtr_t PartialShortcut::createWithTraits(
131		const ProblemConstPtr_t& problem) {
132	✗	PartialShortcut* ptr = new PartialShortcut(problem);
133	✗	ptr->parameters.removeLockedJoints = Traits::removeLockedJoints();
134	✗	ptr->parameters.onlyFullShortcut = Traits::onlyFullShortcut();
135	✗	ptr->parameters.progressionMargin = Traits::progressionMargin();
136	✗	ptr->parameters.numberOfConsecutiveFailurePerJoints =
137	✗	Traits::numberOfConsecutiveFailurePerJoints();
138	✗	return PartialShortcutPtr_t(ptr);
139		}
140		} // namespace pathOptimization
141		} // namespace core
142		} // namespace hpp
143		#endif // HPP_CORE_PATH_OPTIMIZATION_PARTIAL_SHORTCUT_HH
144