GCC Code Coverage Report


Directory: ./
File: src/path-optimization/reeds-shepp/piecewise-quadratic.hh
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 0 9 0.0%
Branches: 0 8 0.0%

Line Branch Exec Source
1 //
2 // Copyright (c) 2019 - 2024 CNRS
3 //
4 // Author: Florent Lamiraux
5 //
6
7 // Redistribution and use in source and binary forms, with or without
8 // modification, are permitted provided that the following conditions are
9 // met:
10 //
11 // 1. Redistributions of source code must retain the above copyright
12 // notice, this list of conditions and the following disclaimer.
13 //
14 // 2. Redistributions in binary form must reproduce the above copyright
15 // notice, this list of conditions and the following disclaimer in the
16 // documentation and/or other materials provided with the distribution.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
21 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
22 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
23 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
24 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
29 // DAMAGE.
30
31 #ifndef HPP_CORE_PATH_OPTIMIZATION_REEDS_SHEPP_PIECEWISE_QUADRATIC_HH
32 #define HPP_CORE_PATH_OPTIMIZATION_REEDS_SHEPP_PIECEWISE_QUADRATIC_HH
33
34 #include <hpp/core/time-parameterization.hh>
35
36 namespace hpp {
37 namespace core {
38 namespace pathOptimization {
39 namespace reedsShepp {
40
41 using hpp::core::interval_t;
42 HPP_PREDEF_CLASS(PiecewiseQuadratic);
43 typedef hpp::shared_ptr<PiecewiseQuadratic> PiecewiseQuadraticPtr_t;
44
45 /// Piecewise quadratic time parameterization
46 ///
47 /// On each interval \f$[t_i,t_{i+1}], f (t) = a_i (t-t_i) + b_i(t-t_i) +
48 /// c_i^2\f$.
49 class PiecewiseQuadratic : public hpp::core::TimeParameterization {
50 public:
51 /// Create an instance
52 /// \param initVel initial velocity
53 static PiecewiseQuadraticPtr_t create(const value_type& initVel);
54 static PiecewiseQuadraticPtr_t createCopy(
55 const PiecewiseQuadraticPtr_t& other);
56 virtual hpp::core::TimeParameterizationPtr_t copy() const;
57 interval_t definitionInterval() const;
58 virtual value_type value(const value_type& t) const;
59 virtual value_type derivative(const value_type& t,
60 const size_type& order) const;
61 virtual value_type derivativeBound(const value_type& low,
62 const value_type& up) const;
63 /// Add up to 3 constant acceleration segments
64 ///
65 /// \param distance distance travelled on these segments,
66 /// \param accel constant acceleration on the first segment,
67 /// \param decel constant negative acceleration on the last segment,
68 /// \param maxVel constant velocity on the middle segment
69 /// \param targetVel velocity at the end of the segment
70 /// \note depending on the distance, one of the above segment might be
71 /// of size 0, and therefore not represented.
72 void addSegments(const value_type& distance, const value_type& accel,
73 const value_type& decel, const value_type& maxVel,
74 const value_type& targetVel);
75
76 protected:
77 PiecewiseQuadratic(const value_type& initVel) : initVel_(initVel) {
78 times_.push_back(0);
79 }
80
81 PiecewiseQuadratic(const PiecewiseQuadratic& other)
82 : hpp::core::TimeParameterization(other),
83 times_(other.times_),
84 a_(other.a_),
85 b_(other.b_),
86 c_(other.c_),
87 initVel_(other.initVel_) {}
88 void init(const PiecewiseQuadraticWkPtr_t& weak);
89
90 private:
91 size_type findInterval(value_type t) const;
92 std::vector<value_type> times_;
93 std::vector<value_type> a_, b_, c_;
94 value_type initVel_;
95 PiecewiseQuadraticWkPtr_t weak_;
96 }; // class PiecewiseQuadratic
97 } // namespace reedsShepp
98 } // namespace pathOptimization
99 } // namespace core
100 } // namespace hpp
101 #endif // HPP_CORE_PATH_OPTIMIZATION_REEDS_SHEPP_PIECEWISE_QUADRATIC_HH
102