Directory: | ./ |
---|---|
File: | src/steering-method/steering-kinodynamic.cc |
Date: | 2024-08-10 11:29:48 |
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 |