| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | #ifdef CURVES_WITH_PINOCCHIO_SUPPORT | ||
| 2 | |||
| 3 | #ifndef NDCURVES_SO3_SMOOTH_H | ||
| 4 | #define NDCURVES_SO3_SMOOTH_H | ||
| 5 | |||
| 6 | #include <Eigen/Geometry> | ||
| 7 | #include <boost/math/constants/constants.hpp> | ||
| 8 | #include <boost/serialization/split_free.hpp> | ||
| 9 | #include <boost/serialization/vector.hpp> | ||
| 10 | #include <pinocchio/fwd.hpp> | ||
| 11 | #include <pinocchio/spatial/explog.hpp> | ||
| 12 | |||
| 13 | #include "ndcurves/MathDefs.h" | ||
| 14 | #include "ndcurves/constant_curve.h" | ||
| 15 | #include "ndcurves/curve_abc.h" | ||
| 16 | #include "ndcurves/polynomial.h" | ||
| 17 | |||
| 18 | namespace ndcurves { | ||
| 19 | |||
| 20 | /// \class SO3Smooth. | ||
| 21 | /// \brief Represents a smooth interpolation in SO3, using an analytical | ||
| 22 | /// interpolation between 2 rotation. | ||
| 23 | /// | ||
| 24 | template <typename Time = double, typename Numeric = Time, bool Safe = false> | ||
| 25 | struct SO3Smooth : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t> { | ||
| 26 | typedef Numeric Scalar; | ||
| 27 | typedef matrix3_t point_t; | ||
| 28 | typedef matrix3_t_cst_ref point_t_ref; | ||
| 29 | typedef point3_t point_derivate_t; | ||
| 30 | typedef Time time_t; | ||
| 31 | typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t; | ||
| 32 | typedef constant_curve<Time, Numeric, Safe, point_derivate_t> | ||
| 33 | curve_derivate_t; | ||
| 34 | typedef SO3Smooth<Time, Numeric, Safe> SO3Smooth_t; | ||
| 35 | typedef polynomial<Time, Numeric, Safe, point1_t> min_jerk_t; | ||
| 36 | |||
| 37 | public: | ||
| 38 | /* Constructors - destructors */ | ||
| 39 | /// \brief Empty constructor. Curve obtained this way can not perform other | ||
| 40 | /// class functions. | ||
| 41 | /// | ||
| 42 | 11 | SO3Smooth() | |
| 43 | : curve_abc_t(), | ||
| 44 |
1/2✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
|
11 | init_rot_(point_t::Identity()), |
| 45 |
2/4✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
|
11 | end_rot_(point_t::Identity()), |
| 46 | 11 | t_min_(0.0), | |
| 47 | 11 | t_max_(1.0), | |
| 48 |
3/6✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
|
11 | min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_, |
| 49 | t_max_)), | ||
| 50 |
3/6✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
|
11 | rot_diff_(pinocchio::log3(init_rot_.transpose() * end_rot_)), |
| 51 |
1/2✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
|
22 | dt_(1e-3) {} |
| 52 | |||
| 53 | /// \brief constructor with initial and final rotation and time bounds | ||
| 54 | 3 | SO3Smooth(const quaternion_t& init_quat, const quaternion_t& end_quat, | |
| 55 | const time_t t_min, const time_t t_max) | ||
| 56 | : curve_abc_t(), | ||
| 57 | 3 | t_min_(t_min), | |
| 58 | 3 | t_max_(t_max), | |
| 59 |
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 | min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_, |
| 60 | t_max_)), | ||
| 61 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
6 | dt_(1e-3) { |
| 62 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | quaternion_t tmp_init_quat = init_quat; |
| 63 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | quaternion_t tmp_end_quat = end_quat; |
| 64 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | tmp_init_quat.normalized(); |
| 65 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | tmp_end_quat.normalized(); |
| 66 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | init_rot_ = tmp_init_quat.toRotationMatrix(); |
| 67 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | end_rot_ = tmp_end_quat.toRotationMatrix(); |
| 68 |
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 | rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_); |
| 69 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | safe_check(); |
| 70 | 3 | } | |
| 71 | |||
| 72 | /// \brief constructor with initial and final rotation expressed as rotation | ||
| 73 | /// matrix and time bounds | ||
| 74 | 1 | SO3Smooth(const matrix3_t& init_rot, const matrix3_t& end_rot, | |
| 75 | const time_t t_min, const time_t t_max) | ||
| 76 | : curve_abc_t(), | ||
| 77 | 1 | init_rot_(init_rot), | |
| 78 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | end_rot_(end_rot), |
| 79 | 1 | t_min_(t_min), | |
| 80 | 1 | t_max_(t_max), | |
| 81 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_, |
| 82 | t_max_)), | ||
| 83 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | rot_diff_(pinocchio::log3(init_rot_.transpose() * end_rot_)), |
| 84 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | dt_(1e-3) { |
| 85 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | safe_check(); |
| 86 | 1 | } | |
| 87 | |||
| 88 | /// \brief constructor with initial and final rotation, time bounds are set to | ||
| 89 | /// [0;1] | ||
| 90 | 1 | SO3Smooth(const quaternion_t& init_rot, const quaternion_t& end_rot) | |
| 91 | : curve_abc_t(), | ||
| 92 | 1 | t_min_(0.), | |
| 93 | 1 | t_max_(1.), | |
| 94 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_, |
| 95 | t_max_)), | ||
| 96 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
2 | dt_(1e-3) { |
| 97 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | quaternion_t init_rot_normalized = init_rot.normalized(); |
| 98 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | quaternion_t end_rot_normalized = end_rot.normalized(); |
| 99 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | init_rot_ = init_rot_normalized.toRotationMatrix(); |
| 100 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | end_rot_ = end_rot_normalized.toRotationMatrix(); |
| 101 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_); |
| 102 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | safe_check(); |
| 103 | 1 | } | |
| 104 | |||
| 105 | /// \brief constructor with initial and final rotation expressed as rotation | ||
| 106 | /// matrix, time bounds are set to [0;1] | ||
| 107 | 1 | SO3Smooth(const matrix3_t& init_rot, const matrix3_t& end_rot) | |
| 108 | : curve_abc_t(), | ||
| 109 | 1 | init_rot_(init_rot), | |
| 110 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | end_rot_(end_rot), |
| 111 | 1 | t_min_(0.), | |
| 112 | 1 | t_max_(1.), | |
| 113 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_, |
| 114 | t_max_)), | ||
| 115 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | dt_(1e-3) { |
| 116 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_); |
| 117 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | safe_check(); |
| 118 | 1 | } | |
| 119 | |||
| 120 | // Generate functions: | ||
| 121 | |||
| 122 | /* Generators. */ | ||
| 123 | /// \brief Empty constructor. Curve obtained this way can not perform other | ||
| 124 | /// class functions. | ||
| 125 | /// | ||
| 126 | 1 | void generate() { | |
| 127 | 1 | init_rot_.setIdentity(); | |
| 128 | 1 | end_rot_.setIdentity(); | |
| 129 | 1 | t_min_ = 0.0; | |
| 130 | 1 | t_max_ = 1.0; | |
| 131 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | min_jerk_t::MinimumJerk(min_jerk_, point1_t(0.0), point1_t(1.0), t_min_, |
| 132 | t_max_); | ||
| 133 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_); |
| 134 | 1 | safe_check(); | |
| 135 | 1 | } | |
| 136 | |||
| 137 | /// \brief constructor with initial and final rotation and time bounds | ||
| 138 | 6 | void generate(const matrix3_t& init_rot, const matrix3_t& end_rot, | |
| 139 | const time_t t_min, const time_t t_max) { | ||
| 140 | 6 | init_rot_ = init_rot; | |
| 141 | 6 | end_rot_ = end_rot; | |
| 142 | 6 | t_min_ = t_min; | |
| 143 | 6 | t_max_ = t_max; | |
| 144 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | min_jerk_t::MinimumJerk(min_jerk_, point1_t(0.0), point1_t(1.0), t_min_, |
| 145 | t_max_); | ||
| 146 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
6 | rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_); |
| 147 | 6 | safe_check(); | |
| 148 | 6 | } | |
| 149 | |||
| 150 | /// \brief constructor with initial and final rotation expressed as rotation | ||
| 151 | /// matrix and time bounds | ||
| 152 | 1 | void generate(const quaternion_t& init_rot, const quaternion_t& end_rot, | |
| 153 | const time_t t_min, const time_t t_max) { | ||
| 154 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | quaternion_t init_rot_normalized = init_rot.normalized(); |
| 155 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | quaternion_t end_rot_normalized = end_rot.normalized(); |
| 156 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | generate(init_rot_normalized.toRotationMatrix(), |
| 157 | end_rot_normalized.toRotationMatrix(), t_min, t_max); | ||
| 158 | 1 | } | |
| 159 | |||
| 160 | 1 | void generate(const quaternion_t& init_rot, const quaternion_t& end_rot) { | |
| 161 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | quaternion_t init_rot_normalized = init_rot.normalized(); |
| 162 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | quaternion_t end_rot_normalized = end_rot.normalized(); |
| 163 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | generate(init_rot_normalized.toRotationMatrix(), |
| 164 | end_rot_normalized.toRotationMatrix(), 0.0, 1.0); | ||
| 165 | 1 | } | |
| 166 | |||
| 167 | /// \brief constructor with initial and final rotation expressed as rotation | ||
| 168 | /// matrix, time bounds are set to [0;1] | ||
| 169 | 1 | void generate(const matrix3_t& init_rot, const matrix3_t& end_rot) { | |
| 170 | 1 | generate(init_rot, end_rot, 0.0, 1.0); | |
| 171 | 1 | } | |
| 172 | |||
| 173 | /// \brief Destructor | ||
| 174 | 36 | virtual ~SO3Smooth() {} | |
| 175 | |||
| 176 | /// \brief Copy constructor | ||
| 177 | 1 | SO3Smooth(const SO3Smooth& other) | |
| 178 | : curve_abc_t(), | ||
| 179 | 1 | init_rot_(other.init_rot_), | |
| 180 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | end_rot_(other.end_rot_), |
| 181 | 1 | t_min_(other.t_min_), | |
| 182 | 1 | t_max_(other.t_max_), | |
| 183 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | min_jerk_(other.min_jerk_), |
| 184 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | rot_diff_(other.rot_diff_), |
| 185 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | dt_(1e-3) {} |
| 186 | |||
| 187 | /// \brief Evaluation of the SO3Smooth at time t using \$f R(t) = R1 * | ||
| 188 | /// exp3(min_jerk(t) log3(R1.T * R2)) \$f. \param t : time when to evaluate | ||
| 189 | /// the spline. \return \f$x(t)\f$ point corresponding on spline at time t. | ||
| 190 | 10453 | virtual point_t operator()(const time_t t) const { | |
| 191 |
2/4✓ Branch 0 taken 10453 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 10453 times.
|
10453 | if ((t < t_min_ || t > t_max_) && Safe) { |
| 192 | ✗ | throw std::invalid_argument( | |
| 193 | "error in SO3Smooth : time t to evaluate derivative should be in " | ||
| 194 | "range [Tmin, Tmax] of the curve"); | ||
| 195 | } | ||
| 196 |
6/12✓ Branch 1 taken 10453 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10453 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10453 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10453 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10453 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10453 times.
✗ Branch 17 not taken.
|
10453 | return init_rot_ * pinocchio::exp3(min_jerk_(t)[0] * rot_diff_); |
| 197 | } | ||
| 198 | |||
| 199 | /** | ||
| 200 | * @brief isApprox check if other and *this are approximately equals. | ||
| 201 | * Only two curves of the same class can be approximately equals, for | ||
| 202 | * comparison between different type of curves see isEquivalent | ||
| 203 | * @param other the other curve to check | ||
| 204 | * @param prec the precision threshold, default | ||
| 205 | * Eigen::NumTraits<Numeric>::dummy_precision() | ||
| 206 | * @return true is the two curves are approximately equals | ||
| 207 | */ | ||
| 208 | 3 | bool isApprox( | |
| 209 | const SO3Smooth_t& other, | ||
| 210 | const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const { | ||
| 211 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | return ndcurves::isApprox<Numeric>(t_min_, other.min()) && |
| 212 | 3 | ndcurves::isApprox<Numeric>(t_max_, other.max()) && | |
| 213 |
2/4✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
9 | init_rot_.isApprox(other.init_rot_, prec) && |
| 214 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | end_rot_.isApprox(other.end_rot_, prec); |
| 215 | } | ||
| 216 | |||
| 217 | ✗ | virtual bool isApprox( | |
| 218 | const curve_abc_t* other, | ||
| 219 | const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const { | ||
| 220 | ✗ | const SO3Smooth_t* other_cast = dynamic_cast<const SO3Smooth_t*>(other); | |
| 221 | ✗ | if (other_cast) | |
| 222 | ✗ | return isApprox(*other_cast, prec); | |
| 223 | else | ||
| 224 | ✗ | return false; | |
| 225 | } | ||
| 226 | |||
| 227 | 3 | virtual bool operator==(const SO3Smooth_t& other) const { | |
| 228 | 3 | return isApprox(other); | |
| 229 | } | ||
| 230 | |||
| 231 | ✗ | virtual bool operator!=(const SO3Smooth_t& other) const { | |
| 232 | ✗ | return !(*this == other); | |
| 233 | } | ||
| 234 | |||
| 235 | /// \brief Evaluation of the derivative of order N of spline at time t. | ||
| 236 | /// \param t : the time when to evaluate the spline. | ||
| 237 | /// \param order : order of derivative. | ||
| 238 | /// \return \f$ \frac{d^N x(t)}{dt^N} \f$ point corresponding on derivative | ||
| 239 | /// spline at time t. Specifically | ||
| 240 | /// \f$ \frac{d R(t)}{dt} = R1 * \frac{d exp^{min_jerk(t) diff}{dw} * | ||
| 241 | /// \frac{d min_jerk(t)}{dt} diff \f$ with \f$ R^3 \f$ -> \f$ SO3 \f$ | ||
| 242 | /// : \f$ R = exp(w) \f$ or \frac{d R(t)}{dt} = = R1 * | ||
| 243 | /// Jexp3(min_jerk(t) diff) * \frac{d min_jerk(t)}{dt} diff \f$. We | ||
| 244 | /// use the pinocchio library in order to compute the exp3, log3, and | ||
| 245 | /// Jexp3 fonctions. | ||
| 246 | 25842 | virtual point_derivate_t derivate(const time_t t, | |
| 247 | const std::size_t order) const { | ||
| 248 |
2/4✓ Branch 1 taken 25842 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25842 times.
✗ Branch 5 not taken.
|
25842 | point_derivate_t out = point_derivate_t::Zero(); |
| 249 |
2/4✓ Branch 0 taken 25842 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 25842 times.
|
25842 | if ((t < t_min_ || t > t_max_) && Safe) { |
| 250 | ✗ | throw std::invalid_argument( | |
| 251 | "error in SO3Smooth : time t to evaluate derivative should be in " | ||
| 252 | "range [Tmin, Tmax] of the curve"); | ||
| 253 | } | ||
| 254 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 25842 times.
|
25842 | if (order > 2) { |
| 255 | ✗ | throw std::invalid_argument("Order must be in {1, 2}."); | |
| 256 |
2/2✓ Branch 0 taken 6460 times.
✓ Branch 1 taken 19382 times.
|
25842 | } else if (order == 2) { |
| 257 | 6460 | double t1 = t; | |
| 258 | 6460 | double t2 = t + dt_; | |
| 259 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 6460 times.
|
6460 | if (t2 > t_max_) { |
| 260 | ✗ | t1 = t - dt_; | |
| 261 | ✗ | t2 = t; | |
| 262 | } | ||
| 263 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 6460 times.
|
6460 | if (t1 < t_min_) { |
| 264 | ✗ | t1 = t_min_; | |
| 265 | ✗ | t2 = t_max_; | |
| 266 | } | ||
| 267 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 6460 times.
|
6460 | if (t1 == t2) { |
| 268 | ✗ | out.setZero(); | |
| 269 | } else { | ||
| 270 |
5/10✓ Branch 1 taken 6460 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6460 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6460 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6460 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6460 times.
✗ Branch 14 not taken.
|
6460 | out = (derivate(t1, 1) - derivate(t2, 1)) / (t2 - t1); |
| 271 | } | ||
| 272 |
1/2✓ Branch 0 taken 19382 times.
✗ Branch 1 not taken.
|
19382 | } else if (order == 1) { |
| 273 |
1/2✓ Branch 1 taken 19382 times.
✗ Branch 2 not taken.
|
19382 | matrix3_t jexp; |
| 274 |
4/8✓ Branch 1 taken 19382 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19382 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19382 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19382 times.
✗ Branch 11 not taken.
|
19382 | pinocchio::Jexp3(min_jerk_(t)[0] * rot_diff_, jexp); |
| 275 |
4/8✓ Branch 1 taken 19382 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19382 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19382 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19382 times.
✗ Branch 11 not taken.
|
19382 | out.noalias() = |
| 276 |
3/6✓ Branch 1 taken 19382 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19382 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19382 times.
✗ Branch 8 not taken.
|
38764 | init_rot_ * jexp * min_jerk_.derivate(t, 1)[0] * rot_diff_; |
| 277 | } else { | ||
| 278 | ✗ | throw std::invalid_argument("Order must be > 0 "); | |
| 279 | } | ||
| 280 | 25842 | return out; | |
| 281 | } | ||
| 282 | |||
| 283 | ✗ | curve_derivate_t compute_derivate(const std::size_t /*order*/) const { | |
| 284 | ✗ | throw std::runtime_error("This function has not been implemented yet."); | |
| 285 | } | ||
| 286 | |||
| 287 | /// \brief Compute the derived curve at order N. | ||
| 288 | /// \param order : order of derivative. | ||
| 289 | /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the | ||
| 290 | /// curve. | ||
| 291 | ✗ | curve_derivate_t* compute_derivate_ptr(const std::size_t order) const { | |
| 292 | ✗ | return new curve_derivate_t(compute_derivate(order)); | |
| 293 | } | ||
| 294 | |||
| 295 | /*Helpers*/ | ||
| 296 | /// \brief Get dimension of curve. | ||
| 297 | /// \return dimension of curve. | ||
| 298 | 14 | std::size_t virtual dim() const { return 3; }; | |
| 299 | /// \brief Get the minimum time for which the curve is defined | ||
| 300 | /// \return \f$t_{min}\f$ lower bound of time range. | ||
| 301 | 20 | time_t min() const { return t_min_; } | |
| 302 | /// \brief Get the maximum time for which the curve is defined. | ||
| 303 | /// \return \f$t_{max}\f$ upper bound of time range. | ||
| 304 | 20 | time_t max() const { return t_max_; } | |
| 305 | /// \brief Get the degree of the curve. | ||
| 306 | /// \return \f$degree\f$, the degree of the curve. | ||
| 307 | 14 | virtual std::size_t degree() const { return 5; } | |
| 308 | 14 | matrix3_t_cst_ref get_init_rotation() const { return init_rot_; } | |
| 309 | 14 | matrix3_t_cst_ref get_end_rotation() const { return end_rot_; } | |
| 310 | |||
| 311 | /*Attributes*/ | ||
| 312 | point_t init_rot_; /*! @brief Initial rotation. */ | ||
| 313 | point_t end_rot_; /*! @brief End rotation. */ | ||
| 314 | time_t t_min_; /*! @brief Start time of the trajectory. */ | ||
| 315 | time_t t_max_; /*! @brief End time of the trajectory. */ | ||
| 316 | min_jerk_t | ||
| 317 | min_jerk_; /*! @brief Min jerk trajectory from t_min_ and t_max_. */ | ||
| 318 | point3_t | ||
| 319 | rot_diff_; /*! @brief The difference between init_rot_ and end_rot_. */ | ||
| 320 | const double dt_; /*! @brief Finite differentiation delta time for the | ||
| 321 | acceleration computation. */ | ||
| 322 | |||
| 323 | // Serialization of the class | ||
| 324 | friend class boost::serialization::access; | ||
| 325 | |||
| 326 | template <class Archive> | ||
| 327 | 6 | void load(Archive& ar, const unsigned int version) { | |
| 328 | if (version) { | ||
| 329 | // Do something depending on version ? | ||
| 330 | } | ||
| 331 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
6 | ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t); |
| 332 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | ar >> boost::serialization::make_nvp("init_rotation", init_rot_); |
| 333 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | ar >> boost::serialization::make_nvp("end_rotation", end_rot_); |
| 334 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | ar >> boost::serialization::make_nvp("t_min", t_min_); |
| 335 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | ar >> boost::serialization::make_nvp("t_max", t_max_); |
| 336 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | ar >> boost::serialization::make_nvp("min_jerk", min_jerk_); |
| 337 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | ar >> boost::serialization::make_nvp("rot_diff", rot_diff_); |
| 338 | 6 | } | |
| 339 | |||
| 340 | template <class Archive> | ||
| 341 | 6 | void save(Archive& ar, const unsigned int version) const { | |
| 342 | if (version) { | ||
| 343 | // Do something depending on version ? | ||
| 344 | } | ||
| 345 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
6 | ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t); |
| 346 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | ar << boost::serialization::make_nvp("init_rotation", init_rot_); |
| 347 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | ar << boost::serialization::make_nvp("end_rotation", end_rot_); |
| 348 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | ar << boost::serialization::make_nvp("t_min", t_min_); |
| 349 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | ar << boost::serialization::make_nvp("t_max", t_max_); |
| 350 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | ar << boost::serialization::make_nvp("min_jerk", min_jerk_); |
| 351 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | ar << boost::serialization::make_nvp("rot_diff", rot_diff_); |
| 352 | 6 | } | |
| 353 | |||
| 354 | 12 | BOOST_SERIALIZATION_SPLIT_MEMBER() | |
| 355 | |||
| 356 | /// \brief Log: SO3 -> so3. | ||
| 357 | /// | ||
| 358 | /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$. | ||
| 359 | /// | ||
| 360 | /// Code from pinocchio. | ||
| 361 | /// | ||
| 362 | /// \param[in] R the rotation matrix. /// | ||
| 363 | /// \return The angular velocity vector associated to the rotation matrix. | ||
| 364 | /// | ||
| 365 | point3_t log3(const matrix3_t& R) { | ||
| 366 | Scalar theta; | ||
| 367 | static const Scalar PI_value = boost::math::constants::pi<Scalar>(); | ||
| 368 | |||
| 369 | point3_t res; | ||
| 370 | const Scalar tr = R.trace(); | ||
| 371 | if (tr > Scalar(3)) | ||
| 372 | theta = 0; // acos((3-1)/2) | ||
| 373 | else if (tr < Scalar(-1)) | ||
| 374 | theta = PI_value; // acos((-1-1)/2) | ||
| 375 | else | ||
| 376 | theta = acos((tr - Scalar(1)) / Scalar(2)); | ||
| 377 | if (!std::isfinite(theta)) { | ||
| 378 | throw std::runtime_error("theta contains some NaN"); | ||
| 379 | } | ||
| 380 | |||
| 381 | // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small. | ||
| 382 | if (theta < PI_value - 1e-2) { | ||
| 383 | const Scalar t = | ||
| 384 | ((theta > | ||
| 385 | std::pow(std::numeric_limits<Scalar>::epsilon(), | ||
| 386 | Scalar(1) / | ||
| 387 | Scalar(4))) // precision of taylor serie of degree 3 | ||
| 388 | ? theta / sin(theta) | ||
| 389 | : Scalar(1)) / | ||
| 390 | Scalar(2); | ||
| 391 | res(0) = t * (R(2, 1) - R(1, 2)); | ||
| 392 | res(1) = t * (R(0, 2) - R(2, 0)); | ||
| 393 | res(2) = t * (R(1, 0) - R(0, 1)); | ||
| 394 | } else { | ||
| 395 | // 1e-2: A low value is not required since the computation is | ||
| 396 | // using explicit formula. However, the precision of this method | ||
| 397 | // is the square root of the precision with the antisymmetric | ||
| 398 | // method (Nominal case). | ||
| 399 | const Scalar cphi = cos(theta - PI_value); | ||
| 400 | const Scalar beta = theta * theta / (Scalar(1) + cphi); | ||
| 401 | point3_t tmp((R.diagonal().array() + cphi) * beta); | ||
| 402 | res(0) = (R(2, 1) > R(1, 2) ? Scalar(1) : Scalar(-1)) * | ||
| 403 | (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0)); | ||
| 404 | res(1) = (R(0, 2) > R(2, 0) ? Scalar(1) : Scalar(-1)) * | ||
| 405 | (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0)); | ||
| 406 | res(2) = (R(1, 0) > R(0, 1) ? Scalar(1) : Scalar(-1)) * | ||
| 407 | (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0)); | ||
| 408 | } | ||
| 409 | |||
| 410 | return res; | ||
| 411 | } | ||
| 412 | |||
| 413 | private: | ||
| 414 | 13 | void safe_check() { | |
| 415 | if (Safe) { | ||
| 416 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 13 times.
|
13 | if (t_min_ > t_max_) { |
| 417 | ✗ | throw std::invalid_argument("Tmin should be inferior to Tmax"); | |
| 418 | } | ||
| 419 | } | ||
| 420 | 13 | } | |
| 421 | |||
| 422 | }; // struct SO3Smooth | ||
| 423 | |||
| 424 | } // namespace ndcurves | ||
| 425 | |||
| 426 | DEFINE_CLASS_TEMPLATE_VERSION( | ||
| 427 | SINGLE_ARG(typename Time, typename Numeric, bool Safe), | ||
| 428 | SINGLE_ARG(ndcurves::SO3Smooth<Time, Numeric, Safe>)) | ||
| 429 | |||
| 430 | #endif // NDCURVES_SO3_SMOOTH_H | ||
| 431 | #endif // CURVES_WITH_PINOCCHIO_SUPPORT | ||
| 432 |