zero vel/acc also for rotations + added copy constructors for spline to avoid pointers

include/spline/curve_abc.h

@@ -31,10 +31,10 @@ struct  curve_abc : std::unary_function<Time, Point>
 /* Constructors - destructors */
 	public:
 	///\brief Constructor
-	curve_abc(){};
+    curve_abc(){}
 
 	///\brief Destructor
-	~curve_abc(){};
+    ~curve_abc(){}
 /* Constructors - destructors */
 
 /*Operations*/

include/spline/exact_cubic.h

@@ -65,6 +65,10 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
     exact_cubic(const t_spline_t& subSplines)
         : curve_abc_t(), subSplines_(subSplines) {}
 
+    ///\brief Copy Constructor
+    exact_cubic(const exact_cubic& other)
+        : curve_abc_t(), subSplines_(other.subSplines_) {}
+
 	///\brief Destructor
     ~exact_cubic(){}
 
@@ -144,8 +148,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
         return subSplines;
     }
 
-	private:
-	exact_cubic(const exact_cubic&);
+    private:
 	exact_cubic& operator=(const exact_cubic&);
 	/* Constructors - destructors */
 

include/spline/helpers/effector_spline_rotation.h

@@ -27,10 +27,34 @@ namespace spline
 {
 namespace helpers
 {
+
 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 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
+{
+    public:
+    time_reparametrization_spline()
+        : spline_deriv_constraint_one_dim(computeWayPoints()){}
+
+    ~time_reparametrization_spline(){}
+
+    spline_deriv_constraint_one_dim computeWayPoints() const
+    {
+        // initializing time reparametrization for spline
+        t_waypoint_one_dim_t waypoints;
+        waypoints.push_back(std::make_pair(0,point_one_dim_t::Zero()));
+        waypoints.push_back(std::make_pair(1,point_one_dim_t::Ones()));
+        return spline_deriv_constraint_one_dim(waypoints.begin(), waypoints.end());
+    }
+};
 
 /// \class effector_spline_rotation
 /// \brief Represents a trajectory for and end effector
@@ -69,6 +93,7 @@ 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_()
     {
         // NOTHING
     }
@@ -97,6 +122,7 @@ 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
@@ -107,8 +133,10 @@ class effector_spline_rotation
         if(t>=time_land_offset_) return land_quat_.coeffs();
         //normalize u
         Numeric u = (t - time_lift_offset_) /(time_land_offset_ - time_lift_offset_);
-        return to_quat_.slerp(u, land_quat_).coeffs();
+        // reparametrize u
+        return to_quat_.slerp(rotation_spline_(u)[0], land_quat_).coeffs();
     }
+
     /*Operations*/
 
     /*Attributes*/
@@ -118,6 +146,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
     /*Attributes*/
 };
 

include/spline/spline_curve.h

@@ -95,8 +95,10 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
         : coefficients_(other.coefficients_), dim_(other.dim_), order_(other.order_),
           t_min_(other.t_min_), t_max_(other.t_max_){}
 
-    private:
+
     //spline_curve& operator=(const spline_curve& other);
+
+    private:
     void safe_check()
     {
         if(Safe)
@@ -162,9 +164,9 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
 
 /*Attributes*/
     public:
-    coeff_t coefficients_;
-    std::size_t dim_;
-    std::size_t order_;
+    coeff_t coefficients_; //const
+    std::size_t dim_; //const
+    std::size_t order_; //const
 
     private:
     time_t t_min_, t_max_;

include/spline/spline_deriv_constraint.h

@@ -79,6 +79,11 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
 	///\brief Destructor
     ~spline_deriv_constraint(){}
 
+    ///\brief Copy Constructor
+    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
@@ -140,8 +145,7 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
         return subSplines;
     }
 
-	private:
-    spline_deriv_constraint(const spline_deriv_constraint&);
+    private:
     spline_deriv_constraint& operator=(const spline_deriv_constraint&);
     /* Constructors - destructors */
 };

src/tests/spline_test/Main.cpp

@@ -474,6 +474,39 @@ void EffectorSplineRotationRotationTest(bool& error)
     ComparePoints(q_end , eff_traj(10),          errmsg,error);
 }
 
+void TestReparametrization(bool& error)
+{
+    helpers::time_reparametrization_spline sp;
+    if(sp.min() != 0)
+    {
+        std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() << std::endl;
+        error = true;
+    }
+    if(sp.max() != 1)
+    {
+        std::cout << "in TestReparametrization; max value is not 1, got " << sp.max() << std::endl;
+        error = true;
+    }
+    if(sp(1)[0] != 1.)
+    {
+        std::cout << "in TestReparametrization; end value is not 1, got " << sp(1)[0] << std::endl;
+        error = true;
+    }
+    if(sp(0)[0] != 0.)
+    {
+        std::cout << "in TestReparametrization; init value is not 0, got " << sp(0)[0] << std::endl;
+        error = true;
+    }
+    for(double i =0; i<1; i+=0.002)
+    {
+        if(sp(i)[0]>sp(i+0.002)[0])
+        {
+            std::cout << "in TestReparametrization; reparametrization not monotonous " << sp.max() << std::endl;
+            error = true;
+        }
+    }
+}
+
 int main(int /*argc*/, char** /*argv[]*/)
 {
     std::cout << "performing tests... \n";
@@ -487,6 +520,7 @@ int main(int /*argc*/, char** /*argv[]*/)
     EffectorTrajectoryTest(error);
     EffectorSplineRotationNoRotationTest(error);
     EffectorSplineRotationRotationTest(error);
+    TestReparametrization(error);
     BezierCurveTest(error);
 	if(error)
 	{