GCC Code Coverage Report

Directory: ./
File: include/hpp/core/path/hermite.hh
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 26 39 66.7%
Branches: 27 72 37.5%
Line Branch Exec Source
1 // Copyright (c) 2016 CNRS
2 // Authors: Joseph Mirabel
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #ifndef HPP_CORE_PATH_HERMITE_HH
30 #define HPP_CORE_PATH_HERMITE_HH
31
32 #include <hpp/core/config.hh>
33 #include <hpp/core/fwd.hh>
34 #include <hpp/core/path/spline.hh>
35
36 namespace hpp {
37 namespace core {
38 namespace path {
39 /// \addtogroup path
40 /// \{
41
42 class HPP_CORE_DLLAPI Hermite : public Spline<BernsteinBasis, 3> {
43 public:
44 typedef Spline<BernsteinBasis, 3> parent_t;
45
46 /// Destructor
47 14820644 virtual ~Hermite() {}
48
49 1852569 static HermitePtr_t create(const DevicePtr_t& device, ConfigurationIn_t init,
50 ConfigurationIn_t end,
51 ConstraintSetPtr_t constraints) {
52
3/6
✓ Branch 2 taken 1852569 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1852569 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1852569 times.
✗ Branch 10 not taken.
 1852569 Hermite* ptr = new Hermite(device, init, end, constraints);
53 1852569 HermitePtr_t shPtr(ptr);
54
1/2
✓ Branch 2 taken 1852569 times.
✗ Branch 3 not taken.
 1852569 ptr->init(shPtr);
55 1852569 return shPtr;
56 }
57
58 /// Create copy and return shared pointer
59 /// \param path path to copy
60 1852592 static HermitePtr_t createCopy(const HermitePtr_t& path) {
61
1/2
✓ Branch 3 taken 1852592 times.
✗ Branch 4 not taken.
 1852592 Hermite* ptr = new Hermite(*path);
62 1852592 HermitePtr_t shPtr(ptr);
63
1/2
✓ Branch 2 taken 1852592 times.
✗ Branch 3 not taken.
 1852592 ptr->init(shPtr);
64 1852592 return shPtr;
65 }
66
67 /// Create copy and return shared pointer
68 /// \param path path to copy
69 /// \param constraints the path is subject to
70 ✗ static HermitePtr_t createCopy(const HermitePtr_t& path,
71 const ConstraintSetPtr_t& constraints) {
72 ✗ Hermite* ptr = new Hermite(*path, constraints);
73 ✗ HermitePtr_t shPtr(ptr);
74 ✗ ptr->init(shPtr);
75 ✗ return shPtr;
76 }
77
78 /// Return a shared pointer to this
79 ///
80 /// As StaightPath are immutable, and refered to by shared pointers,
81 /// they do not need to be copied.
82
1/2
✓ Branch 2 taken 1852592 times.
✗ Branch 3 not taken.
 1852592 virtual PathPtr_t copy() const { return createCopy(weak_.lock()); }
83
84 /// Return a shared pointer to a copy of this and set constraints
85 ///
86 /// \param constraints constraints to apply to the copy
87 /// \pre *this should not have constraints.
88 ✗ virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const {
89 ✗ return createCopy(weak_.lock(), constraints);
90 }
91
92 /// Return the internal robot.
93 DevicePtr_t device() const;
94
95 3705105 void v0(const vectorIn_t& speed) {
96
6/12
✓ Branch 1 taken 3705105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3705105 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3705105 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3705105 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3705105 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3705105 times.
✗ Branch 17 not taken.
 3705105 parameters_.row(1) = parameters_.row(0) + speed.transpose() / 3;
97 3705105 hermiteLength_ = -1;
98 3705105 }
99
100 3705105 void v1(const vectorIn_t& speed) {
101
6/12
✓ Branch 1 taken 3705105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3705105 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3705105 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3705105 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3705105 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3705105 times.
✗ Branch 17 not taken.
 3705105 parameters_.row(2) = parameters_.row(3) - speed.transpose() / 3;
102 3705105 hermiteLength_ = -1;
103 3705105 }
104
105 926268 vector_t v0() const {
106
4/8
✓ Branch 2 taken 926268 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 926268 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 926268 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 926268 times.
✗ Branch 12 not taken.
 1852536 return 3 * (parameters_.row(1) - parameters_.row(0));
107 // TODO Should be equivalent to
108 // vector_t res (outputDerivativeSize());
109 // derivative (res, timeRange().first, 1);
110 // return res;
111 }
112
113
4/8
✓ Branch 2 taken 926268 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 926268 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 926268 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 926268 times.
✗ Branch 12 not taken.
 1852536 vector_t v1() const { return 3 * (parameters_.row(3) - parameters_.row(2)); }
114
115 4631585 virtual Configuration_t initial() const { return init_; }
116
117 4631551 virtual Configuration_t end() const { return end_; }
118
119 4631532 const value_type& hermiteLength() const { return hermiteLength_; }
120
121 void computeHermiteLength();
122
123 vector_t velocity(const value_type& t) const;
124
125 protected:
126 /// Print path in a stream
127 ✗ virtual std::ostream& print(std::ostream& os) const {
128 ✗ os << "Hermite:" << std::endl;
129 ✗ Path::print(os);
130 ✗ os << "initial configuration: " << initial().transpose() << std::endl;
131 ✗ os << "final configuration: " << end().transpose() << std::endl;
132 ✗ return os;
133 }
134
135 /// Constructor
136 Hermite(const DevicePtr_t& robot, ConfigurationIn_t init,
137 ConfigurationIn_t end);
138
139 /// Constructor with constraints
140 Hermite(const DevicePtr_t& robot, ConfigurationIn_t init,
141 ConfigurationIn_t end, ConstraintSetPtr_t constraints);
142
143 /// Copy constructor
144 Hermite(const Hermite& path);
145
146 /// Copy constructor with constraints
147 Hermite(const Hermite& path, const ConstraintSetPtr_t& constraints);
148
149 void init(HermitePtr_t self);
150
151 private:
152 // void computeVelocities ();
153 void projectVelocities(ConfigurationIn_t qi, ConfigurationIn_t qe);
154
155 DevicePtr_t device_;
156 Configuration_t init_, end_;
157 value_type hermiteLength_;
158
159 HermiteWkPtr_t weak_;
160 }; // class Hermite
161 /// \}
162 } // namespace path
163 } // namespace core
164 } // namespace hpp
165 #endif // HPP_CORE_PATH_HERMITE_HH
166