GCC Code Coverage Report

Directory: ./
File: src/steering-method/steering-kinodynamic.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 229 290 79.0%
Branches: 208 422 49.3%
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 #include <hpp/core/kinodynamic-oriented-path.hh>
30 #include <hpp/core/kinodynamic-path.hh>
31 #include <hpp/core/problem.hh>
32 #include <hpp/core/steering-method/steering-kinodynamic.hh>
33 #include <hpp/core/weighed-distance.hh>
34 #include <hpp/pinocchio/body.hh>
35 #include <hpp/pinocchio/configuration.hh>
36 #include <hpp/pinocchio/joint.hh>
37
38 namespace hpp {
39 namespace core {
40 namespace steeringMethod {
41
42 /**
43 * @brief sgnenum
44 * @param val
45 * @return return -1 if val is negative, 1 if val positive, 0 if val == 0
46 */
47 12042 inline int sgnenum(double val) { return ((0. < val) - (val < 0.)); }
48
49
2/2
✓ Branch 0 taken 6259 times.
✓ Branch 1 taken 2326 times.
 8585 inline int sgn(double d) { return d >= 0.0 ? 1 : -1; }
50
51
2/2
✓ Branch 0 taken 4012 times.
✓ Branch 1 taken 4006 times.
 8018 inline double sgnf(double d) { return d >= 0.0 ? 1.0 : -1.0; }
52
53 6138 bool belong(double t, interval_t interval) {
54
4/4
✓ Branch 0 taken 198 times.
✓ Branch 1 taken 5940 times.
✓ Branch 2 taken 57 times.
✓ Branch 3 taken 141 times.
 6138 return ((t > interval.first) && (t < interval.second));
55 }
56
57 2007 double computeMaxRequiredTime(double Tmax,
58 std::vector<interval_t> infIntervals) {
59 double T;
60 2007 bool inInterval = true;
61 2007 double minUpperBound = Tmax;
62
63
2/2
✓ Branch 0 taken 2046 times.
✓ Branch 1 taken 2007 times.
 4053 while (inInterval) {
64 2046 T = minUpperBound;
65 2046 minUpperBound = std::numeric_limits<value_type>::infinity();
66 2046 inInterval = false;
67
2/2
✓ Branch 1 taken 6138 times.
✓ Branch 2 taken 2046 times.
 8184 for (size_t i = 0; i < infIntervals.size(); ++i) {
68
2/2
✓ Branch 2 taken 57 times.
✓ Branch 3 taken 6081 times.
 6138 if (belong(T, infIntervals[i])) inInterval = true;
69
4/4
✓ Branch 1 taken 6117 times.
✓ Branch 2 taken 21 times.
✓ Branch 3 taken 46 times.
✓ Branch 4 taken 6092 times.
 12255 if ((infIntervals[i].second < minUpperBound) &&
70
2/2
✓ Branch 1 taken 46 times.
✓ Branch 2 taken 6071 times.
 6117 (infIntervals[i].second > T))
71 46 minUpperBound = infIntervals[i].second;
72 }
73 }
74
75 2007 return T;
76 }
77
78 2007 PathPtr_t Kinodynamic::impl_compute(ConfigurationIn_t q1,
79 ConfigurationIn_t q2) const {
80 2007 double Tmax = 0;
81 2007 double T = 0;
82 double t0, t1, tv, t2, a1, vLim;
83
7/14
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2007 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2007 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2007 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2007 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2007 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2007 times.
✗ Branch 20 not taken.
 2007 pinocchio::vector3_t dir(q2[0] - q1[0], q2[1] - q1[1], q2[2] - q1[2]);
84 hppDout(notice, "direction = " << dir);
85 2007 interval_t infeasibleInterval;
86 2007 std::vector<interval_t> infIntervalsVector;
87
1/2
✓ Branch 5 taken 2007 times.
✗ Branch 6 not taken.
 2007 size_type configSize = problem()->robot()->configSize() -
88 2007 problem()->robot()->extraConfigSpace().dimension();
89 // looking for Tmax
90 hppDout(notice, "## Looking for Tmax :");
91 hppDout(info, "between : " << pinocchio::displayConfig(q1));
92 hppDout(info, "and : " << pinocchio::displayConfig(q2));
93
94 // for all joints
95
2/2
✓ Branch 0 taken 6021 times.
✓ Branch 1 taken 2007 times.
 8028 for (int indexConfig = 0; indexConfig < 3;
96 indexConfig++) { // FIX ME : only work for freeflyer
97 6021 size_type indexVel = indexConfig + configSize;
98 hppDout(
99 notice,
100 "For joint :"
101 << problem()->robot()->getJointAtConfigRank(indexConfig)->name());
102
4/8
✓ Branch 5 taken 6021 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 6021 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6021 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 6021 times.
✗ Branch 17 not taken.
 6021 if (problem()->robot()->getJointAtConfigRank(indexConfig)->name() !=
103 "base_joint_SO3") {
104
5/10
✓ Branch 1 taken 6021 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6021 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6021 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6021 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6021 times.
✗ Branch 14 not taken.
 6021 T = computeMinTime(indexConfig, q1[indexConfig], q2[indexConfig],
105 q1[indexVel], q2[indexVel], &infeasibleInterval);
106
1/2
✓ Branch 1 taken 6021 times.
✗ Branch 2 not taken.
 6021 infIntervalsVector.push_back(infeasibleInterval);
107
2/2
✓ Branch 0 taken 3031 times.
✓ Branch 1 taken 2990 times.
 6021 if (T > Tmax) Tmax = T;
108 } else {
109 hppDout(notice, "!! Steering method for quaternion not implemented yet.");
110 }
111 }
112
113
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2007 times.
 2007 if (Tmax <= std::numeric_limits<double>::epsilon()) {
114 hppDout(notice, "Steering kinodynamic : no translation mouvement.");
115 // TODO : it mean that you must rotate without translation, not implemented
116 // yet
117 ✗ return StraightPath::create(device_.lock(), q1, q2, 0.);
118 }
119
120
2/4
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2007 times.
✗ Branch 5 not taken.
 2007 value_type length = computeMaxRequiredTime(Tmax, infIntervalsVector);
121 hppDout(notice,
122 "Tmax = " << Tmax << " after infeasible interval : " << length);
123 // create array of times intervals and acceleration values:
124
1/2
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
 2007 Configuration_t a1_t(3);
125
1/2
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
 2007 Configuration_t t0_t(3);
126
1/2
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
 2007 Configuration_t t1_t(3);
127
1/2
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
 2007 Configuration_t t2_t(3);
128
1/2
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
 2007 Configuration_t tv_t(3);
129
1/2
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
 2007 Configuration_t vLim_t(3);
130
131 // compute trajectory with fixed time T found
132 /*for (model::JointVector_t::const_iterator itJoint = jv.begin (); itJoint !=
133 jv.end (); itJoint++) { size_type indexConfig =
134 (*itJoint)->rankInConfiguration (); size_type indexVel =
135 (*itJoint)->rankInVelocity() + configSize; hppDout(notice,"For joint
136 "<<(*itJoint)->name()); if((*itJoint)->configSize() >= 1){
137 fixedTimeTrajectory(Tmax,q1[indexConfig],q2[indexConfig],q1[indexVel],q2[indexVel],&a1,&t1,&tv,&t2);
138 a1_t[indexConfig]=a1;
139 t1_t[indexConfig]=t1;
140 tv_t[indexConfig]=tv;
141 t2_t[indexConfig]=t2;
142 }
143
144 }// for all joints
145 */
146 hppDout(info, "compute fixed end-time trajectory for each joint : ");
147
2/2
✓ Branch 0 taken 6021 times.
✓ Branch 1 taken 2007 times.
 8028 for (int indexConfig = 0; indexConfig < 3; indexConfig++) {
148 6021 size_type indexVel = indexConfig + configSize;
149 hppDout(
150 notice,
151 "For joint :"
152 << problem()->robot()->getJointAtConfigRank(indexConfig)->name());
153
4/8
✓ Branch 5 taken 6021 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 6021 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6021 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 6021 times.
✗ Branch 17 not taken.
 6021 if (problem()->robot()->getJointAtConfigRank(indexConfig)->name() !=
154 "base_joint_SO3") {
155
5/10
✓ Branch 1 taken 6021 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6021 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6021 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6021 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6021 times.
✗ Branch 14 not taken.
 6021 fixedTimeTrajectory(indexConfig, length, q1[indexConfig], q2[indexConfig],
156 q1[indexVel], q2[indexVel], &a1, &t0, &t1, &tv, &t2,
157 &vLim);
158
1/2
✓ Branch 1 taken 6021 times.
✗ Branch 2 not taken.
 6021 a1_t[indexConfig] = a1;
159
1/2
✓ Branch 1 taken 6021 times.
✗ Branch 2 not taken.
 6021 t0_t[indexConfig] = t0;
160
1/2
✓ Branch 1 taken 6021 times.
✗ Branch 2 not taken.
 6021 t1_t[indexConfig] = t1;
161
1/2
✓ Branch 1 taken 6021 times.
✗ Branch 2 not taken.
 6021 tv_t[indexConfig] = tv;
162
1/2
✓ Branch 1 taken 6021 times.
✗ Branch 2 not taken.
 6021 t2_t[indexConfig] = t2;
163
1/2
✓ Branch 1 taken 6021 times.
✗ Branch 2 not taken.
 6021 vLim_t[indexConfig] = vLim;
164 } else {
165 hppDout(notice, "!! Steering method for quaternion not implemented yet.");
166 /* a1_t[indexConfig]=0;
167 t1_t[indexConfig]=0;
168 tv_t[indexConfig]=0;
169 t2_t[indexConfig]=0; */
170 }
171 }
172
173 KinodynamicPathPtr_t path = KinodynamicPath::create(
174
9/18
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2007 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2007 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2007 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2007 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2007 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2007 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2007 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 2007 times.
✗ Branch 27 not taken.
 4014 device_.lock(), q1, q2, length, a1_t, t0_t, t1_t, tv_t, t2_t, vLim_t);
175
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 1007 times.
 2007 if (orientedPath_) {
176 KinodynamicOrientedPathPtr_t orientedPath =
177
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 KinodynamicOrientedPath::create(path, orientationIgnoreZValue_);
178 1000 return orientedPath;
179 1000 } else {
180 1007 return path;
181 }
182 2007 }
183
184 3 Kinodynamic::Kinodynamic(const ProblemConstPtr_t &problem)
185 : SteeringMethod(problem),
186
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 aMax_(Vector3::Ones(3)),
187
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 vMax_(Vector3::Ones(3)),
188 3 device_(problem->robot()),
189
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 weak_() {
190
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 3 times.
 3 if (((problem->robot()->extraConfigSpace().dimension())) < 6) {
191 ✗ std::cout << "Error : you need at least " << 6
192 ✗ << " extra DOF to use this steering method" << std::endl;
193 hppDout(error, "Error : you need at least "
194 << 6 << " extra DOF to use this steering method");
195 }
196
197 // get velocity and acceleration bounds from problem :
198
199 3 aMaxFixed_ =
200
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 6 problem->getParameter(std::string("Kinodynamic/accelerationBound"))
201
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 .floatValue();
202
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 aMax_ = Vector3::Ones(3) * aMaxFixed_;
203 3 aMaxFixed_Z_ =
204 problem
205
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 6 ->getParameter(std::string("Kinodynamic/verticalAccelerationBound"))
206
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 .floatValue();
207
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 vMax_ = Vector3::Ones(3) *
208
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 6 problem->getParameter(std::string("Kinodynamic/velocityBound"))
209
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 6 .floatValue();
210
211 hppDout(info, "#### create steering kinodynamic, vMax = "
212 << vMax_ << "; aMax_ = " << aMax_);
213
214 3 synchronizeVerticalAxis_ =
215
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 6 problem->getParameter(std::string("Kinodynamic/synchronizeVerticalAxis"))
216
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 .boolValue();
217 hppDout(notice, "synchronizeVerticalAxis in steering method = "
218 << synchronizeVerticalAxis_);
219 3 orientedPath_ =
220
2/8
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 6 problem->getParameter(std::string("Kinodynamic/forceAllOrientation"))
221
4/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
✓ Branch 8 taken 1 times.
 8 .boolValue() ||
222
7/14
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 2 times.
✓ Branch 9 taken 1 times.
✓ Branch 11 taken 2 times.
✓ Branch 12 taken 1 times.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 5 problem->getParameter(std::string("Kinodynamic/forceYawOrientation"))
223
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
 2 .boolValue();
224 3 orientationIgnoreZValue_ =
225
2/8
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 6 problem->getParameter(std::string("Kinodynamic/forceYawOrientation"))
226
3/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 3 times.
 6 .boolValue() &&
227
3/14
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 3 times.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 3 !problem->getParameter(std::string("Kinodynamic/forceAllOrientation"))
228 ✗ .boolValue();
229 hppDout(notice, "oriented path : " << orientedPath_);
230 hppDout(notice, "oriented path only constraint yaw (ignore z value) : "
231 << orientationIgnoreZValue_);
232 3 }
233
234 /// Copy constructor
235 ✗ Kinodynamic::Kinodynamic(const Kinodynamic &other)
236 : SteeringMethod(other),
237 ✗ aMax_(other.aMax_),
238 ✗ vMax_(other.vMax_),
239 ✗ synchronizeVerticalAxis_(other.synchronizeVerticalAxis_),
240 ✗ orientedPath_(other.orientedPath_),
241 ✗ orientationIgnoreZValue_(other.orientationIgnoreZValue_),
242 ✗ device_(other.device_) {}
243
244 6021 double Kinodynamic::computeMinTime(int index, double p1, double p2, double v1,
245 double v2, interval_t *infInterval) const {
246 hppDout(info, "p1 = " << p1 << " p2 = " << p2 << " ; v1 = " << v1
247 << " v2 = " << v2);
248 // compute the sign of each acceleration
249
2/4
✓ Branch 0 taken 6021 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 6021 times.
✗ Branch 3 not taken.
 6021 assert(index >= 0 && index < 3 &&
250 "index of joint should be between in [0;2]");
251
1/2
✓ Branch 1 taken 6021 times.
✗ Branch 2 not taken.
 6021 double aMax = std::fabs(aMax_[index]);
252 hppDout(notice, "amax used in computeMinTime(" << index << ") = " << aMax);
253
1/2
✓ Branch 1 taken 6021 times.
✗ Branch 2 not taken.
 6021 double vMax = vMax_[index];
254 double t1, t2, tv;
255 int sigma;
256 6021 double deltaPacc = 0.5 * (v1 + v2) * (fabs(v2 - v1) / aMax);
257 6021 sigma = sgnenum(p2 - p1 - deltaPacc); // TODO bug sigma == 0, temp fix ?
258 hppDout(notice, "deltaPacc = " << deltaPacc);
259 hppDout(info, "sigma = " << sigma);
260
2/2
✓ Branch 0 taken 2012 times.
✓ Branch 1 taken 4009 times.
 6021 if (sigma == 0) { // ??? FIXME
261 2012 sigma = sgn(p2 - p1);
262 hppDout(info, "sigma Bis= " << sigma);
263 }
264 6021 double a1 = (sigma)*aMax;
265 6021 double a2 = -a1;
266 6021 double vLim = (sigma)*vMax;
267 hppDout(info, "Vlim = " << vLim << " ; aMax = " << aMax);
268
4/4
✓ Branch 1 taken 2013 times.
✓ Branch 2 taken 4008 times.
✓ Branch 3 taken 2012 times.
✓ Branch 4 taken 4009 times.
 8034 if (fabs(p2 - p1) < (std::numeric_limits<double>::epsilon() * 100.) &&
269
2/2
✓ Branch 1 taken 2012 times.
✓ Branch 2 taken 1 times.
 2013 fabs(v2 - v1) < (std::numeric_limits<double>::epsilon() * 100.)) {
270 hppDout(notice, "No movement in this joints, abort.");
271 2012 return 0.;
272 }
273 // test if two segment trajectory is valid :
274 4009 bool twoSegment = false;
275
276 // solve quadratic equation (cf eq 13 article)
277 4009 const double a = a1;
278 4009 const double b = 2. * v1;
279 4009 const double c = (0.5 * (v1 + v2) * (v2 - v1) / a2) - (p2 - p1);
280
281 4009 const double q = -0.5 * (b + sgnf(b) * sqrt(b * b - 4 * a * c));
282 hppDout(info, "sign of " << b << " is : " << sgnf(b));
283 4009 double x1 = 0;
284 4009 double x2 = 0;
285
1/2
✓ Branch 0 taken 4009 times.
✗ Branch 1 not taken.
 4009 if (a != 0) x1 = q / a;
286
1/2
✓ Branch 0 taken 4009 times.
✗ Branch 1 not taken.
 4009 if (q != 0) x2 = c / q;
287 4009 const double x = std::max(x1, x2);
288 hppDout(info, "Solve quadratic equation : x1 = " << x1 << " ; x2 = " << x2);
289 hppDout(info, " x = " << x);
290 hppDout(info, "t1 before vel limit = " << x);
291
292 hppDout(info, "inf bound on t1 (from t2 > 0) " << -((v2 - v1) / a2));
293 4009 double minT1 = std::max(
294 4009 0., -((v2 - v1) / a2)); // lower bound for valid t1 value (cf eq 14)
295
296
1/2
✓ Branch 0 taken 4009 times.
✗ Branch 1 not taken.
 4009 if (x >= minT1) {
297 4009 twoSegment = true;
298 4009 t1 = x;
299 hppDout(info, "t1 >= minT1");
300 }
301
1/2
✓ Branch 0 taken 4009 times.
✗ Branch 1 not taken.
 4009 if (twoSegment) { // check if max velocity is respected
302
2/2
✓ Branch 1 taken 2151 times.
✓ Branch 2 taken 1858 times.
 4009 if (std::abs(v1 + (t1)*a1) > vMax) {
303 2151 twoSegment = false;
304 hppDout(info, "Doesn't respect max velocity, need 3 segments");
305 }
306 }
307
2/2
✓ Branch 0 taken 1858 times.
✓ Branch 1 taken 2151 times.
 4009 if (twoSegment) { // compute t2 for two segment trajectory
308 1858 tv = 0.;
309 1858 t2 = ((v2 - v1) / a2) + (t1); // eq 14
310 } else { // compute 3 segment trajectory, with constant velocity phase :
311 2151 t1 = (vLim - v1) / a1; // eq 15
312 2151 tv = ((v1 * v1 + v2 * v2 - 2 * vLim * vLim) / (2 * vLim * a1)) +
313 2151 (p2 - p1) / vLim; // eq 16
314 2151 t2 = (v2 - vLim) / a2; // eq 17
315 }
316 if (twoSegment) {
317 hppDout(notice, "Trajectory with 2 segments");
318 } else {
319 hppDout(notice, "Trajectory with 3 segments");
320 }
321 hppDout(notice, "a1 = " << a1 << " ; a2 =" << a2);
322 hppDout(notice, "t = " << (t1) << " ; " << (tv) << " ; " << (t2));
323 4009 double T = (t1) + (tv) + (t2);
324 hppDout(notice, "T = " << T);
325
326 // Compute infeasible interval :
327
328 hppDout(info, " !!! CHECKING INFEASIBLE INTERVALS : ");
329
330
4/4
✓ Branch 0 taken 2038 times.
✓ Branch 1 taken 1971 times.
✓ Branch 2 taken 1156 times.
✓ Branch 3 taken 882 times.
 4009 if (v1 * v2 <= 0.0 || a1 * v1 < 0.0) {
331 // If minimum-time solution goes through zero-velocity, there is no
332 // infeasible time interval, because we can stop and wait at zero-velocity
333 3127 infInterval->first = std::numeric_limits<value_type>::infinity();
334 3127 infInterval->second = 0;
335 3127 hppDout(info, " ! go through v=0, no infeasible interval");
336 } else {
337 882 double zeroTime1 = std::abs(v1) / aMax;
338 882 double zeroTime2 = std::abs(v2) / aMax;
339 882 double zeroDistance = zeroTime1 * v1 / 2.0 + zeroTime2 * v2 / 2.0;
340
2/2
✓ Branch 2 taken 779 times.
✓ Branch 3 taken 103 times.
 882 if (std::abs(zeroDistance) < std::abs(p2 - p1)) { // no infeasible interval
341 779 infInterval->first = std::numeric_limits<value_type>::infinity();
342 779 infInterval->second = 0;
343 hppDout(info, " ! not region I");
344 } else { // "region I", infeasible interval exist
345 hppDout(info, " ! region I, infeasible interval exist");
346 103 a1 = a2;
347 103 a2 = -a2;
348 103 vLim = -vLim;
349 // solve eq 13 with new a1/a2 :
350 103 const double ai = a1;
351 103 const double bi = 2. * v1;
352 103 const double ci = (0.5 * (v1 + v2) * (v2 - v1) / a2) - (p2 - p1);
353 103 const double deltai = bi * bi - 4.0 * ai * ci;
354
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 103 times.
 103 if (deltai < 0)
355 ✗ std::cout << "Error : determinant of quadratic function negative"
356 ✗ << std::endl;
357
358 103 const double x1i = (-bi + sqrt(deltai)) / (2 * ai);
359 103 const double x2i = (-bi - sqrt(deltai)) / (2 * ai);
360 103 const double xi = std::max(x1i, x2i);
361 hppDout(info, "quadratic equation solutions : " << x1i << " , " << x2i);
362 // min bound of infeasible interval is given by lesser solution
363 103 t1 = std::min(x1i, x2i);
364 103 infInterval->first = (((v2 - v1) / a2) + (t1)) + t1; // eq 14 (T = t2+t1)
365 // check if greater solution violate velocity limits :
366 103 minT1 = -minT1;
367
1/2
✓ Branch 0 taken 103 times.
✗ Branch 1 not taken.
 103 if (xi > minT1) {
368 103 twoSegment = true;
369 103 t1 = xi;
370 }
371
1/2
✓ Branch 0 taken 103 times.
✗ Branch 1 not taken.
 103 if (twoSegment) { // check if max velocity is respected
372
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 103 times.
 103 if (std::abs(v1 + (t1)*a1) > vMax) twoSegment = false;
373 }
374
1/2
✓ Branch 0 taken 103 times.
✗ Branch 1 not taken.
 103 if (twoSegment) { // compute t2 for two segment trajectory
375 103 tv = 0.;
376 103 t2 = ((v2 - v1) / a2) + (t1); // eq 14
377 } else { // compute 3 segment trajectory, with constant velocity phase :
378 ✗ t1 = (vLim - v1) / a1; // eq 15
379 ✗ tv = ((v1 * v1 + v2 * v2 - 2 * vLim * vLim) / (2 * vLim * a1)) +
380 ✗ (p2 - p1) / vLim; // eq 16
381 ✗ t2 = (v2 - vLim) / a2; // eq 17
382 }
383 103 infInterval->second = t1 + tv + t2;
384 hppDout(info, "! INFEASIBLE INTERVAL : [" << infInterval->first << " ; "
385 << infInterval->second << " ]");
386 }
387 }
388
389 4009 return T;
390 }
391
392 6021 void Kinodynamic::fixedTimeTrajectory(int index, double T, double p1, double p2,
393 double v1, double v2, double *a1,
394 double *t0, double *t1, double *tv,
395 double *t2, double *vLim) const {
396 hppDout(info, "p1 = " << p1 << " p2 = " << p2 << " ; v1 = " << v1
397 << " v2 = " << v2 << " T = " << T);
398 6021 double v12 = v1 + v2;
399 6021 double v2_1 = v2 - v1;
400 6021 double p2_1 = p2 - p1;
401
2/4
✓ Branch 0 taken 6021 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 6021 times.
✗ Branch 3 not taken.
 6021 assert(index >= 0 && index < 3 &&
402 "index of joint should be between in [0;2]");
403 6021 double vMax = vMax_[index];
404 6021 double aMax = std::abs(aMax_[index]);
405 hppDout(info,
406 "v12 = " << v12 << " ; v21 = " << v2_1 << " ; p21 = " << p2_1);
407 int sigma;
408 6021 double deltaPacc = 0.5 * (v1 + v2) * (fabs(v2 - v1) / aMax);
409 6021 sigma = sgnenum(p2 - p1 - deltaPacc);
410 hppDout(notice, "deltaPacc = " << deltaPacc);
411 hppDout(info, "sigma = " << sigma);
412
2/2
✓ Branch 0 taken 2012 times.
✓ Branch 1 taken 4009 times.
 6021 if (sigma == 0) {
413 2012 sigma = sgn(p2 - p1);
414 hppDout(info, "sigma Bis= " << sigma);
415 }
416
417
4/4
✓ Branch 1 taken 2020 times.
✓ Branch 2 taken 4001 times.
✓ Branch 3 taken 2012 times.
✓ Branch 4 taken 4009 times.
 8041 if (fabs(v2_1) < (std::numeric_limits<double>::epsilon() * 100.) &&
418
2/2
✓ Branch 1 taken 2012 times.
✓ Branch 2 taken 8 times.
 2020 fabs(p2_1) < (std::numeric_limits<double>::epsilon() * 100.)) {
419 2012 *a1 = 0;
420 2012 *t0 = 0;
421 2012 *t1 = 0;
422 2012 *tv = T;
423 2012 *t2 = 0;
424 2012 *vLim = 0;
425 2012 return;
426 }
427
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4009 times.
 4009 if (!synchronizeVerticalAxis_) { // if true : make the trajectory along z
428 // axis as short as possible (with the
429 // maximal acceleration) and then stay at
430 // null acceleration
431 ✗ if (index == 2 && /*((v1 == 0) ||*/ (v2 == 0)) { // FIXME : axis z ?
432 hppDout(notice, "FIXED TIME TRAJ for axis Z : ");
433 ✗ assert(index >= 0 && index < 3 &&
434 "index of joint should be between in [0;2]");
435 ✗ *a1 = (sigma)*aMax;
436 ✗ double a2 = -*a1;
437 ✗ (*vLim) = (sigma)*vMax;
438 hppDout(info, "Vlim = " << (*vLim) << " ; aMax = " << aMax);
439 // test if two segment trajectory is valid :
440 ✗ bool twoSegment = false;
441
442 // solve quadratic equation (cf eq 13 article)
443 ✗ const double a = *a1;
444 ✗ const double b = 2. * v1;
445 ✗ const double c = (0.5 * (v1 + v2) * (v2 - v1) / a2) - (p2 - p1);
446
447 ✗ const double q = -0.5 * (b + sgnf(b) * sqrt(b * b - 4 * a * c));
448 ✗ double x1 = 0;
449 ✗ double x2 = 0;
450 ✗ if (fabs(a) > std::numeric_limits<double>::epsilon() * 100.)
451 ✗ x1 = q / a;
452 else
453 ✗ x1 = sigma * aMax_[index];
454 ✗ if (fabs(q) > std::numeric_limits<double>::epsilon() * 100.)
455 ✗ x2 = c / q;
456 else
457 ✗ x2 = sigma * aMax_[index];
458
459 hppDout(info, "sign of " << b << " is : " << sgnf(b));
460
461 ✗ const double x = std::max(x1, x2);
462 hppDout(info,
463 "Solve quadratic equation : x1 = " << x1 << " ; x2 = " << x2);
464 hppDout(info, " x = " << x);
465 hppDout(info, "t1 before vel limit = " << x);
466
467 hppDout(info, "inf bound on t1 (from t2 > 0) " << -((v2 - v1) / a2));
468 ✗ double minT1 = std::max(
469 ✗ 0., -((v2 - v1) / a2)); // lower bound for valid t1 value (cf eq 14)
470
471 ✗ if (x >= minT1) {
472 ✗ twoSegment = true;
473 ✗ *t1 = x;
474 hppDout(info, "t1 >= minT1");
475 }
476 ✗ if (twoSegment) { // check if max velocity is respected
477 ✗ if (std::abs(v1 + (*t1) * (*a1)) > vMax) {
478 ✗ twoSegment = false;
479 hppDout(info, "Doesn't respect max velocity, need 3 segments");
480 }
481 }
482 ✗ if (twoSegment) { // compute t2 for two segment trajectory
483 ✗ *tv = 0.;
484 ✗ *t2 = ((v2 - v1) / a2) + (*t1); // eq 14
485 } else { // compute 3 segment trajectory, with constant velocity phase :
486 ✗ *t1 = ((*vLim) - v1) / (*a1); // eq 15
487 ✗ *tv = ((v1 * v1 + v2 * v2 - 2 * (*vLim) * (*vLim)) /
488 ✗ (2 * (*vLim) * (*a1))) +
489 ✗ (p2 - p1) / (*vLim); // eq 16
490 ✗ *t2 = (v2 - (*vLim)) / a2; // eq 17
491 }
492 if (twoSegment) {
493 hppDout(notice, "Trajectory with 2 segments");
494 } else {
495 hppDout(notice, "Trajectory with 3 segments");
496 }
497 hppDout(notice, "a1 = " << (*a1) << " ; a2 =" << a2);
498 ✗ if (v1 == 0)
499 ✗ *t0 = T - *t1 - *tv - *t2;
500 else
501 ✗ *t0 = 0;
502 hppDout(notice, "t0 = " << *t0);
503 hppDout(notice,
504 "t = " << (*t1) << " ; " << (*tv) << " ; " << (*t2));
505 hppDout(notice, "T = " << T);
506 ✗ return;
507 } else {
508 ✗ *t0 = 0.;
509 }
510 }
511 4009 *t0 = 0.;
512
513 // quadratic equation (20)
514 4009 double a = T * T;
515 4009 double b = 2 * T * (v12)-4 * p2_1;
516 4009 double c = -(v2_1 * v2_1);
517 4009 const double q = -0.5 * (b + sgnf(b) * sqrt(b * b - 4 * a * c));
518 4009 double x1 = 0;
519 4009 double x2 = 0;
520
1/2
✓ Branch 1 taken 4009 times.
✗ Branch 2 not taken.
 4009 if (fabs(a) > std::numeric_limits<double>::epsilon() * 100.)
521 4009 x1 = q / a;
522 else {
523 ✗ x1 = sigma * aMax_[index];
524 hppDout(notice, "a == 0, take x1 = aMax");
525 }
526
1/2
✓ Branch 1 taken 4009 times.
✗ Branch 2 not taken.
 4009 if (fabs(q) > std::numeric_limits<double>::epsilon() * 100.)
527 4009 x2 = c / q;
528 else {
529 ✗ x2 = sigma * aMax_[index];
530 hppDout(notice, "q == 0, take x2 = aMax");
531 }
532 hppDout(notice,
533 "epsilon = " << std::numeric_limits<double>::epsilon() * 100.);
534
535 hppDout(notice,
536 "a = " << a << " ; b = " << b << " ; c = " << c << " ; q = " << q);
537 hppDout(notice, "x1 = " << x1 << " ; x2 = " << x2);
538
1/2
✓ Branch 0 taken 4009 times.
✗ Branch 1 not taken.
 4009 if (fabs(x1) > fabs(x2))
539 4009 *a1 = x1;
540 else
541 ✗ *a1 = x2;
542
2/2
✓ Branch 1 taken 187 times.
✓ Branch 2 taken 3822 times.
 4009 if (fabs(*a1) > aMax_[index]) // x1 or x2 could be sligtly greater than aMax
543 // because of numerical imprecision
544 187 *a1 = aMax_[index] * sgn(*a1);
545 4009 double a2 = -(*a1);
546 hppDout(notice, "a1 = " << *a1);
547 4009 *t1 = 0.5 * ((v2_1 / (*a1)) + T);
548 4009 *vLim = sgn((*a1)) * vMax;
549 hppDout(notice, "vLim = " << *vLim);
550 hppDout(notice, "t1 before velocity limit : " << *t1);
551
2/2
✓ Branch 1 taken 2423 times.
✓ Branch 2 taken 1586 times.
 4009 if (std::abs(v1 + (*t1) * (*a1)) <= vMax) { // two segment trajectory
552 hppDout(notice, "Trajectory with 2 segments");
553 2423 *t2 = T - (*t1);
554 2423 *tv = 0.;
555 } else { // three segment trajectory
556 hppDout(notice, "Trajectory with 3 segments");
557 // adjust acceleration :
558 3172 if (fabs(q) >
559
1/2
✓ Branch 1 taken 1586 times.
✗ Branch 2 not taken.
 1586 std::numeric_limits<double>::epsilon() *
560 100.) // otherwise this mean that the solution have only a constant
561 // velocity segment, and the following equation will lead to
562 // a NaN because (*vLim*T- p2_1) == 0
563 1586 *a1 = ((*vLim - v1) * (*vLim - v1) + (*vLim - v2) * (*vLim - v2)) /
564 1586 (2 * (*vLim * T - p2_1));
565 else {
566 ✗ *a1 = sigma * aMax_[index];
567 hppDout(notice, "q == 0, take x1 = aMax");
568 }
569 3172 if (fabs(*a1) >
570
2/2
✓ Branch 1 taken 365 times.
✓ Branch 2 taken 1221 times.
 1586 aMax_[index]) // after this equations a1 could be slightly greater than
571 // aMax because of numerical imprecision
572 365 *a1 = aMax_[index] * sgn(*a1);
573
574 1586 a2 = -(*a1);
575 // compute new time intervals :
576 1586 *t1 = (*vLim - v1) / (*a1);
577 1586 *tv = (v1 * v1 + v2 * v2 - 2 * (*vLim) * (*vLim)) / (2 * (*vLim) * (*a1)) +
578 1586 (p2 - p1) / (*vLim);
579 1586 *t2 = (v2 - *vLim) / (a2);
580 }
581
582 hppDout(notice, "a1 = " << *a1 << " ; a2 =" << a2);
583 hppDout(notice, "t = " << (*t1) << " ; " << (*tv) << " ; " << (*t2));
584 }
585
586 18 HPP_START_PARAMETER_DECLARATION(Kinodynamic)
587
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Problem::declareParameter(ParameterDescription(
588 Parameter::FLOAT, "Kinodynamic/velocityBound",
589 "The maximal magnitude of the velocity along the trajectory. ",
590
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(1.)));
591
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Problem::declareParameter(ParameterDescription(
592 Parameter::FLOAT, "Kinodynamic/accelerationBound",
593 "The maximal magnitude of the acceleration along the trajectory. ",
594
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(10.)));
595
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Problem::declareParameter(ParameterDescription(
596 Parameter::FLOAT, "Kinodynamic/verticalAccelerationBound",
597 "The maximal magnitude of the acceleration along the z axis. ",
598
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(10.)));
599
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Problem::declareParameter(ParameterDescription(
600 Parameter::BOOL, "Kinodynamic/forceAllOrientation",
601 "If true, the orientation of the root is always aligned with the velocity",
602
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(false)));
603
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 Problem::declareParameter(
604
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
 36 ParameterDescription(Parameter::BOOL, "Kinodynamic/forceYawOrientation",
605 "If true, the yaw orientation (along z axis) of the "
606 "root is always aligned with the velocity",
607
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(false)));
608
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Problem::declareParameter(ParameterDescription(
609 Parameter::BOOL, "Kinodynamic/synchronizeVerticalAxis",
610 "If false, the acceleration along the vertical axis is not synchronized "
611 "wwith the other axis. This result in a greater vertical acceleration with "
612 "a longer phase at constant velocity, resulting in a motion with less "
613 "displacement along the vertical axis",
614
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(true)));
615 18 HPP_END_PARAMETER_DECLARATION(Kinodynamic)
616
617 } // namespace steeringMethod
618 } // namespace core
619 } // namespace hpp
620