GCC Code Coverage Report

Directory: ./
File: include/hpp/core/dubins-path.hh
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 1 24 4.2%
Branches: 0 28 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2017 CNRS
3 // Authors: Florent Lamiraux
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_DUBINS_PATH_HH
31 #define HPP_CORE_DUBINS_PATH_HH
32
33 #include <hpp/core/config.hh>
34 #include <hpp/core/fwd.hh>
35 #include <hpp/core/path-vector.hh>
36 #include <hpp/pinocchio/device.hh>
37
38 namespace hpp {
39 namespace core {
40 /// \addtogroup path
41 /// \{
42
43 /// Car like motion going only forward
44 ///
45 /// Implement a Dubins motion generation on the base joint.
46 /// Degrees of freedom are interpolated depending on the type of
47 /// \link hpp::pinocchio::Joint joint \endlink
48 /// they parameterize:
49 /// The following interpolation is made:
50 /// \li Reeds and Shepp interpolation for the base_joint_xy and
51 /// base_joint_rz
52 /// \li If the wheel joints are passed using setWheelJoints,
53 /// the configuration parameter of those joints are computed so that
54 /// the wheel is aligned with the velocity.
55 /// \li linear interpolation for the other joints
56 class DubinsPath : public PathVector {
57 public:
58 typedef core::PathVector parent_t;
59
60 /// Destructor
61 ✗ virtual ~DubinsPath() {}
62
63 /// Create instance and return shared pointer
64 /// \param device Robot corresponding to configurations
65 /// \param init, end Start and end configurations of the path
66 /// \param extraLength Part of path length due to non Dubins degrees of
67 /// freedom,
68 /// \param rho The radius of a turn,
69 /// \param xyId, rzId indices in configuration vector of translation
70 /// and rotation of the car,
71 /// \param wheels vector of joints that represent turning wheels.
72 static DubinsPathPtr_t create(const DevicePtr_t& device,
73 ConfigurationIn_t init, ConfigurationIn_t end,
74 value_type extraLength, value_type rho,
75 size_type xyId, size_type rzId,
76 const std::vector<JointPtr_t> wheels);
77 /// Create instance and return shared pointer
78 /// \param device Robot corresponding to configurations
79 /// \param init, end Start and end configurations of the path
80 /// \param extraLength Part of path length due to non Dubins degrees of
81 /// freedom,
82 /// \param rho The radius of a turn.
83 /// \param xyId, rzId indices in configuration vector of translation
84 /// and rotation of the car,
85 /// \param wheels vector of joints that represent turning wheels,
86 /// \param constraints the path is subject to
87 static DubinsPathPtr_t create(const DevicePtr_t& device,
88 ConfigurationIn_t init, ConfigurationIn_t end,
89 value_type extraLength, value_type rho,
90 size_type xyId, size_type rzId,
91 const std::vector<JointPtr_t> wheels,
92 ConstraintSetPtr_t constraints);
93
94 /// Create copy and return shared pointer
95 /// \param path path to copy
96 ✗ static DubinsPathPtr_t createCopy(const DubinsPathPtr_t& path) {
97 ✗ DubinsPath* ptr = new DubinsPath(*path);
98 ✗ DubinsPathPtr_t shPtr(ptr);
99 ✗ ptr->init(shPtr);
100 ✗ return shPtr;
101 }
102
103 /// Create copy and return shared pointer
104 /// \param path path to copy
105 /// \param constraints the path is subject to
106 ✗ static DubinsPathPtr_t createCopy(const DubinsPathPtr_t& path,
107 const ConstraintSetPtr_t& constraints) {
108 ✗ DubinsPath* ptr = new DubinsPath(*path, constraints);
109 ✗ DubinsPathPtr_t shPtr(ptr);
110 ✗ ptr->init(shPtr);
111 ✗ return shPtr;
112 }
113
114 /// Return a shared pointer to a copy of this object
115 ✗ virtual PathPtr_t copy() const { return createCopy(weak_.lock()); }
116
117 /// Return a shared pointer to a copy of this and set constraints
118 ///
119 /// \param constraints constraints to apply to the copy
120 /// \pre *this should not have constraints.
121 ✗ virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const {
122 ✗ return createCopy(weak_.lock(), constraints);
123 }
124
125 /// Return the internal robot.
126 inline DevicePtr_t device() const { return device_; }
127
128 /// Get the initial configuration
129 ✗ inline Configuration_t initial() const { return initial_; }
130
131 /// Get the final configuration
132 ✗ inline Configuration_t end() const { return end_; }
133
134 protected:
135 /// Print path in a stream
136 ✗ virtual std::ostream& print(std::ostream& os) const {
137 ✗ os << "DubinsPath:" << std::endl;
138 ✗ Path::print(os);
139 ✗ os << "initial configuration: " << initial_.transpose() << std::endl;
140 ✗ os << "final configuration: " << end_.transpose() << std::endl;
141 ✗ return os;
142 }
143 /// Constructor
144 DubinsPath(const DevicePtr_t& robot, ConfigurationIn_t init,
145 ConfigurationIn_t end, value_type extraLength, value_type rho,
146 size_type xyId, size_type rzId,
147 const std::vector<JointPtr_t> wheels);
148
149 /// Constructor with constraints
150 DubinsPath(const DevicePtr_t& robot, ConfigurationIn_t init,
151 ConfigurationIn_t end, value_type extraLength, value_type rho,
152 size_type xyId, size_type rzId,
153 const std::vector<JointPtr_t> wheels,
154 ConstraintSetPtr_t constraints);
155
156 /// Copy constructor
157 DubinsPath(const DubinsPath& path);
158
159 /// Copy constructor with constraints
160 DubinsPath(const DubinsPath& path, const ConstraintSetPtr_t& constraints);
161
162 void init(DubinsPathPtr_t self);
163
164 /// For serialization only.
165 ✗ DubinsPath() : xyId_(0), rzId_(0) {}
166
167 private:
168 void dubins_init_normalised(double alpha, double beta, double d);
169 void dubins_init(vector3_t q0, vector3_t q1);
170 typedef Eigen::Matrix<value_type, 3, 1> Lengths_t;
171
172 DevicePtr_t device_;
173 Configuration_t initial_;
174 Configuration_t end_;
175 const size_type xyId_, rzId_;
176 size_type dxyId_, drzId_;
177 std::vector<JointPtr_t> wheels_;
178 std::size_t typeId_;
179 Lengths_t lengths_;
180 value_type extraLength_, rho_;
181
182 vector3_t qi_; // the initial configuration
183 DubinsPathWkPtr_t weak_;
184
185 HPP_SERIALIZABLE();
186 }; // class DubinsPath
187
188 /// \}
189 } // namespace core
190 } // namespace hpp
191
192 18 BOOST_CLASS_EXPORT_KEY(hpp::core::DubinsPath)
193
194 #endif // HPP_CORE_DUBINS_PATH_HH
195