GCC Code Coverage Report

Directory: ./
File: include/hpp/core/kinodynamic-oriented-path.hh
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 12 37 32.4%
Branches: 4 36 11.1%
Line Branch Exec Source
1 // Copyright (c) 2016, LAAS-CNRS
2 // Authors: Pierre Fernbach (pierre.fernbach@laas.fr)
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_KINODYNAMIC_ORIENTED_PATH_HH
30 #define HPP_CORE_KINODYNAMIC_ORIENTED_PATH_HH
31
32 #include <hpp/core/kinodynamic-path.hh>
33
34 namespace hpp {
35 namespace core {
36 /// This class is similar to \link hpp::core::KinodynamicPath KinodynamicPath
37 /// \endlink exept that the orientation of the robot always follow the direction
38 /// of the velocity. If the problem parameter "Kinodynamic/forceYawOrientation"
39 /// have been set to True, only the orientation around the z axis is set to
40 /// follow the direction of the velocity.
41
42 class HPP_CORE_DLLAPI KinodynamicOrientedPath : public KinodynamicPath {
43 public:
44 typedef KinodynamicPath parent_t;
45 44000 virtual ~KinodynamicOrientedPath() {}
46
47 /// Create instance and return shared pointer
48 /// \param device Robot corresponding to configurations
49 /// \param init, end Start and end configurations of the path
50 /// \param length Distance between the configurations.
51 static KinodynamicOrientedPathPtr_t create(
52 const DevicePtr_t& device, ConfigurationIn_t init, ConfigurationIn_t end,
53 value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0,
54 ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2,
55 ConfigurationIn_t vLim, bool ignoreZValue = false) {
56 KinodynamicOrientedPath* ptr = new KinodynamicOrientedPath(
57 device, init, end, length, a1, t0, t1, tv, t2, vLim, ignoreZValue);
58 KinodynamicOrientedPathPtr_t shPtr(ptr);
59 ptr->init(shPtr);
60 ptr->checkPath();
61 return shPtr;
62 }
63
64 /// Create instance and return shared pointer
65 /// \param device Robot corresponding to configurations
66 /// \param init, end Start and end configurations of the path
67 /// \param length Distance between the configurations.
68 /// \param constraints the path is subject to
69 static KinodynamicOrientedPathPtr_t create(
70 const DevicePtr_t& device, ConfigurationIn_t init, ConfigurationIn_t end,
71 value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0,
72 ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2,
73 ConfigurationIn_t vLim, ConstraintSetPtr_t constraints,
74 bool ignoreZValue = false) {
75 KinodynamicOrientedPath* ptr =
76 new KinodynamicOrientedPath(device, init, end, length, a1, t0, t1, tv,
77 t2, vLim, constraints, ignoreZValue);
78 KinodynamicOrientedPathPtr_t shPtr(ptr);
79 ptr->init(shPtr);
80 ptr->checkPath();
81 return shPtr;
82 }
83
84 /// Create copy and return shared pointer
85 /// \param path path to copy
86 ✗ static KinodynamicOrientedPathPtr_t createCopy(
87 const KinodynamicOrientedPathPtr_t& path) {
88 ✗ KinodynamicOrientedPath* ptr = new KinodynamicOrientedPath(*path);
89 ✗ KinodynamicOrientedPathPtr_t shPtr(ptr);
90 ✗ ptr->init(shPtr);
91 ✗ ptr->checkPath();
92 ✗ return shPtr;
93 }
94
95 11000 static KinodynamicOrientedPathPtr_t create(const KinodynamicPathPtr_t& path,
96 bool ignoreZValue = false) {
97 KinodynamicOrientedPath* ptr =
98
1/2
✓ Branch 3 taken 11000 times.
✗ Branch 4 not taken.
 11000 new KinodynamicOrientedPath(*path, ignoreZValue);
99 11000 KinodynamicOrientedPathPtr_t shPtr(ptr);
100
1/2
✓ Branch 2 taken 11000 times.
✗ Branch 3 not taken.
 11000 ptr->init(shPtr);
101
1/2
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
 11000 ptr->checkPath();
102 11000 return shPtr;
103 }
104
105 /// Create copy and return shared pointer
106 /// \param path path to copy
107 /// \param constraints the path is subject to
108 ✗ static KinodynamicOrientedPathPtr_t createCopy(
109 const KinodynamicOrientedPathPtr_t& path,
110 const ConstraintSetPtr_t& constraints) {
111 KinodynamicOrientedPath* ptr =
112 ✗ new KinodynamicOrientedPath(*path, constraints);
113 ✗ KinodynamicOrientedPathPtr_t shPtr(ptr);
114 ✗ ptr->init(shPtr);
115 ✗ ptr->checkPath();
116 ✗ return shPtr;
117 }
118
119 /// Return a shared pointer to this
120 ///
121 /// As StaightPath are immutable, and refered to by shared pointers,
122 /// they do not need to be copied.
123 ✗ virtual PathPtr_t copy() const { return createCopy(weak_.lock()); }
124
125 /// Return a shared pointer to a copy of this and set constraints
126 ///
127 /// \param constraints constraints to apply to the copy
128 /// \pre *this should not have constraints.
129 ✗ virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const {
130 ✗ return createCopy(weak_.lock(), constraints);
131 }
132
133 ✗ bool ignoreZValue() const { return ignoreZValue_; }
134 void ignoreZValue(bool ignoreZValue) { ignoreZValue_ = ignoreZValue; }
135
136 protected:
137 /// Print path in a stream
138 ✗ virtual std::ostream& print(std::ostream& os) const {
139 ✗ os << "KinodynamicOrientedPath:" << std::endl;
140 ✗ os << "interval: [ " << timeRange().first << ", " << timeRange().second
141 ✗ << " ]" << std::endl;
142 ✗ os << "initial configuration: " << pinocchio::displayConfig(initial_)
143 ✗ << std::endl;
144 ✗ os << "final configuration: " << pinocchio::displayConfig(end_)
145 ✗ << std::endl;
146 ✗ return os;
147 }
148
149 void orienteInitAndGoal(const DevicePtr_t& device);
150
151 /// Constructor
152 KinodynamicOrientedPath(const DevicePtr_t& robot, ConfigurationIn_t init,
153 ConfigurationIn_t end, value_type length,
154 ConfigurationIn_t a1, ConfigurationIn_t t0,
155 ConfigurationIn_t t1, ConfigurationIn_t tv,
156 ConfigurationIn_t t2, ConfigurationIn_t vLim,
157 bool ignoreZValue);
158
159 /// Constructor with constraints
160 KinodynamicOrientedPath(const DevicePtr_t& robot, ConfigurationIn_t init,
161 ConfigurationIn_t end, value_type length,
162 ConfigurationIn_t a1, ConfigurationIn_t t0,
163 ConfigurationIn_t t1, ConfigurationIn_t tv,
164 ConfigurationIn_t t2, ConfigurationIn_t vLim,
165 ConstraintSetPtr_t constraints, bool ignoreZValue);
166
167 /// Copy constructor
168 KinodynamicOrientedPath(const KinodynamicOrientedPath& path);
169
170 /// constructor from KinodynamicPath
171 KinodynamicOrientedPath(const KinodynamicPath& path, bool ignoreZValue);
172
173 /// Copy constructor with constraints
174 KinodynamicOrientedPath(const KinodynamicOrientedPath& path,
175 const ConstraintSetPtr_t& constraints);
176
177 11000 void init(KinodynamicOrientedPathPtr_t self) {
178
1/2
✓ Branch 2 taken 11000 times.
✗ Branch 3 not taken.
 11000 parent_t::init(self);
179 11000 weak_ = self;
180 11000 checkPath();
181 11000 }
182
183 virtual bool impl_compute(ConfigurationOut_t result, value_type t) const;
184
185 virtual PathPtr_t impl_extract(const interval_t& subInterval) const;
186
187 private:
188 KinodynamicOrientedPathWkPtr_t weak_;
189 bool ignoreZValue_;
190 }; // class kinodynamic oriented path
191 } // namespace core
192 } // namespace hpp
193
194 #endif // HPP_CORE_KINODYNAMIC_ORIENTED_PATH_HH
195