diff --git a/include/spline/curve_abc.h b/include/spline/curve_abc.h
index 86f75198ef82f6316928b6dbb83f45a294b95148..c0bf4e15e744a0d69a5403f82b5c64a929f9d96f 100644
--- a/include/spline/curve_abc.h
+++ b/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*/
diff --git a/include/spline/exact_cubic.h b/include/spline/exact_cubic.h
index 5f507176f95ff42f18f5282434ab008e085c7db3..5e7b7a7ac25fe2023a1631f22ca0d4abcdfc97fe 100644
--- a/include/spline/exact_cubic.h
+++ b/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 */
diff --git a/include/spline/helpers/effector_spline_rotation.h b/include/spline/helpers/effector_spline_rotation.h
index f0377e02b247cce670fd3e2c14479c9aa4cbb996..a6f4422ba72afb161ea07658cd49d493f526302f 100644
--- a/include/spline/helpers/effector_spline_rotation.h
+++ b/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*/
};
diff --git a/include/spline/spline_curve.h b/include/spline/spline_curve.h
index 07f8248054ac1c669fec03233bc20052664208c0..f2347bac5c59838db6d147e63433eab7358c91ef 100644
--- a/include/spline/spline_curve.h
+++ b/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_;
diff --git a/include/spline/spline_deriv_constraint.h b/include/spline/spline_deriv_constraint.h
index 28cb05e903ce51e9149793073fda1be223045378..b8153381185c5117b3d7ee64df5c22abbb1c8770 100644
--- a/include/spline/spline_deriv_constraint.h
+++ b/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 */
};
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index 0f0a7ff27ee966170b7e60571eb43e2fb64a50d5..84e6108b9a408f91c6ff59ed40ca7f6f6a778de5 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/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)
{