Directory: | ./ |
---|---|
File: | src/steering-method/constant-curvature.cc |
Date: | 2024-12-13 16:14:03 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 142 | 309 | 46.0% |
Branches: | 108 | 443 | 24.4% |
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 | #include <boost/serialization/vector.hpp> | ||
31 | #include <boost/serialization/weak_ptr.hpp> | ||
32 | #include <hpp/core/steering-method/constant-curvature.hh> | ||
33 | #include <hpp/pinocchio/configuration.hh> | ||
34 | #include <hpp/pinocchio/device.hh> | ||
35 | #include <hpp/pinocchio/joint.hh> | ||
36 | #include <hpp/pinocchio/liegroup.hh> | ||
37 | #include <hpp/pinocchio/serialization.hh> | ||
38 | #include <hpp/util/serialization.hh> | ||
39 | #include <pinocchio/serialization/eigen.hpp> | ||
40 | #include <pinocchio/spatial/se3.hpp> | ||
41 | |||
42 | namespace hpp { | ||
43 | namespace core { | ||
44 | namespace steeringMethod { | ||
45 | namespace details { | ||
46 | using std::max; | ||
47 | |||
48 | /// \param t0, t1 angles between \f$ 0 \f$ and \f$ 2 \pi \f$. | ||
49 | ✗ | value_type absCosineMax(const value_type& t0, const value_type& t1) { | |
50 | ✗ | assert(0 <= t0 && t0 <= t1 && t1 <= 2 * M_PI); | |
51 | ✗ | if (t1 <= M_PI) return max(fabs(cos(t0)), fabs(cos(t1))); | |
52 | // M_PI < t1 <= 2*M_PI | ||
53 | ✗ | if (t0 <= M_PI) return 1.; | |
54 | // M_PI < t0 <= t1 <= 2*M_PI | ||
55 | ✗ | return max(fabs(cos(t0)), fabs(cos(t1))); | |
56 | } | ||
57 | |||
58 | /// \param t0, t1 angles between \f$ 0 \f$ and \f$ 2 \pi \f$. | ||
59 | ✗ | value_type absSineMax(const value_type& t0, const value_type& t1) { | |
60 | ✗ | assert(0 <= t0 && t0 <= t1 && t1 <= 2 * M_PI); | |
61 | ✗ | if (t1 <= M_PI_2) return sin(t1); | |
62 | ✗ | if (t0 <= M_PI_2) return 1.; | |
63 | ✗ | if (t0 >= 3 * M_PI_2) return -sin(t0); | |
64 | // M_PI_2 < t0 < 3*M_PI_2 | ||
65 | ✗ | if (t1 >= 3 * M_PI_2) return 1.; | |
66 | ✗ | return max(sin(t0), sin(t1)); | |
67 | } | ||
68 | } // namespace details | ||
69 | |||
70 | 17 | ConstantCurvaturePtr_t ConstantCurvature::create( | |
71 | const DevicePtr_t& robot, ConfigurationIn_t init, ConfigurationIn_t end, | ||
72 | value_type curveLength, value_type pathLength, value_type curvature, | ||
73 | size_type xyId, size_type rzId, const JointPtr_t rz, | ||
74 | const std::vector<JointPtr_t> wheels, | ||
75 | const ConstraintSetPtr_t& constraints) { | ||
76 | ConstantCurvature* ptr; | ||
77 |
2/2✓ Branch 1 taken 15 times.
✓ Branch 2 taken 2 times.
|
17 | if (constraints) |
78 |
2/4✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
|
30 | ptr = new ConstantCurvature(robot, init, end, curveLength, pathLength, |
79 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
15 | curvature, xyId, rzId, rz, wheels, constraints); |
80 | else | ||
81 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
|
4 | ptr = new ConstantCurvature(robot, init, end, curveLength, pathLength, |
82 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | curvature, xyId, rzId, rz, wheels); |
83 | 17 | ConstantCurvaturePtr_t shPtr(ptr); | |
84 |
1/2✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
|
17 | ptr->init(shPtr); |
85 | 17 | return shPtr; | |
86 | } | ||
87 | |||
88 | 12 | ConstantCurvaturePtr_t ConstantCurvature::createCopy( | |
89 | const ConstantCurvaturePtr_t& other) { | ||
90 |
1/2✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
|
12 | ConstantCurvature* ptr(new ConstantCurvature(*other)); |
91 | 12 | ConstantCurvaturePtr_t shPtr(ptr); | |
92 |
1/2✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
|
12 | ptr->init(shPtr); |
93 | 12 | return shPtr; | |
94 | } | ||
95 | |||
96 | ✗ | ConstantCurvaturePtr_t ConstantCurvature::createCopy( | |
97 | const ConstantCurvaturePtr_t& other, | ||
98 | const ConstraintSetPtr_t& constraints) { | ||
99 | ✗ | ConstantCurvature* ptr = new ConstantCurvature(*other, constraints); | |
100 | ✗ | ConstantCurvaturePtr_t shPtr(ptr); | |
101 | ✗ | ptr->init(shPtr); | |
102 | ✗ | return shPtr; | |
103 | } | ||
104 | |||
105 | /// Return a shared pointer to a copy of this | ||
106 | ✗ | PathPtr_t ConstantCurvature::copy() const { return createCopy(weak_.lock()); } | |
107 | |||
108 | ✗ | std::ostream& ConstantCurvature::print(std::ostream& os) const { | |
109 | ✗ | os << "-- ConstantCurvature" << std::endl; | |
110 | ✗ | os << "from " << initial_.transpose() << std::endl; | |
111 | ✗ | os << " length: " << forward_ * paramLength() << std::endl; | |
112 | ✗ | os << " curvature: " << curvature_ << std::endl; | |
113 | ✗ | return os; | |
114 | } | ||
115 | |||
116 | 15 | ConstantCurvature::ConstantCurvature( | |
117 | const DevicePtr_t& robot, ConfigurationIn_t init, ConfigurationIn_t end, | ||
118 | value_type curveLength, value_type pathLength, value_type curvature, | ||
119 | size_type xyId, size_type rzId, const JointPtr_t rz, | ||
120 | 15 | const std::vector<JointPtr_t> wheels, ConstraintSetPtr_t constraints) | |
121 | ✗ | : Path(std::make_pair(0., fabs(pathLength)), robot->configSize(), | |
122 | robot->numberDof(), constraints), | ||
123 | 15 | robot_(robot), | |
124 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | initial_(init), |
125 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | end_(end), |
126 | 15 | curveLength_(curveLength), | |
127 | 15 | curvature_(curvature), | |
128 | 15 | xyId_(xyId), | |
129 | 15 | rzId_(rzId), | |
130 |
4/6✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 15 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✓ Branch 12 taken 10 times.
|
30 | forward_(curveLength > 0 ? 1 : -1) { |
131 | // Find rank of translation and rotation in velocity vectors | ||
132 | // Hypothesis: degrees of freedom all belong to a planar joint or | ||
133 | // xyId_ belong to a tranlation joint, rzId_ belongs to a SO2 joint. | ||
134 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
15 | JointPtr_t joint(robot_->getJointAtConfigRank(xyId_)); |
135 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
15 | size_type offset(xyId_ - joint->rankInConfiguration()); |
136 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
15 | dxyId_ = joint->rankInVelocity() + offset; |
137 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
15 | joint = robot_->getJointAtConfigRank(rzId_); |
138 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
15 | offset = rzId_ - joint->rankInConfiguration(); |
139 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
15 | drzId_ = joint->rankInVelocity() + offset; |
140 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
|
15 | setWheelJoints(rz, wheels); |
141 |
2/4✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
|
15 | impl_compute(end_, paramRange().second); |
142 | 15 | } | |
143 | |||
144 | 2 | ConstantCurvature::ConstantCurvature( | |
145 | const DevicePtr_t& robot, ConfigurationIn_t init, ConfigurationIn_t end, | ||
146 | value_type curveLength, value_type pathLength, value_type curvature, | ||
147 | size_type xyId, size_type rzId, const JointPtr_t rz, | ||
148 | 2 | const std::vector<JointPtr_t> wheels) | |
149 | ✗ | : Path(std::make_pair(0., fabs(pathLength)), robot->configSize(), | |
150 | robot->numberDof()), | ||
151 | 2 | robot_(robot), | |
152 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | initial_(init), |
153 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | end_(end), |
154 | 2 | curveLength_(curveLength), | |
155 | 2 | curvature_(curvature), | |
156 | 2 | xyId_(xyId), | |
157 | 2 | rzId_(rzId), | |
158 |
3/6✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 2 times.
|
4 | forward_(curveLength > 0 ? 1 : -1) { |
159 | // Find rank of translation and rotation in velocity vectors | ||
160 | // Hypothesis: degrees of freedom all belong to a planar joint or | ||
161 | // xyId_ belong to a tranlation joint, rzId_ belongs to a SO2 joint. | ||
162 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | JointPtr_t joint(robot_->getJointAtConfigRank(xyId_)); |
163 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | size_type offset(xyId_ - joint->rankInConfiguration()); |
164 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | dxyId_ = joint->rankInVelocity() + offset; |
165 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | joint = robot_->getJointAtConfigRank(rzId_); |
166 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | offset = rzId_ - joint->rankInConfiguration(); |
167 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | drzId_ = joint->rankInVelocity() + offset; |
168 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
2 | setWheelJoints(rz, wheels); |
169 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
2 | impl_compute(end_, paramRange().second); |
170 | 2 | } | |
171 | |||
172 | 12 | ConstantCurvature::ConstantCurvature(const ConstantCurvature& other) | |
173 | : Path(other), | ||
174 | 12 | robot_(other.robot_), | |
175 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | initial_(other.initial_), |
176 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | end_(other.end_), |
177 | 12 | curveLength_(other.curveLength_), | |
178 | 12 | curvature_(other.curvature_), | |
179 | 12 | xyId_(other.xyId_), | |
180 | 12 | rzId_(other.rzId_), | |
181 | 12 | dxyId_(other.dxyId_), | |
182 | 12 | drzId_(other.drzId_), | |
183 | 12 | forward_(other.forward_), | |
184 |
1/2✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
|
24 | wheels_(other.wheels_) {} |
185 | |||
186 | ✗ | ConstantCurvature::ConstantCurvature(const ConstantCurvature& other, | |
187 | ✗ | const ConstraintSetPtr_t& constraints) | |
188 | : parent_t(other, constraints), | ||
189 | ✗ | robot_(other.robot_), | |
190 | ✗ | initial_(other.initial_), | |
191 | ✗ | end_(other.end_), | |
192 | ✗ | curveLength_(other.curveLength_), | |
193 | ✗ | curvature_(other.curvature_), | |
194 | ✗ | xyId_(other.xyId_), | |
195 | ✗ | rzId_(other.rzId_), | |
196 | ✗ | dxyId_(other.dxyId_), | |
197 | ✗ | drzId_(other.drzId_), | |
198 | ✗ | forward_(other.forward_), | |
199 | ✗ | wheels_(other.wheels_) {} | |
200 | |||
201 | 379 | bool ConstantCurvature::impl_compute(ConfigurationOut_t result, | |
202 | value_type param) const { | ||
203 | 379 | const value_type L = paramLength(); | |
204 | // Does a linear interpolation on all the joints. | ||
205 |
2/2✓ Branch 0 taken 376 times.
✓ Branch 1 taken 3 times.
|
379 | const value_type u = (L == 0) ? 0 : ((param - paramRange().first) / L); |
206 |
4/8✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 379 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 379 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 379 times.
✗ Branch 11 not taken.
|
379 | pinocchio::interpolate<pinocchio::RnxSOnLieGroupMap>(robot_, initial_, end_, |
207 | u, result); | ||
208 | |||
209 | 379 | value_type t(u * curveLength_); | |
210 |
2/4✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 379 times.
✗ Branch 5 not taken.
|
379 | value_type x0(initial_[xyId_ + 0]), y0(initial_[xyId_ + 1]); |
211 |
2/4✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 379 times.
✗ Branch 5 not taken.
|
379 | value_type c0(initial_[rzId_ + 0]), s0(initial_[rzId_ + 1]); |
212 | value_type x, y; | ||
213 | 379 | value_type c(cos(curvature_ * t)), s(sin(curvature_ * t)); | |
214 | |||
215 |
2/2✓ Branch 0 taken 271 times.
✓ Branch 1 taken 108 times.
|
379 | if (curvature_ == 0) { |
216 | 271 | x = x0 + t * c0; | |
217 | 271 | y = y0 + t * s0; | |
218 | } else { | ||
219 | 108 | value_type r(1. / curvature_); | |
220 | 108 | x = x0 + r * (s0 * (c - 1) + c0 * s); | |
221 | 108 | y = y0 + r * (c0 * (1 - c) + s0 * s); | |
222 | } | ||
223 |
1/2✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
|
379 | result[xyId_ + 0] = x; |
224 |
1/2✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
|
379 | result[xyId_ + 1] = y; |
225 |
1/2✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
|
379 | result[rzId_ + 0] = c0 * c - s0 * s; |
226 |
1/2✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
|
379 | result[rzId_ + 1] = c0 * s + s0 * c; |
227 | |||
228 | // Set wheel joint positions | ||
229 | 379 | for (std::vector<Wheels_t>::const_iterator w = wheels_.begin(); | |
230 |
2/2✓ Branch 2 taken 2 times.
✓ Branch 3 taken 379 times.
|
381 | w < wheels_.end(); ++w) { |
231 |
2/4✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
2 | result[w->j->rankInConfiguration()] = w->value; |
232 | } | ||
233 | 379 | return true; | |
234 | } | ||
235 | |||
236 | ✗ | void ConstantCurvature::impl_derivative(vectorOut_t result, | |
237 | const value_type& param, | ||
238 | size_type order) const { | ||
239 | ✗ | const value_type L = paramLength(); | |
240 | // Does a linear interpolation on all the joints. | ||
241 | ✗ | const value_type u = (L == 0) ? 0 : ((param - paramRange().first) / L); | |
242 | ✗ | if (order == 1) { | |
243 | ✗ | pinocchio::difference<pinocchio::RnxSOnLieGroupMap>(robot_, end_, initial_, | |
244 | result); | ||
245 | ✗ | result /= L; | |
246 | ✗ | } else if (order > 1) { | |
247 | ✗ | result.setZero(); | |
248 | } | ||
249 | |||
250 | ✗ | value_type t(u * curveLength_); | |
251 | ✗ | value_type beta(curveLength_ / L); | |
252 | ✗ | if (L == 0) beta = 1; | |
253 | ✗ | value_type alpha(fabs(beta)); | |
254 | ✗ | if (forward_ == -1 && order % 2 == 1) alpha *= -1; | |
255 | ✗ | value_type c0(initial_[rzId_ + 0]), s0(initial_[rzId_ + 1]); | |
256 | ✗ | value_type dx, dy, dtheta = 0; | |
257 | ✗ | value_type c(cos(curvature_ * t)), s(sin(curvature_ * t)); | |
258 | |||
259 | ✗ | if (order <= 0) { | |
260 | ✗ | std::ostringstream oss; | |
261 | ✗ | oss << "order of derivative (" << order << ") should be positive."; | |
262 | ✗ | throw std::runtime_error(oss.str().c_str()); | |
263 | } | ||
264 | ✗ | if (order == 1) { | |
265 | ✗ | dx = alpha * (c0 * c - s0 * s); | |
266 | ✗ | dy = alpha * (c0 * s + s0 * c); | |
267 | ✗ | dtheta = alpha * curvature_; | |
268 | } else { | ||
269 | ✗ | switch (order % 4) { | |
270 | ✗ | case 2: | |
271 | ✗ | dx = alpha * pow(curvature_ * beta, (value_type)(order - 1)) * | |
272 | ✗ | (c0 * s + s0 * c); | |
273 | ✗ | dy = -alpha * pow(curvature_ * beta, (value_type)(order - 1)) * | |
274 | ✗ | (c0 * c - s0 * s); | |
275 | ✗ | break; | |
276 | ✗ | case 3: | |
277 | ✗ | dx = -alpha * pow(curvature_ * beta, (value_type)(order - 1)) * | |
278 | ✗ | (c0 * c - s0 * s); | |
279 | ✗ | dy = -alpha * pow(curvature_ * beta, (value_type)(order - 1)) * | |
280 | ✗ | (c0 * s + s0 * c); | |
281 | ✗ | break; | |
282 | ✗ | case 0: | |
283 | ✗ | dx = alpha * pow(curvature_ * beta, (value_type)(order - 1)) * | |
284 | ✗ | (c0 * s + s0 * c); | |
285 | ✗ | dy = -alpha * pow(curvature_ * beta, (value_type)(order - 1)) * | |
286 | ✗ | (c0 * c - s0 * s); | |
287 | ✗ | break; | |
288 | ✗ | case 1: | |
289 | ✗ | dx = alpha * pow(curvature_ * beta, (value_type)(order - 1)) * | |
290 | ✗ | (c0 * c - s0 * s); | |
291 | ✗ | dy = alpha * pow(curvature_ * beta, (value_type)(order - 1)) * | |
292 | ✗ | (c0 * s + s0 * c); | |
293 | ✗ | break; | |
294 | } | ||
295 | } | ||
296 | |||
297 | ✗ | result[dxyId_ + 0] = dx; | |
298 | ✗ | result[dxyId_ + 1] = dy; | |
299 | ✗ | result[drzId_] = dtheta; | |
300 | // Express velocity in local frame | ||
301 | ✗ | Eigen::Matrix<value_type, 2, 2> R; | |
302 | ✗ | R.col(0) << c0 * c - s0 * s, c0 * s + s0 * c; | |
303 | ✗ | R.col(1) << -R(1, 0), R(0, 0); | |
304 | ✗ | result.segment<2>(dxyId_) = R.transpose() * result.segment<2>(dxyId_); | |
305 | |||
306 | // Set wheel joint velocities | ||
307 | ✗ | for (std::vector<Wheels_t>::const_iterator w = wheels_.begin(); | |
308 | ✗ | w < wheels_.end(); ++w) { | |
309 | ✗ | result[w->j->rankInVelocity()] = 0; | |
310 | } | ||
311 | } | ||
312 | |||
313 | ✗ | void ConstantCurvature::impl_velocityBound(vectorOut_t bound, | |
314 | const value_type& param0, | ||
315 | const value_type& param1) const { | ||
316 | // Does a linear interpolation on all the joints. | ||
317 | ✗ | if (paramRange().first == paramRange().second) { | |
318 | ✗ | bound.setZero(); | |
319 | ✗ | return; | |
320 | } | ||
321 | ✗ | const value_type L = paramLength(); | |
322 | ✗ | assert(L > 0); | |
323 | ✗ | pinocchio::difference<pinocchio::RnxSOnLieGroupMap>(robot_, end_, initial_, | |
324 | bound); | ||
325 | ✗ | bound.noalias() = bound.cwiseAbs() / L; | |
326 | |||
327 | ✗ | value_type alpha(fabs(curveLength_) / L); | |
328 | |||
329 | // Compute max of | ||
330 | // dx = alpha * (c0 * c - s0 * s); | ||
331 | // dy = alpha * (c0 * s + s0 * c); | ||
332 | // dtheta = alpha * curvature_; | ||
333 | // onto [ param0, param1 ] | ||
334 | ✗ | value_type Mx = std::numeric_limits<value_type>::quiet_NaN(), | |
335 | ✗ | My = std::numeric_limits<value_type>::quiet_NaN(); | |
336 | |||
337 | ✗ | if (curvature_ != 0) { | |
338 | ✗ | value_type t0 = curvature_ * curveLength_ * (param0 - paramRange().first) / | |
339 | ✗ | L, | |
340 | ✗ | t1 = curvature_ * curveLength_ * (param1 - paramRange().first) / | |
341 | ✗ | L; | |
342 | ✗ | if (t0 > t1) std::swap(t0, t1); | |
343 | ✗ | if (t1 - t0 >= M_PI) | |
344 | ✗ | Mx = My = 1; | |
345 | else { | ||
346 | // Compute thetaI | ||
347 | ✗ | const value_type c0(initial_[rzId_ + 0]), s0(initial_[rzId_ + 1]); | |
348 | ✗ | const value_type tI(atan2(s0, c0)); | |
349 | ✗ | t0 += tI; | |
350 | ✗ | t1 += tI; | |
351 | // max(|c0 * c - s0 * s|) = max(|cos(t)|) for t in [t0, t1] | ||
352 | // max(|c0 * s + s0 * c|) = max(|sin(t)|) for t in [t0, t1] | ||
353 | |||
354 | ✗ | value_type kd = std::floor(t0 * M_1_PI / 2) * 2 * M_PI; | |
355 | ✗ | t0 -= kd; | |
356 | ✗ | t1 -= kd; | |
357 | ✗ | assert(0 <= t0 && t0 <= t1 && t1 <= 2 * M_PI); | |
358 | |||
359 | ✗ | Mx = details::absCosineMax(t0, t1); | |
360 | ✗ | My = details::absSineMax(t0, t1); | |
361 | } | ||
362 | } else { | ||
363 | ✗ | Mx = 1.; | |
364 | ✗ | My = 0.; | |
365 | } | ||
366 | ✗ | assert(Mx >= 0. && My >= 0.); | |
367 | ✗ | assert(Mx <= 1. && My <= 1.); | |
368 | |||
369 | ✗ | bound[dxyId_ + 0] = alpha * Mx; | |
370 | ✗ | bound[dxyId_ + 1] = alpha * My; | |
371 | ✗ | bound[drzId_] = alpha * std::fabs(curvature_); | |
372 | |||
373 | // Set wheel joint velocities | ||
374 | ✗ | for (std::vector<Wheels_t>::const_iterator w = wheels_.begin(); | |
375 | ✗ | w < wheels_.end(); ++w) { | |
376 | ✗ | bound[w->j->rankInVelocity()] = 0; | |
377 | } | ||
378 | } | ||
379 | |||
380 | 4 | PathPtr_t ConstantCurvature::impl_extract( | |
381 | const interval_t& paramInterval) const { | ||
382 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
|
4 | assert(!timeParameterization()); |
383 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
|
4 | assert(timeRange().second - timeRange().first >= 0); |
384 | 4 | value_type L(timeRange().second - timeRange().first); | |
385 | 4 | value_type tmin(paramInterval.first), tmax(paramInterval.second); | |
386 | 4 | value_type curveLength = 0, pathLength = 0; | |
387 |
1/2✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
|
4 | if (L != 0) { |
388 | 4 | curveLength = ((tmax - tmin) / L * curveLength_); | |
389 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | pathLength = fabs(tmax - tmin) / L * length(); |
390 | } | ||
391 |
4/8✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 4 times.
✗ Branch 13 not taken.
|
4 | Configuration_t init(robot_->configSize()), end(robot_->configSize()); |
392 | 4 | auto tr = timeRange(); | |
393 | 4 | bool res(true); | |
394 |
1/2✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
|
4 | if (tmin == tr.first) |
395 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | init = initial_; |
396 | ✗ | else if (tmin == tr.second) | |
397 | ✗ | init = end_; | |
398 | else | ||
399 | ✗ | res = impl_compute(init, tmin); | |
400 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
|
4 | assert(res); |
401 | (void)res; | ||
402 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
|
4 | if (tmax == tr.first) |
403 | ✗ | end = initial_; | |
404 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
|
4 | else if (tmax == tr.second) |
405 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | end = end_; |
406 | else | ||
407 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | res = impl_compute(end, tmax); |
408 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
|
4 | assert(res); |
409 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | ConstantCurvaturePtr_t result(createCopy(weak_.lock())); |
410 | // swap to avoid memory allocation. | ||
411 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | result->initial_.swap(init); |
412 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | result->end_.swap(end); |
413 | 4 | result->curveLength_ = curveLength; | |
414 |
1/2✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | result->timeRange(interval_t(0, pathLength)); |
415 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
|
4 | result->forward_ = curveLength > 0 ? 1 : -1; |
416 | 8 | return result; | |
417 | 4 | } | |
418 | |||
419 | 8 | PathPtr_t ConstantCurvature::reverse() const { | |
420 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
|
8 | assert(!timeParameterization()); |
421 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
|
8 | assert(timeRange().second - timeRange().first >= 0); |
422 | |||
423 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | ConstantCurvaturePtr_t result(createCopy(weak_.lock())); |
424 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | result->initial_ = end_; |
425 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | result->end_ = initial_; |
426 | 8 | result->curveLength_ = -curveLength_; | |
427 |
2/4✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
|
8 | result->timeRange(interval_t(0, length())); |
428 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
|
8 | result->forward_ = curveLength_ < 0 ? 1 : -1; |
429 | 16 | return result; | |
430 | 8 | } | |
431 | |||
432 | 2 | inline value_type meanBounds(const JointPtr_t& j, const size_type& i) { | |
433 | 2 | return (j->upperBound(i) + j->lowerBound(i)) / 2; | |
434 | } | ||
435 | |||
436 | ✗ | inline value_type saturate(const value_type& v, const JointPtr_t& j, | |
437 | const size_type& i) { | ||
438 | ✗ | return std::min(j->upperBound(i), std::max(j->lowerBound(i), v)); | |
439 | } | ||
440 | |||
441 | 17 | void ConstantCurvature::setWheelJoints(const JointPtr_t rz, | |
442 | const std::vector<JointPtr_t> wheels) { | ||
443 |
2/4✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
|
17 | Transform3s zt(rz->currentTransformation().inverse()); |
444 |
1/2✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
|
17 | wheels_.resize(wheels.size()); |
445 | 17 | std::size_t rk = 0; | |
446 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 11 times.
|
17 | if (curvature_ == 0) { |
447 | 6 | for (std::vector<JointPtr_t>::const_iterator _wheels = wheels.begin(); | |
448 |
2/2✓ Branch 2 taken 2 times.
✓ Branch 3 taken 6 times.
|
8 | _wheels != wheels.end(); ++_wheels) { |
449 | 2 | wheels_[rk].j = *_wheels; | |
450 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | wheels_[rk].value = meanBounds(wheels_[rk].j, 0); |
451 | 2 | ++rk; | |
452 | } | ||
453 | } else { | ||
454 | 11 | value_type rho(1. / curvature_); | |
455 | 11 | for (std::vector<JointPtr_t>::const_iterator _wheels = wheels.begin(); | |
456 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 11 times.
|
11 | _wheels != wheels.end(); ++_wheels) { |
457 | ✗ | wheels_[rk].j = *_wheels; | |
458 | ✗ | wheels_[rk].value = meanBounds(wheels_[rk].j, 0); | |
459 | const vector3_t wheelPos = | ||
460 | ✗ | zt.act(wheels_[rk].j->currentTransformation().translation()); | |
461 | ✗ | const value_type value(std::atan(wheelPos[0] / (rho - wheelPos[1]))); | |
462 | ✗ | wheels_[rk].value = | |
463 | ✗ | saturate(meanBounds(wheels_[rk].j, 0) + value, *_wheels, 0); | |
464 | ✗ | ++rk; | |
465 | } | ||
466 | } | ||
467 | 17 | } | |
468 | |||
469 | template <class Archive> | ||
470 | ✗ | void ConstantCurvature::serialize(Archive& ar, const unsigned int version) { | |
471 | using namespace boost::serialization; | ||
472 | (void)version; | ||
473 | ✗ | ar& make_nvp("base", base_object<Path>(*this)); | |
474 | ✗ | ar& BOOST_SERIALIZATION_NVP(robot_); | |
475 | ✗ | serialization::remove_duplicate::serialize_vector(ar, "initial", initial_, | |
476 | version); | ||
477 | ✗ | serialization::remove_duplicate::serialize_vector(ar, "end", end_, version); | |
478 | ✗ | ar& BOOST_SERIALIZATION_NVP(curveLength_); | |
479 | ✗ | ar& make_nvp("curvature_", const_cast<value_type&>(curvature_)); | |
480 | ✗ | ar& make_nvp("xyId_", const_cast<size_type&>(xyId_)); | |
481 | ✗ | ar& make_nvp("rzId_", const_cast<size_type&>(rzId_)); | |
482 | ✗ | ar& BOOST_SERIALIZATION_NVP(dxyId_); | |
483 | ✗ | ar& BOOST_SERIALIZATION_NVP(drzId_); | |
484 | ✗ | ar& BOOST_SERIALIZATION_NVP(forward_); | |
485 | ✗ | ar& BOOST_SERIALIZATION_NVP(wheels_); | |
486 | ✗ | ar& BOOST_SERIALIZATION_NVP(weak_); | |
487 | } | ||
488 | |||
489 | HPP_SERIALIZATION_IMPLEMENT(ConstantCurvature); | ||
490 | } // namespace steeringMethod | ||
491 | } // namespace core | ||
492 | } // namespace hpp | ||
493 | |||
494 | namespace boost { | ||
495 | namespace serialization { | ||
496 | template <class Archive> | ||
497 | ✗ | void serialize(Archive& ar, | |
498 | hpp::core::steeringMethod::ConstantCurvature::Wheels_t& c, | ||
499 | const unsigned int version) { | ||
500 | (void)version; | ||
501 | ✗ | ar& make_nvp("value", c.value); | |
502 | ✗ | ar& make_nvp("joint", c.j); | |
503 | } | ||
504 | } // namespace serialization | ||
505 | } // namespace boost | ||
506 | |||
507 | 18 | BOOST_CLASS_EXPORT(hpp::core::steeringMethod::ConstantCurvature) | |
508 |