| Directory: | ./ |
|---|---|
| File: | src/kinodynamic-distance.cc |
| Date: | 2024-12-13 16:14:03 |
| 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 |