GCC Code Coverage Report

Directory: ./
File: include/hpp/core/steering-method/steering-kinodynamic.hh
Date: 2024-08-10 11:29:48
Exec Total Coverage
Lines: 9 15 60.0%
Branches: 1 6 16.7%
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_STEERING_METHOD_KINODYNAMIC_HH
30 #define HPP_CORE_STEERING_METHOD_KINODYNAMIC_HH
31
32 #include <hpp/core/config.hh>
33 #include <hpp/core/fwd.hh>
34 #include <hpp/core/steering-method.hh>
35 #include <hpp/util/debug.hh>
36 #include <hpp/util/pointer.hh>
37
38 namespace hpp {
39 namespace core {
40 namespace steeringMethod {
41
42 typedef Eigen::Matrix<value_type, 3, 1> Vector3;
43
44 /// \addtogroup steering_method
45 /// \{
46
47 /// Steering method that creates KinodynamicPath instances.
48 /// It produce a "bang-bang" trajectory connecting exactly the two given state
49 /// (position and velocity) which respect velocity and acceleration bounds
50 /// defined by the user (see Problem parameters : Kinodynamic/velocityBound and
51 /// Kinodynamic/accelerationBound)
52 ///
53 /// Implementation based on the equation of the article
54 /// https://ieeexplore.ieee.org/document/6943083
55 ///
56 class HPP_CORE_DLLAPI Kinodynamic : public SteeringMethod {
57 public:
58 /// Create an instance
59 3 static KinodynamicPtr_t create(const ProblemConstPtr_t& problem) {
60
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 Kinodynamic* ptr = new Kinodynamic(problem);
61 3 KinodynamicPtr_t shPtr(ptr);
62 3 ptr->init(shPtr);
63 3 return shPtr;
64 }
65
66 /// Copy instance and return shared pointer
67 ✗ static KinodynamicPtr_t createCopy(const KinodynamicPtr_t& other) {
68 ✗ Kinodynamic* ptr = new Kinodynamic(*other);
69 ✗ KinodynamicPtr_t shPtr(ptr);
70 ✗ ptr->init(shPtr);
71 ✗ return shPtr;
72 }
73
74 /// Copy instance and return shared pointer
75 ✗ virtual SteeringMethodPtr_t copy() const { return createCopy(weak_.lock()); }
76
77 /// create a path between two configurations
78 virtual PathPtr_t impl_compute(ConfigurationIn_t q1,
79 ConfigurationIn_t q2) const;
80
81 /**
82 * @brief computeMinTime compute the minimum time required to go from state
83 * (p1,v1) to (p2,v2)
84 * @param p1 position at state 1
85 * @param p2 position at state 2
86 * @param v1 velocity at state 1
87 * @param v2 velocity at state 2
88 * @param infInterval : infeasible interval
89 * @return T the minimal time
90 */
91 double computeMinTime(int index, double p1, double p2, double v1, double v2,
92 interval_t* infInterval) const;
93
94 /**
95 * @brief fixedTimeTrajectory compute the minimum acceleration trajectory for
96 * desired time T (1 dimension)
97 * @param T lenght of the trajectory
98 * @param p1 position at state 1
99 * @param p2 position at state 2
100 * @param v1 velocity at state 1
101 * @param v2 velocity at state 2
102 * output :
103 * @param a1 acceleration during first phase
104 * @param t1 time of the first segment
105 * @param tv time of constant velocity segment (can be = 0)
106 * @param t2 time of the last segment
107 * @return T the minimal time
108 */
109 virtual void fixedTimeTrajectory(int index, double T, double p1, double p2,
110 double v1, double v2, double* a1, double* t0,
111 double* t1, double* tv, double* t2,
112 double* vLim) const;
113
114 void setAmax(Vector3 aMax) { aMax_ = aMax; }
115
116 void setVmax(Vector3 vMax) { vMax_ = vMax; }
117
118 protected:
119 /// Constructor
120 Kinodynamic(const ProblemConstPtr_t& problem);
121
122 /// Copy constructor
123 Kinodynamic(const Kinodynamic& other);
124
125 /// Store weak pointer to itself
126 3 void init(KinodynamicWkPtr_t weak) {
127 3 core::SteeringMethod::init(weak);
128 3 weak_ = weak;
129 3 }
130
131 Vector3 aMax_;
132 Vector3 vMax_;
133 double aMaxFixed_;
134 double aMaxFixed_Z_;
135 bool synchronizeVerticalAxis_;
136 bool orientedPath_;
137 bool orientationIgnoreZValue_;
138
139 private:
140 DeviceWkPtr_t device_;
141 KinodynamicWkPtr_t weak_;
142 }; // Kinodynamic
143 /// \}
144 } // namespace steeringMethod
145 } // namespace core
146 } // namespace hpp
147 #endif // HPP_CORE_STEERING_METHOD_KINODYNAMIC_HH
148