Commit e1e38ce6 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

uniformize code in se3 and so3 classes

parent 80dc71cb
...@@ -25,9 +25,6 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric ...@@ -25,9 +25,6 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
typedef Eigen::Transform<Numeric, 3, Eigen::Affine> transform_t; typedef Eigen::Transform<Numeric, 3, Eigen::Affine> transform_t;
typedef transform_t point_t; typedef transform_t point_t;
typedef Eigen::Matrix<Scalar, 6, 1> point_derivate_t; typedef Eigen::Matrix<Scalar, 6, 1> point_derivate_t;
typedef Eigen::Matrix<Scalar, 3, 1> point3_t;
typedef Eigen::Matrix<Scalar, -1, 1> pointX_t;
typedef Eigen::Matrix<Scalar, 3, 3> matrix3_t;
typedef Eigen::Quaternion<Scalar> Quaternion; typedef Eigen::Quaternion<Scalar> Quaternion;
typedef Time time_t; typedef Time time_t;
typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t; // parent class typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t; // parent class
......
...@@ -17,10 +17,8 @@ namespace curves { ...@@ -17,10 +17,8 @@ namespace curves {
/// ///
/// ///
template <typename Time = double, typename Numeric = Time, bool Safe = false> template <typename Time = double, typename Numeric = Time, bool Safe = false>
struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric, 3, 3>, Eigen::Matrix<Numeric, 3, 1> > { struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
typedef Numeric Scalar; typedef Numeric Scalar;
typedef Eigen::Matrix<Scalar, 3, 1> point3_t;
typedef Eigen::Matrix<Scalar, 3, 3> matrix3_t;
typedef matrix3_t point_t; typedef matrix3_t point_t;
typedef point3_t point_derivate_t; typedef point3_t point_derivate_t;
typedef Eigen::Quaternion<Scalar> quaternion_t; typedef Eigen::Quaternion<Scalar> quaternion_t;
...@@ -76,7 +74,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric, ...@@ -76,7 +74,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
dim_(3), dim_(3),
init_rot_(quaternion_t(init_rot)), init_rot_(quaternion_t(init_rot)),
end_rot_(quaternion_t(end_rot)), end_rot_(quaternion_t(end_rot)),
angular_vel_(log3(init_rot.toRotationMatrix().transpose() * end_rot.toRotationMatrix())), angular_vel_(log3(init_rot.transpose() * end_rot)),
T_min_(0.), T_min_(0.),
T_max_(1.) { T_max_(1.) {
safe_check(); safe_check();
...@@ -108,7 +106,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric, ...@@ -108,7 +106,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
/// \brief Evaluation of the SO3Linear at time t using Eigen slerp. /// \brief Evaluation of the SO3Linear at time t using Eigen slerp.
/// \param t : time when to evaluate the spline. /// \param t : time when to evaluate the spline.
/// \return \f$x(t)\f$ point corresponding on spline at time t. /// \return \f$x(t)\f$ point corresponding on spline at time t.
virtual matrix3_t operator()(const time_t t) const { return computeAsQuaternion(t).toRotationMatrix(); } virtual point_t operator()(const time_t t) const { return computeAsQuaternion(t).toRotationMatrix(); }
/** /**
* @brief isApprox check if other and *this are approximately equals. * @brief isApprox check if other and *this are approximately equals.
...@@ -141,7 +139,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric, ...@@ -141,7 +139,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
/// \param t : the time when to evaluate the spline. /// \param t : the time when to evaluate the spline.
/// \param order : order of derivative. /// \param order : order of derivative.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline at time t. /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline at time t.
virtual point3_t derivate(const time_t t, const std::size_t order) const { virtual point_derivate_t derivate(const time_t t, const std::size_t order) const {
if ((t < T_min_ || t > T_max_) && Safe) { if ((t < T_min_ || t > T_max_) && Safe) {
throw std::invalid_argument( throw std::invalid_argument(
"error in SO3_linear : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve"); "error in SO3_linear : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment