Directory: | ./ |
---|---|
File: | src/kinodynamic-distance.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 60 | 82 | 73.2% |
Branches: | 30 | 62 | 48.4% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2014 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 <Eigen/SVD> | ||
31 | #include <hpp/core/kinodynamic-distance.hh> | ||
32 | #include <hpp/core/problem.hh> | ||
33 | #include <hpp/pinocchio/body.hh> | ||
34 | #include <hpp/pinocchio/configuration.hh> | ||
35 | #include <hpp/pinocchio/device.hh> | ||
36 | #include <hpp/pinocchio/joint.hh> | ||
37 | #include <hpp/util/debug.hh> | ||
38 | #include <limits> | ||
39 | |||
40 | namespace hpp { | ||
41 | namespace core { | ||
42 | |||
43 | ✗ | KinodynamicDistancePtr_t KinodynamicDistance::create(const DevicePtr_t& robot) { | |
44 | ✗ | KinodynamicDistance* ptr = new KinodynamicDistance(robot); | |
45 | ✗ | KinodynamicDistancePtr_t shPtr(ptr); | |
46 | ✗ | ptr->init(shPtr); | |
47 | ✗ | return shPtr; | |
48 | } | ||
49 | |||
50 | 3 | KinodynamicDistancePtr_t KinodynamicDistance::createFromProblem( | |
51 | const ProblemConstPtr_t& problem) { | ||
52 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | KinodynamicDistance* ptr = new KinodynamicDistance(problem); |
53 | 3 | KinodynamicDistancePtr_t shPtr(ptr); | |
54 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | ptr->init(shPtr); |
55 | 3 | return shPtr; | |
56 | } | ||
57 | |||
58 | ✗ | KinodynamicDistancePtr_t KinodynamicDistance::createCopy( | |
59 | const KinodynamicDistancePtr_t& distance) { | ||
60 | ✗ | KinodynamicDistance* ptr = new KinodynamicDistance(*distance); | |
61 | ✗ | KinodynamicDistancePtr_t shPtr(ptr); | |
62 | ✗ | ptr->init(shPtr); | |
63 | ✗ | return shPtr; | |
64 | } | ||
65 | |||
66 | ✗ | DistancePtr_t KinodynamicDistance::clone() const { | |
67 | ✗ | return createCopy(weak_.lock()); | |
68 | } | ||
69 | |||
70 | ✗ | KinodynamicDistance::KinodynamicDistance(const DevicePtr_t& robot) | |
71 | ✗ | : robot_(robot) { | |
72 | ✗ | if (robot_->extraConfigSpace().dimension() < 6) { | |
73 | ✗ | std::cout << "Error : you need at least 6 extra DOF" << std::endl; | |
74 | hppDout(error, "Error : you need at least 6 extra DOF"); | ||
75 | } | ||
76 | hppDout(warning, | ||
77 | "Kinodynamic distance create from robot, cannot access user-defined " | ||
78 | "velocity and acceleration bounds. Use default values"); | ||
79 | ✗ | aMax_ = 10.; | |
80 | ✗ | vMax_ = 1.; | |
81 | } | ||
82 | |||
83 | 3 | KinodynamicDistance::KinodynamicDistance(const ProblemConstPtr_t& problem) | |
84 | 3 | : robot_(problem->robot()) { | |
85 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
|
3 | if (robot_->extraConfigSpace().dimension() < 6) { |
86 | ✗ | std::cout << "Error : you need at least 6 extra DOF" << std::endl; | |
87 | hppDout(error, "Error : you need at least 6 extra DOF"); | ||
88 | } | ||
89 |
3/6✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
|
3 | aMax_ = problem->getParameter("Kinodynamic/accelerationBound").floatValue(); |
90 |
3/6✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
|
3 | vMax_ = problem->getParameter("Kinodynamic/velocityBound").floatValue(); |
91 | 3 | } | |
92 | |||
93 | ✗ | KinodynamicDistance::KinodynamicDistance(const KinodynamicDistance& distance) | |
94 | ✗ | : robot_(distance.robot_) {} | |
95 | |||
96 | 3 | void KinodynamicDistance::init(KinodynamicDistanceWkPtr_t self) { | |
97 | 3 | weak_ = self; | |
98 | 3 | } | |
99 | |||
100 | inline double sgnenum(double val) { return ((0. < val) - (val < 0.)); } | ||
101 | |||
102 |
2/2✓ Branch 0 taken 4000 times.
✓ Branch 1 taken 2015 times.
|
6015 | inline int sgn(double d) { return d >= 0.0 ? 1 : -1; } |
103 | |||
104 |
2/2✓ Branch 0 taken 1963 times.
✓ Branch 1 taken 2042 times.
|
4005 | inline double sgnf(double d) { return d >= 0.0 ? 1.0 : -1.0; } |
105 | |||
106 | 6015 | double KinodynamicDistance::computeMinTime(double p1, double p2, double v1, | |
107 | double v2) const { | ||
108 | // hppDout(info,"p1 = "<<p1<<" p2 = "<<p2<<" ; v1 = "<<v1<<" v2 = | ||
109 | // "<<v2); | ||
110 | // compute the sign of each acceleration | ||
111 | double t1, t2, tv; | ||
112 | int sigma; | ||
113 | 6015 | double deltaPacc = 0.5 * (v1 + v2) * (fabs(v2 - v1) / aMax_); | |
114 | 6015 | sigma = sgn(p2 - p1 - deltaPacc); | |
115 | // hppDout(info,"sigma = "<<sigma); | ||
116 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 6015 times.
|
6015 | if (sigma == 0) { |
117 | ✗ | sigma = sgn(p2 - p1); | |
118 | // hppDout(info,"sigma Bis= "<<sigma); | ||
119 | } | ||
120 | 6015 | double a1 = (sigma)*aMax_; | |
121 | 6015 | double a2 = -a1; | |
122 | 6015 | double vLim = (sigma)*vMax_; | |
123 | // hppDout(info,"Vlim = "<<vLim<<" ; aMax = "<<aMax_); | ||
124 |
4/4✓ Branch 1 taken 2011 times.
✓ Branch 2 taken 4004 times.
✓ Branch 3 taken 2010 times.
✓ Branch 4 taken 4005 times.
|
8026 | if (fabs(p2 - p1) < (std::numeric_limits<double>::epsilon() * 100.) && |
125 |
2/2✓ Branch 1 taken 2010 times.
✓ Branch 2 taken 1 times.
|
2011 | fabs(v2 - v1) < (std::numeric_limits<double>::epsilon() * 100.)) { |
126 | // hppDout(notice,"No movement in this joints, abort."); | ||
127 | 2010 | return 0.; | |
128 | } | ||
129 | // test if two segment trajectory is valid : | ||
130 | 4005 | bool twoSegment = false; | |
131 | |||
132 | // solve quadratic equation (cf eq 13 article) | ||
133 | 4005 | const double a = a1; | |
134 | 4005 | const double b = 2. * v1; | |
135 | 4005 | const double c = (0.5 * (v1 + v2) * (v2 - v1) / a2) - (p2 - p1); | |
136 | |||
137 | 4005 | const double q = -0.5 * (b + sgnf(b) * sqrt(b * b - 4 * a * c)); | |
138 | // hppDout(info, "sign of "<<b<<" is : "<<sgnf(b)); | ||
139 | 4005 | const double x1 = q / a; | |
140 | 4005 | const double x2 = c / q; | |
141 | 4005 | const double x = std::max(x1, x2); | |
142 | // hppDout(info,"Solve quadratic equation : x1 = "<<x1<<" ; x2 = "<<x2); | ||
143 | // hppDout(info," x = "<<x); | ||
144 | // hppDout(info,"t1 before vel limit = "<<x); | ||
145 | |||
146 | // hppDout(info,"inf bound on t1 (from t2 > 0) "<<-((v2-v1)/a2)); | ||
147 | 4005 | double minT1 = std::max( | |
148 | 4005 | 0., -((v2 - v1) / a2)); // lower bound for valid t1 value (cf eq 14) | |
149 | |||
150 |
1/2✓ Branch 0 taken 4005 times.
✗ Branch 1 not taken.
|
4005 | if (x >= minT1) { |
151 | 4005 | twoSegment = true; | |
152 | 4005 | t1 = x; | |
153 | // hppDout(info,"t1 >= minT1"); | ||
154 | } | ||
155 |
1/2✓ Branch 0 taken 4005 times.
✗ Branch 1 not taken.
|
4005 | if (twoSegment) { // check if max velocity is respected |
156 |
2/2✓ Branch 1 taken 2150 times.
✓ Branch 2 taken 1855 times.
|
4005 | if (std::abs(v1 + (t1)*a1) > vMax_) { |
157 | 2150 | twoSegment = false; | |
158 | // hppDout(info,"Doesn't respect max velocity, need 3 segments"); | ||
159 | } | ||
160 | } | ||
161 |
2/2✓ Branch 0 taken 1855 times.
✓ Branch 1 taken 2150 times.
|
4005 | if (twoSegment) { // compute t2 for two segment trajectory |
162 | 1855 | tv = 0.; | |
163 | 1855 | t2 = ((v2 - v1) / a2) + (t1); // eq 14 | |
164 | } else { // compute 3 segment trajectory, with constant velocity phase : | ||
165 | 2150 | t1 = (vLim - v1) / a1; // eq 15 | |
166 | 2150 | tv = ((v1 * v1 + v2 * v2 - 2 * vLim * vLim) / (2 * vLim * a1)) + | |
167 | 2150 | (p2 - p1) / vLim; // eq 16 | |
168 | 2150 | t2 = (v2 - vLim) / a2; // eq 17 | |
169 | } | ||
170 | /* | ||
171 | if(twoSegment){ | ||
172 | hppDout(notice,"Trajectory with 2 segments"); | ||
173 | }else{ | ||
174 | hppDout(notice,"Trajectory with 3 segments"); | ||
175 | } | ||
176 | */ | ||
177 | // hppDout(notice,"a1 = "<<a1<<" ; a2 ="<<a2); | ||
178 | // hppDout(notice,"t = "<<(t1)<<" ; "<<(tv)<<" ; "<<(t2)); | ||
179 | // double T = (t1)+(tv)+(t2); | ||
180 | // hppDout(notice,"T = "<<T); | ||
181 | 4005 | return (t1) + (tv) + (t2); | |
182 | } | ||
183 | |||
184 | 2005 | value_type KinodynamicDistance::impl_distance(ConfigurationIn_t q1, | |
185 | ConfigurationIn_t q2) const { | ||
186 | 2005 | double Tmax = 0; | |
187 | 2005 | double T = 0; | |
188 | |||
189 | size_type configSize = | ||
190 | 2005 | robot_->configSize() - robot_->extraConfigSpace().dimension(); | |
191 | // looking for Tmax | ||
192 | // hppDout(notice,"KinodynamicDistance : Looking for Tmax :"); | ||
193 | // hppDout(info,"Distance between : "<<pinocchio::displayConfig(q1)); | ||
194 | // hppDout(info,"and : "<<pinocchio::displayConfig(q2)); | ||
195 | |||
196 |
2/2✓ Branch 0 taken 6015 times.
✓ Branch 1 taken 2005 times.
|
8020 | for (int indexConfig = 0; indexConfig < 3; |
197 | indexConfig++) { // FIX ME : only work with freeflyer | ||
198 | // hppDout(notice,"For joint | ||
199 | // :"<<robot_->getJointAtConfigRank(indexConfig)->name()); | ||
200 | // if(robot_->getJointAtConfigRank(indexConfig)->name() != "base_joint_SO3" | ||
201 | // true){ | ||
202 | 6015 | T = computeMinTime(q1[indexConfig], q2[indexConfig], | |
203 | 6015 | q1[indexConfig + configSize], | |
204 | 6015 | q2[indexConfig + configSize]); | |
205 |
2/2✓ Branch 0 taken 3028 times.
✓ Branch 1 taken 2987 times.
|
6015 | if (T > Tmax) Tmax = T; |
206 | /*}else{ | ||
207 | hppDout(notice,"!! Kinodynamic distance for quaternion not implemented | ||
208 | yet."); | ||
209 | }*/ | ||
210 | } | ||
211 | // hppDout(info," is : "<<Tmax); | ||
212 | 2005 | return Tmax; | |
213 | } | ||
214 | } // namespace core | ||
215 | } // namespace hpp | ||
216 |