added waypoints at zero velocity in quaternion interpolation. Added copy...

added waypoints at zero velocity in quaternion interpolation. Added copy constructors for exact_cubic and curve_splines. template type for exact_cubic primitive splines

include/spline/exact_cubic.h

@@ -36,7 +36,8 @@ namespace spline
 /// crossing each of the waypoint given in its initialization
 ///
 template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
-, typename Point= Eigen::Matrix<Numeric, Dim, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
+, typename Point= Eigen::Matrix<Numeric, Dim, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> >
+, typename SplineBase=spline_curve<Time, Numeric, Dim, Safe, Point, T_Point> >
 struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
 {
 	typedef Point 	point_t;
@@ -44,7 +45,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
     typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
 	typedef Time 	time_t;
 	typedef Numeric	num_t;
-    typedef spline_curve<time_t, Numeric, Dim, Safe, point_t, t_point_t> spline_t;
+    typedef SplineBase spline_t;
     typedef typename std::vector<spline_t> t_spline_t;
     typedef typename t_spline_t::iterator it_spline_t;
     typedef typename t_spline_t::const_iterator cit_spline_t;
@@ -149,7 +150,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
     }
 
     private:
-	exact_cubic& operator=(const exact_cubic&);
+    //exact_cubic& operator=(const exact_cubic&);
 	/* Constructors - destructors */
 
 	/*Operations*/
@@ -193,7 +194,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
 
 	/*Attributes*/
     public:
-    const t_spline_t subSplines_;
+    t_spline_t subSplines_; // const
 	/*Attributes*/
 };
 }

include/spline/helpers/effector_spline_rotation.h

@@ -21,6 +21,7 @@
 #define _CLASS_EFFECTOR_SPLINE_ROTATION
 
 #include "spline/helpers/effector_spline.h"
+#include "spline/curve_abc.h"
 #include <Eigen/Geometry>
 
 namespace spline
@@ -32,19 +33,51 @@ typedef Eigen::Matrix<Numeric, 4, 1> quat_t;
 typedef Eigen::Ref<quat_t> quat_ref_t;
 typedef const Eigen::Ref<const quat_t> quat_ref_const_t;
 typedef Eigen::Matrix<Numeric, 7, 1> config_t;
+typedef curve_abc<Time, Numeric, 4, false, quat_t> curve_abc_quat_t;
+typedef std::pair<Numeric, quat_t > waypoint_quat_t;
+typedef std::vector<waypoint_quat_t> t_waypoint_quat_t;
 typedef Eigen::Matrix<Numeric, 1, 1> point_one_dim_t;
 typedef spline_deriv_constraint <Numeric, Numeric, 1, false, point_one_dim_t> spline_deriv_constraint_one_dim;
 typedef std::pair<Numeric, point_one_dim_t > waypoint_one_dim_t;
 typedef std::vector<waypoint_one_dim_t> t_waypoint_one_dim_t;
 
 
-class time_reparametrization_spline: public spline_deriv_constraint_one_dim
+class rotation_spline: public curve_abc_quat_t
 {
     public:
-    time_reparametrization_spline()
-        : spline_deriv_constraint_one_dim(computeWayPoints()){}
+     rotation_spline (quat_ref_const_t quat_from=quat_t(0,0,0,1), quat_ref_const_t quat_to=quat_t(0,0,0,1),
+                               const double min = 0., const double max = 1.)
+         : curve_abc_quat_t()
+         , quat_from_(quat_from.data()), quat_to_(quat_to.data()), min_(min), max_(max)
+         , time_reparam_(computeWayPoints())
+     {}
 
-    ~time_reparametrization_spline(){}
+    ~rotation_spline() {}
+
+    /* Copy Constructors / operator=*/
+    rotation_spline& operator=(const rotation_spline& from)
+    {
+        quat_from_ = from.quat_from_;
+        quat_to_ = from.quat_to_;
+        min_ =from.min_; max_ = from.max_;
+        time_reparam_ = spline_deriv_constraint_one_dim(from.time_reparam_);
+    }
+    /* Copy Constructors / operator=*/
+
+    quat_t operator()(const Numeric t) const
+    {
+        if(t<=min()) return quat_from_.coeffs();
+        if(t>=max()) return quat_to_.coeffs();
+        //normalize u
+        Numeric u = (t - min()) /(max() - min());
+        // reparametrize u
+        return quat_from_.slerp(time_reparam_(u)[0], quat_to_).coeffs();
+    }
+
+    virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const
+    {
+        throw std::runtime_error("TODO quaternion spline does not implement derivate");
+    }
 
     spline_deriv_constraint_one_dim computeWayPoints() const
     {
@@ -54,8 +87,22 @@ class time_reparametrization_spline: public spline_deriv_constraint_one_dim
         waypoints.push_back(std::make_pair(1,point_one_dim_t::Ones()));
         return spline_deriv_constraint_one_dim(waypoints.begin(), waypoints.end());
     }
+
+    virtual time_t min() const{return min_;}
+    virtual time_t max() const{return max_;}
+
+    public:
+    Eigen::Quaterniond quat_from_; //const
+    Eigen::Quaterniond quat_to_;   //const
+    double min_;                   //const
+    double max_;                   //const
+    spline_deriv_constraint_one_dim time_reparam_; //const
 };
 
+
+typedef exact_cubic<Time, Numeric, 4, false, quat_t, std::vector<quat_t,Eigen::aligned_allocator<quat_t> >, rotation_spline> exact_cubic_quat_t;
+
+
 /// \class effector_spline_rotation
 /// \brief Represents a trajectory for and end effector
 /// uses the method effector_spline to create a spline trajectory.
@@ -93,7 +140,40 @@ class effector_spline_rotation
         , to_quat_(to_quat.data()), land_quat_(land_quat.data())
         , time_lift_offset_(spline_->min()+lift_offset_duration)
         , time_land_offset_(spline_->max()-land_offset_duration)
-        , rotation_spline_()
+        , quat_spline_(simple_quat_spline())
+    {
+        // NOTHING
+    }
+
+    /// \brief Constructor.
+    /// Given a set of waypoints, and the normal vector of the start and
+    /// ending positions, automatically create the spline such that:
+    /// + init and end velocities / accelerations are 0.
+    /// + the effector lifts and lands exactly in the direction of the specified normals
+    /// \param wayPointsBegin       : an iterator pointing to the first element of a waypoint container
+    /// \param wayPointsEnd         : an iterator pointing to the end           of a waypoint container
+    /// \param quatWayPointsBegin   : en iterator pointing to the first element of a 4D vector (x, y, z, w) container of
+    ///  quaternions indicating rotation at specific time steps.
+    /// \param quatWayPointsEnd     : en iterator pointing to the end           of a 4D vector (x, y, z, w) container of
+    ///  quaternions indicating rotation at specific time steps.
+    /// \param lift_normal          : normal to be followed by end effector at take-off
+    /// \param land_normal          : normal to be followed by end effector at landing
+    /// \param lift_offset          : length of the straight line along normal at take-off
+    /// \param land_offset          : length of the straight line along normal at landing
+    /// \param lift_offset_duration : time travelled along straight line at take-off
+    /// \param land_offset_duration : time travelled along straight line at landing
+    ///
+    template<typename In, typename InQuat>
+    effector_spline_rotation(In wayPointsBegin, In wayPointsEnd,
+            InQuat quatWayPointsBegin, InQuat quatWayPointsEnd,
+            const Point& lift_normal=Eigen::Vector3d::UnitZ(), const Point& land_normal=Eigen::Vector3d::UnitZ(),
+            const Numeric lift_offset=0.02, const Numeric land_offset=0.02,
+            const Time lift_offset_duration=0.02, const Time land_offset_duration=0.02)
+        : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset, lift_offset_duration, land_offset_duration))
+        , to_quat_((quatWayPointsBegin->second).data()), land_quat_(((quatWayPointsEnd-1)->second).data())
+        , time_lift_offset_(spline_->min()+lift_offset_duration)
+        , time_land_offset_(spline_->max()-land_offset_duration)
+        , quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd))
     {
         // NOTHING
     }
@@ -122,19 +202,39 @@ class effector_spline_rotation
         return res;
     }
 
-
-    ///  \brief Interpolates between two quaternions
-    ///  \param t : the time when to evaluate the spline
-    ///  \param quat : quaternion updated as the interpolation result
-    ///
-    quat_t interpolate_quat(Numeric t) const
+    public:
+    quat_t interpolate_quat(const Numeric t) const
     {
         if(t<=time_lift_offset_) return to_quat_.coeffs();
         if(t>=time_land_offset_) return land_quat_.coeffs();
-        //normalize u
-        Numeric u = (t - time_lift_offset_) /(time_land_offset_ - time_lift_offset_);
-        // reparametrize u
-        return to_quat_.slerp(rotation_spline_(u)[0], land_quat_).coeffs();
+        return quat_spline_(t);
+    }
+
+    private:
+    exact_cubic_quat_t simple_quat_spline() const
+    {
+        std::vector<rotation_spline> splines;
+        splines.push_back(rotation_spline(to_quat_.coeffs(),land_quat_.coeffs(),time_lift_offset_, time_land_offset_));
+        return exact_cubic_quat_t(splines);
+    }
+
+    template <typename InQuat>
+    exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, InQuat quatWayPointsEnd) const
+    {
+        if(std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1)
+            return simple_quat_spline();
+        exact_cubic_quat_t::t_spline_t splines;
+        InQuat it(quatWayPointsBegin);
+        Time init = time_lift_offset_;
+        Eigen::Quaterniond current_quat = to_quat_;
+        for(; it != quatWayPointsEnd; ++it)
+        {
+            splines.push_back(rotation_spline(current_quat.coeffs(), it->second, init, it->first));
+            current_quat = it->second;
+            init = it->first;
+        }
+        splines.push_back(rotation_spline(current_quat.coeffs(), land_quat_.coeffs(), init, time_land_offset_));
+        return exact_cubic_quat_t(splines);
     }
 
     /*Operations*/
@@ -146,7 +246,7 @@ class effector_spline_rotation
     const Eigen::Quaterniond land_quat_;
     const double time_lift_offset_;
     const double time_land_offset_;
-    const time_reparametrization_spline rotation_spline_; // should be static
+    const exact_cubic_quat_t quat_spline_;
     /*Attributes*/
 };
 

include/spline/spline_deriv_constraint.h

@@ -38,8 +38,9 @@ namespace spline
 ///
 template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
          typename Point= Eigen::Matrix<Numeric, Dim, 1>,
-         typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
-struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point>
+         typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> >,
+         typename SplineBase=spline_curve<Time, Numeric, Dim, Safe, Point, T_Point> >
+struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase>
 {
     typedef Point 	point_t;
     typedef T_Point t_point_t;
@@ -83,7 +84,6 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
     spline_deriv_constraint(const spline_deriv_constraint& other)
         : exact_cubic_t(other.subSplines_) {}
 
-
     private:
     template<typename In>
     void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
@@ -146,7 +146,6 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
     }
 
     private:
-    spline_deriv_constraint& operator=(const spline_deriv_constraint&);
     /* Constructors - destructors */
 };
 }

src/tests/spline_test/Main.cpp

@@ -429,6 +429,13 @@ helpers::quat_t GetXRotQuat(const double theta)
     return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
 }
 
+double GetXRotFromQuat(helpers::quat_ref_const_t q)
+{
+    Eigen::Quaterniond quat (q.data());
+    Eigen::AngleAxisd m (quat);
+    return m.angle() / M_PI * 180.;
+}
+
 void EffectorSplineRotationNoRotationTest(bool& error)
 {
     // create arbitrary trajectory
@@ -466,7 +473,7 @@ void EffectorSplineRotationRotationTest(bool& error)
     helpers::config_t q_to   = q_init; q_to(2)  +=0.02;
     helpers::config_t q_land = q_end ; q_land(2)+=0.02;
     helpers::quat_t q_mod = GetXRotQuat(M_PI_2);;
-    std::string errmsg("Error in EffectorSplineRotationNoRotationTest; while checking waypoints (expected / obtained)");
+    std::string errmsg("Error in EffectorSplineRotationRotationTest; while checking waypoints (expected / obtained)");
     ComparePoints(q_init, eff_traj(0),           errmsg,error);
     ComparePoints(q_to  , eff_traj(0.02),        errmsg,error);
     ComparePoints(q_land, eff_traj(9.98),        errmsg,error);
@@ -474,9 +481,46 @@ void EffectorSplineRotationRotationTest(bool& error)
     ComparePoints(q_end , eff_traj(10),          errmsg,error);
 }
 
+void EffectorSplineRotationWayPointRotationTest(bool& error)
+{
+    // create arbitrary trajectory
+    spline::T_Waypoint waypoints;
+    for(double i = 0; i <= 10; i = i + 2)
+    {
+        waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+    }
+    helpers::quat_t init_quat = GetXRotQuat(0);
+    helpers::t_waypoint_quat_t quat_waypoints_;
+
+
+    helpers::quat_t q_pi_0 = GetXRotQuat(0);
+    helpers::quat_t q_pi_2 = GetXRotQuat(M_PI_2);
+    helpers::quat_t q_pi   = GetXRotQuat(M_PI);
+
+    quat_waypoints_.push_back(std::make_pair(0.4,q_pi_0));
+    quat_waypoints_.push_back(std::make_pair(6,q_pi_2));
+    quat_waypoints_.push_back(std::make_pair(8,q_pi));
+
+
+    helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end(),
+                                               quat_waypoints_.begin(), quat_waypoints_.end());
+    helpers::config_t q_init =  helpers::config_t::Zero(); q_init.tail<4>() = init_quat;
+    helpers::config_t q_end; q_end      << 10.,10.,10.,0.,0.,0.,1.; q_end.tail<4>() = q_pi;
+    helpers::config_t q_mod; q_mod.head<3>() = point_t(6,6,6) ; q_mod.tail<4>() = q_pi_2;
+    helpers::config_t q_to   = q_init; q_to(2)  +=0.02;
+    helpers::config_t q_land = q_end ; q_land(2)+=0.02;
+    std::string errmsg("Error in EffectorSplineRotationWayPointRotationTest; while checking waypoints (expected / obtained)");
+    ComparePoints(q_init, eff_traj(0),           errmsg,error);
+    ComparePoints(q_to  , eff_traj(0.02),        errmsg,error);
+    ComparePoints(q_land, eff_traj(9.98),        errmsg,error);
+    ComparePoints(q_mod , eff_traj(6), errmsg,error);
+    ComparePoints(q_end , eff_traj(10),          errmsg,error);
+}
+
 void TestReparametrization(bool& error)
 {
-    helpers::time_reparametrization_spline sp;
+    helpers::rotation_spline s;
+    const helpers::spline_deriv_constraint_one_dim& sp = s.time_reparam_;
     if(sp.min() != 0)
     {
         std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() << std::endl;
@@ -521,6 +565,7 @@ int main(int /*argc*/, char** /*argv[]*/)
     EffectorSplineRotationNoRotationTest(error);
     EffectorSplineRotationRotationTest(error);
     TestReparametrization(error);
+    EffectorSplineRotationWayPointRotationTest(error);
     BezierCurveTest(error);
 	if(error)
 	{