se3_curve.h 9.63 KB
 Pierre Fernbach committed Oct 09, 2019 1 2 3 4 5 6 7 8 9 10 #ifndef _STRUCT_SE3_CURVE_H #define _STRUCT_SE3_CURVE_H #include "MathDefs.h" #include "curve_abc.h" #include "so3_linear.h" #include "polynomial.h" #include #include  Guilhem Saurel committed Nov 01, 2019 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 namespace curves { /// \class SE3Curve. /// \brief Composition of a curve of any type of dimension 3 and a curve representing an rotation /// (in current implementation, only SO3Linear can be used for the rotation part) /// The output is a vector of size 7 (pos_x,pos_y,pos_z,quat_x,quat_y,quat_z,quat_w) /// The output of the derivative of any order is a vector of size 6 /// (linear_x,linear_y,linear_z,angular_x,angular_y,angular_z) /// /// template struct SE3Curve : public curve_abc, Eigen::Matrix > { typedef Numeric Scalar; typedef Eigen::Transform transform_t; typedef transform_t point_t; typedef Eigen::Matrix point_derivate_t; typedef Eigen::Matrix point3_t; typedef Eigen::Matrix pointX_t; typedef Eigen::Matrix matrix3_t; typedef Eigen::Quaternion Quaternion; typedef Time time_t; typedef curve_abc curve_abc_t; // parent class typedef curve_abc curve_X_t; // generic class of curve  35 36 37 38  typedef curve_abc curve_rotation_t; // templated class used for the rotation (return dimension are fixed) typedef boost::shared_ptr curve_ptr_t; typedef boost::shared_ptr curve_rotation_ptr_t;  Guilhem Saurel committed Nov 01, 2019 39 40 41 42 43 44 45  typedef SO3Linear SO3Linear_t; typedef polynomial polynomial_t; typedef SE3Curve SE3Curve_t; public: /* Constructors - destructors */ /// \brief Empty constructor. Curve obtained this way can not perform other class functions.  Pierre Fernbach committed Oct 09, 2019 46  ///  47  SE3Curve() : curve_abc_t(), dim_(3), translation_curve_(), rotation_curve_(), T_min_(0), T_max_(0) {}  Guilhem Saurel committed Nov 01, 2019 48 49 50 51 52 53 54 55 56 57 58  /// \brief Destructor ~SE3Curve() { // should we delete translation_curve and rotation_curve here ? // better switch to shared ptr } /* Constructor without curve object for the translation : */ /// \brief Constructor from init/end transform use polynomial of degree 1 for position and SO3Linear for rotation SE3Curve(const transform_t& init_transform, const transform_t& end_transform, const time_t& t_min, const time_t& t_max)  Pierre Fernbach committed Oct 09, 2019 59 60  : curve_abc_t(), dim_(6),  Guilhem Saurel committed Nov 01, 2019 61 62 63 64 65 66 67 68 69 70 71 72  translation_curve_(new polynomial_t(pointX_t(init_transform.translation()), pointX_t(end_transform.translation()), t_min, t_max)), rotation_curve_(new SO3Linear_t(init_transform.rotation(), end_transform.rotation(), t_min, t_max)), T_min_(t_min), T_max_(t_max) { safe_check(); } /// \brief Constructor from init/end pose, with quaternion. use polynomial of degree 1 for position and SO3Linear for /// rotation SE3Curve(const pointX_t& init_pos, const pointX_t& end_pos, const Quaternion& init_rot, const Quaternion& end_rot, const time_t& t_min, const time_t& t_max)  Pierre Fernbach committed Oct 09, 2019 73  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 74 75 76 77 78 79 80 81 82 83 84 85  dim_(6), translation_curve_(new polynomial_t(init_pos, end_pos, t_min, t_max)), rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)), T_min_(t_min), T_max_(t_max) { safe_check(); } /// \brief Constructor from init/end pose, with rotation matrix. use polynomial of degree 1 for position and /// SO3Linear for rotation SE3Curve(const pointX_t& init_pos, const pointX_t& end_pos, const matrix3_t& init_rot, const matrix3_t& end_rot, const time_t& t_min, const time_t& t_max)  Pierre Fernbach committed Oct 09, 2019 86  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 87 88 89 90 91 92 93 94 95 96 97  dim_(6), translation_curve_(new polynomial_t(init_pos, end_pos, t_min, t_max)), rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)), T_min_(t_min), T_max_(t_max) { safe_check(); } /* Constructor with curve object for the translation : */ /// \brief Constructor from curve for the translation and init/end rotation, with quaternion. /// Use SO3Linear for rotation with the same time bounds as the  98  SE3Curve(curve_ptr_t translation_curve, const Quaternion& init_rot, const Quaternion& end_rot)  Pierre Fernbach committed Oct 09, 2019 99  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 100 101 102 103 104 105 106 107 108  dim_(6), translation_curve_(translation_curve), rotation_curve_(new SO3Linear_t(init_rot, end_rot, translation_curve->min(), translation_curve->max())), T_min_(translation_curve->min()), T_max_(translation_curve->max()) { safe_check(); } /// \brief Constructor from curve for the translation and init/end rotation, with rotation matrix. /// Use SO3Linear for rotation with the same time bounds as the  109  SE3Curve(curve_ptr_t translation_curve, const matrix3_t& init_rot, const matrix3_t& end_rot)  Pierre Fernbach committed Oct 09, 2019 110  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 111 112 113 114 115 116 117 118 119 120  dim_(6), translation_curve_(translation_curve), rotation_curve_(new SO3Linear_t(init_rot, end_rot, translation_curve->min(), translation_curve->max())), T_min_(translation_curve->min()), T_max_(translation_curve->max()) { safe_check(); } /* Constructor from translation and rotation curves object : */ /// \brief Constructor from from translation and rotation curves object  121  SE3Curve(curve_ptr_t translation_curve, curve_rotation_ptr_t rotation_curve)  Pierre Fernbach committed Oct 09, 2019 122  : curve_abc_t(),  Guilhem Saurel committed Nov 01, 2019 123 124  dim_(6), translation_curve_(translation_curve),  Pierre Fernbach committed Oct 09, 2019 125  rotation_curve_(rotation_curve),  Guilhem Saurel committed Nov 01, 2019 126 127 128 129  T_min_(translation_curve->min()), T_max_(translation_curve->max()) { if (translation_curve->dim() != 3) { throw std::invalid_argument("The translation curve should be of dimension 3.");  Pierre Fernbach committed Oct 09, 2019 130  }  Guilhem Saurel committed Nov 01, 2019 131 132  if (rotation_curve->min() != T_min_) { throw std::invalid_argument("Min bounds of translation and rotation curve are not the same.");  Pierre Fernbach committed Oct 09, 2019 133  }  Guilhem Saurel committed Nov 01, 2019 134 135  if (rotation_curve->max() != T_max_) { throw std::invalid_argument("Max bounds of translation and rotation curve are not the same.");  Pierre Fernbach committed Oct 09, 2019 136  }  Guilhem Saurel committed Nov 01, 2019 137 138 139 140 141 142 143 144 145  safe_check(); } /// \brief Evaluation of the SE3Curve at time t /// \param t : time when to evaluate the spline. /// \return \f$x(t)\f$ point corresponding on spline at time t. (pos_x,pos_y,pos_z,quat_x,quat_y,quat_z,quat_w) virtual point_t operator()(const time_t t) const { if (translation_curve_->dim() != 3) { throw std::invalid_argument("Translation curve should always be of dimension 3");  Pierre Fernbach committed Oct 09, 2019 146  }  Guilhem Saurel committed Nov 01, 2019 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166  point_t res = point_t::Identity(); res.translate(point3_t((*translation_curve_)(t))); res.rotate((*rotation_curve_)(t)); return res; } /// \brief Evaluation of the derivative of order N of spline at time t. /// \param t : the time when to evaluate the spline. /// \param order : order of derivative. /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline at time t. virtual point_derivate_t derivate(const time_t t, const std::size_t order) const { if (translation_curve_->dim() != 3) { throw std::invalid_argument("Translation curve should always be of dimension 3"); } point_derivate_t res = point_derivate_t::Zero(); res.segment(0, 3) = point3_t(translation_curve_->derivate(t, order)); res.segment(3, 3) = rotation_curve_->derivate(t, order); return res; }  167  SE3Curve_t compute_derivate(const std::size_t /*order*/) const {  168 169 170  throw std::logic_error("Compute derivate for SE3 is not implemented yet."); }  171 172 173 174 175 176 177  /// \brief Compute the derived curve at order N. /// \param order : order of derivative. /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve. SE3Curve_t* compute_derivate_ptr(const std::size_t order) const { return new SE3Curve_t(compute_derivate(order)); }  Guilhem Saurel committed Nov 01, 2019 178 179 180 181 182 183 184 185 186 187  /*Helpers*/ /// \brief Get dimension of curve. /// \return dimension of curve. std::size_t virtual dim() const { return dim_; }; /// \brief Get the minimum time for which the curve is defined /// \return \f$t_{min}\f$ lower bound of time range. time_t min() const { return T_min_; } /// \brief Get the maximum time for which the curve is defined. /// \return \f$t_{max}\f$ upper bound of time range. time_t max() const { return T_max_; }  Pierre Fernbach committed Nov 30, 2019 188 189 190  /// \brief Get the degree of the curve. /// \return \f$degree\f$, the degree of the curve. virtual std::size_t degree() const {return translation_curve_->degree();}  Guilhem Saurel committed Nov 01, 2019 191 192 193 194  /*Helpers*/ /*Attributes*/ std::size_t dim_; // dim doesn't mean anything in this class ...  195 196  curve_ptr_t translation_curve_; curve_rotation_ptr_t rotation_curve_;  Guilhem Saurel committed Nov 01, 2019 197 198 199 200 201 202 203 204 205 206 207  time_t T_min_, T_max_; /*Attributes*/ // Serialization of the class friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int version) { if (version) { // Do something depending on version ? }  Pierre Fernbach committed Nov 29, 2019 208  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);  Guilhem Saurel committed Nov 01, 2019 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223  ar& boost::serialization::make_nvp("dim", dim_); ar& boost::serialization::make_nvp("translation_curve", translation_curve_); ar& boost::serialization::make_nvp("rotation_curve", rotation_curve_); ar& boost::serialization::make_nvp("T_min", T_min_); ar& boost::serialization::make_nvp("T_max", T_max_); } private: void safe_check() { if (Safe) { if (T_min_ > T_max_) { throw std::invalid_argument("Tmin should be inferior to Tmax"); } if (translation_curve_->dim() != 3) { throw std::invalid_argument("Translation curve should always be of dimension 3");  Pierre Fernbach committed Oct 09, 2019 224 225  } }  Guilhem Saurel committed Nov 01, 2019 226  }  Pierre Fernbach committed Oct 09, 2019 227   Guilhem Saurel committed Nov 01, 2019 228 }; // SE3Curve  Pierre Fernbach committed Oct 09, 2019 229   Guilhem Saurel committed Nov 01, 2019 230 } // namespace curves  Pierre Fernbach committed Oct 09, 2019 231   Guilhem Saurel committed Nov 01, 2019 232 #endif // SE3_CURVE_H