| Directory: | ./ |
|---|---|
| File: | include/hpp/core/kinodynamic-path.hh |
| Date: | 2024-08-10 11:29:48 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 24 | 48 | 50.0% |
| Branches: | 23 | 74 | 31.1% |
| 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 | #ifndef HPP_CORE_KINODYNAMIC_PATH_HH | ||
| 30 | #define HPP_CORE_KINODYNAMIC_PATH_HH | ||
| 31 | |||
| 32 | #include <hpp/core/config.hh> | ||
| 33 | #include <hpp/core/fwd.hh> | ||
| 34 | #include <hpp/core/straight-path.hh> | ||
| 35 | #include <hpp/pinocchio/configuration.hh> | ||
| 36 | |||
| 37 | namespace hpp { | ||
| 38 | namespace core { | ||
| 39 | /// \addtogroup path | ||
| 40 | /// \{ | ||
| 41 | |||
| 42 | /// Kino-dynamic straight path | ||
| 43 | /// | ||
| 44 | /// This Path has the same behavior as the \link hpp::core::StraightPath | ||
| 45 | /// StraightPath \endlink class except for the translation part of the | ||
| 46 | /// free-flyer. For the translation part of the free-flyer KinodynamicPath store | ||
| 47 | /// a "bang-bang" trajectory dependent on time with either 2 segment of constant | ||
| 48 | /// acceleration or 3 segments with a constant velocity segment | ||
| 49 | /// | ||
| 50 | /// In current implementation, only the translation part of the freeflyer joint | ||
| 51 | /// is considered by this class. The value of all other joint are interpolated | ||
| 52 | /// between the initial and end value using the interpolate() method. | ||
| 53 | /// | ||
| 54 | /// The current implementation assume that : | ||
| 55 | /// * The first joint of the robot is a freeflyer | ||
| 56 | /// * The robot have an extra Config Space of dimension >= 6. | ||
| 57 | /// The first 3 values of the extraConfig are the velocity of the root and the 3 | ||
| 58 | /// other values are the aceleration. | ||
| 59 | class HPP_CORE_DLLAPI KinodynamicPath : public StraightPath { | ||
| 60 | public: | ||
| 61 | typedef StraightPath parent_t; | ||
| 62 | /// Destructor | ||
| 63 | 110044 | virtual ~KinodynamicPath() {} | |
| 64 | |||
| 65 | /// Create instance and return shared pointer | ||
| 66 | /// \param device Robot corresponding to configurations | ||
| 67 | /// \param init, end Start and end configurations of the path | ||
| 68 | /// \param length Distance between the configurations. | ||
| 69 | 2007 | static KinodynamicPathPtr_t create(const DevicePtr_t& device, | |
| 70 | ConfigurationIn_t init, | ||
| 71 | ConfigurationIn_t end, value_type length, | ||
| 72 | ConfigurationIn_t a1, ConfigurationIn_t t0, | ||
| 73 | ConfigurationIn_t t1, ConfigurationIn_t tv, | ||
| 74 | ConfigurationIn_t t2, | ||
| 75 | ConfigurationIn_t vLim) { | ||
| 76 | KinodynamicPath* ptr = new KinodynamicPath(device, init, end, length, a1, | ||
| 77 |
9/18✓ Branch 2 taken 2007 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2007 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2007 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2007 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2007 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2007 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2007 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 2007 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 2007 times.
✗ Branch 27 not taken.
|
2007 | t0, t1, tv, t2, vLim); |
| 78 | 2007 | KinodynamicPathPtr_t shPtr(ptr); | |
| 79 |
1/2✓ Branch 2 taken 2007 times.
✗ Branch 3 not taken.
|
2007 | ptr->init(shPtr); |
| 80 |
1/2✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
|
2007 | ptr->checkPath(); |
| 81 | 2007 | return shPtr; | |
| 82 | } | ||
| 83 | |||
| 84 | /// Create instance and return shared pointer | ||
| 85 | /// \param device Robot corresponding to configurations | ||
| 86 | /// \param init, end Start and end configurations of the path | ||
| 87 | /// \param length Distance between the configurations. | ||
| 88 | /// \param constraints the path is subject to | ||
| 89 | 20004 | static KinodynamicPathPtr_t create( | |
| 90 | const DevicePtr_t& device, ConfigurationIn_t init, ConfigurationIn_t end, | ||
| 91 | value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0, | ||
| 92 | ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, | ||
| 93 | ConfigurationIn_t vLim, ConstraintSetPtr_t constraints) { | ||
| 94 | KinodynamicPath* ptr = new KinodynamicPath( | ||
| 95 |
9/18✓ Branch 2 taken 20004 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20004 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 20004 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 20004 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 20004 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 20004 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 20004 times.
✗ Branch 21 not taken.
✓ Branch 24 taken 20004 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 20004 times.
✗ Branch 28 not taken.
|
20004 | device, init, end, length, a1, t0, t1, tv, t2, vLim, constraints); |
| 96 | 20004 | KinodynamicPathPtr_t shPtr(ptr); | |
| 97 |
1/2✓ Branch 2 taken 20004 times.
✗ Branch 3 not taken.
|
20004 | ptr->init(shPtr); |
| 98 |
1/2✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
|
20004 | ptr->checkPath(); |
| 99 | 20004 | return shPtr; | |
| 100 | } | ||
| 101 | |||
| 102 | /// Create copy and return shared pointer | ||
| 103 | /// \param path path to copy | ||
| 104 | ✗ | static KinodynamicPathPtr_t createCopy(const KinodynamicPathPtr_t& path) { | |
| 105 | ✗ | KinodynamicPath* ptr = new KinodynamicPath(*path); | |
| 106 | ✗ | KinodynamicPathPtr_t shPtr(ptr); | |
| 107 | ✗ | ptr->init(shPtr); | |
| 108 | ✗ | ptr->checkPath(); | |
| 109 | ✗ | return shPtr; | |
| 110 | } | ||
| 111 | |||
| 112 | /// Create copy and return shared pointer | ||
| 113 | /// \param path path to copy | ||
| 114 | /// \param constraints the path is subject to | ||
| 115 | ✗ | static KinodynamicPathPtr_t createCopy( | |
| 116 | const KinodynamicPathPtr_t& path, const ConstraintSetPtr_t& constraints) { | ||
| 117 | ✗ | KinodynamicPath* ptr = new KinodynamicPath(*path, constraints); | |
| 118 | ✗ | KinodynamicPathPtr_t shPtr(ptr); | |
| 119 | ✗ | ptr->init(shPtr); | |
| 120 | ✗ | ptr->checkPath(); | |
| 121 | ✗ | return shPtr; | |
| 122 | } | ||
| 123 | |||
| 124 | /// Return a shared pointer to this | ||
| 125 | /// | ||
| 126 | /// As StaightPath are immutable, and refered to by shared pointers, | ||
| 127 | /// they do not need to be copied. | ||
| 128 | ✗ | virtual PathPtr_t copy() const { return createCopy(weak_.lock()); } | |
| 129 | |||
| 130 | /// Return a shared pointer to a copy of this and set constraints | ||
| 131 | /// | ||
| 132 | /// \param constraints constraints to apply to the copy | ||
| 133 | /// \pre *this should not have constraints. | ||
| 134 | ✗ | virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const { | |
| 135 | ✗ | return createCopy(weak_.lock(), constraints); | |
| 136 | } | ||
| 137 | |||
| 138 | /// Extraction/Reversion of a sub-path | ||
| 139 | /// \param subInterval interval of definition of the extract path | ||
| 140 | /// If upper bound of subInterval is smaller than lower bound, return an empty | ||
| 141 | /// path | ||
| 142 | virtual PathPtr_t impl_extract(const interval_t& paramInterval) const; | ||
| 143 | |||
| 144 | 63042 | vector_t getT0() { return t0_; } | |
| 145 | |||
| 146 | 123052 | vector_t getT1() { return t1_; } | |
| 147 | |||
| 148 | 123046 | vector_t getT2() { return t2_; } | |
| 149 | |||
| 150 | 123038 | vector_t getTv() { return tv_; } | |
| 151 | |||
| 152 | 43 | vector_t getA1() { return a1_; } | |
| 153 | |||
| 154 | protected: | ||
| 155 | /// Print path in a stream | ||
| 156 | ✗ | virtual std::ostream& print(std::ostream& os) const { | |
| 157 | ✗ | os << "KinodynamicPath:" << std::endl; | |
| 158 | ✗ | os << "interval: [ " << timeRange().first << ", " << timeRange().second | |
| 159 | ✗ | << " ]" << std::endl; | |
| 160 | ✗ | os << "initial configuration: " << pinocchio::displayConfig(initial_) | |
| 161 | ✗ | << std::endl; | |
| 162 | ✗ | os << "final configuration: " << pinocchio::displayConfig(end_) | |
| 163 | ✗ | << std::endl; | |
| 164 | ✗ | return os; | |
| 165 | } | ||
| 166 | /// Constructor | ||
| 167 | KinodynamicPath(const DevicePtr_t& robot, ConfigurationIn_t init, | ||
| 168 | ConfigurationIn_t end, value_type length, | ||
| 169 | ConfigurationIn_t a1, ConfigurationIn_t t0, | ||
| 170 | ConfigurationIn_t t1, ConfigurationIn_t tv, | ||
| 171 | ConfigurationIn_t t2, ConfigurationIn_t vLim); | ||
| 172 | |||
| 173 | /// Constructor with constraints | ||
| 174 | KinodynamicPath(const DevicePtr_t& robot, ConfigurationIn_t init, | ||
| 175 | ConfigurationIn_t end, value_type length, | ||
| 176 | ConfigurationIn_t a1, ConfigurationIn_t t0, | ||
| 177 | ConfigurationIn_t t1, ConfigurationIn_t tv, | ||
| 178 | ConfigurationIn_t t2, ConfigurationIn_t vLim, | ||
| 179 | ConstraintSetPtr_t constraints); | ||
| 180 | |||
| 181 | /// Copy constructor | ||
| 182 | KinodynamicPath(const KinodynamicPath& path); | ||
| 183 | |||
| 184 | /// Copy constructor with constraints | ||
| 185 | KinodynamicPath(const KinodynamicPath& path, | ||
| 186 | const ConstraintSetPtr_t& constraints); | ||
| 187 | |||
| 188 | 33011 | void init(KinodynamicPathPtr_t self) { | |
| 189 |
1/2✓ Branch 2 taken 33011 times.
✗ Branch 3 not taken.
|
33011 | parent_t::init(self); |
| 190 | 33011 | weak_ = self; | |
| 191 | 33011 | checkPath(); | |
| 192 | 33011 | } | |
| 193 | |||
| 194 | virtual bool impl_compute(ConfigurationOut_t result, value_type t) const; | ||
| 195 | |||
| 196 | inline double sgnenum(double val) const { return ((0. < val) - (val < 0.)); } | ||
| 197 | |||
| 198 | inline int sgn(double d) const { return d >= 0.0 ? 1 : -1; } | ||
| 199 | |||
| 200 | inline double sgnf(double d) const { return d >= 0.0 ? 1.0 : -1.0; } | ||
| 201 | |||
| 202 | 1175139 | const DevicePtr_t& device() const { return device_; } | |
| 203 | |||
| 204 | private: | ||
| 205 | KinodynamicPathWkPtr_t weak_; | ||
| 206 | DevicePtr_t device_; | ||
| 207 | vector_t a1_; | ||
| 208 | vector_t t0_, t1_, tv_, t2_, vLim_; | ||
| 209 | }; // class KinodynamicPath | ||
| 210 | /// \} | ||
| 211 | } // namespace core | ||
| 212 | } // namespace hpp | ||
| 213 | #endif // HPP_CORE_KINODYNAMIC_PATH_HH | ||
| 214 |