From 48067afceafc115086cfbeee1546986d85192a36 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Fri, 25 Nov 2016 19:22:36 +0100
Subject: [PATCH] zero init velocity
---
include/spline/exact_cubic.h | 6 +-
include/spline/exact_cubic_vel_acc_cons.h | 123 ++++++++++++++++++++--
include/spline/spline_curve.h | 8 +-
3 files changed, 123 insertions(+), 14 deletions(-)
diff --git a/include/spline/exact_cubic.h b/include/spline/exact_cubic.h
index 3de5908..c87863b 100644
--- a/include/spline/exact_cubic.h
+++ b/include/spline/exact_cubic.h
@@ -41,7 +41,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef T_Point t_point_t;
- typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+ 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;
@@ -57,9 +57,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
template<typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
- : curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd))
- {
- }
+ : curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
///\brief Destructor
~exact_cubic(){}
diff --git a/include/spline/exact_cubic_vel_acc_cons.h b/include/spline/exact_cubic_vel_acc_cons.h
index 2093e25..7b64109 100644
--- a/include/spline/exact_cubic_vel_acc_cons.h
+++ b/include/spline/exact_cubic_vel_acc_cons.h
@@ -39,7 +39,7 @@ 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 cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point>
+struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef T_Point t_point_t;
@@ -47,7 +47,7 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
typedef Time time_t;
typedef Numeric num_t;
typedef spline_curve<time_t, Numeric, Dim, Safe, point_t, t_point_t> spline_t;
- typedef exact_cubic<Time, Numeric, Dim, Safe, Point> exact_cubic_t;
+ typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_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;
@@ -59,18 +59,129 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
template<typename In>
cubic_zero_vel(In wayPointsBegin, In wayPointsEnd)
- : exact_cubic_t(wayPointsBegin, wayPointsEnd)
- {
- // NOTHING
- }
+ : curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
///\brief Destructor
~cubic_zero_vel(){}
+ protected:
+ template<typename In>
+ t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const
+ {
+ std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
+ if(Safe && size < 1)
+ {
+ throw; // TODO
+ }
+ t_spline_t subSplines; subSplines.reserve(size);
+
+ // refer to the paper to understand all this.
+ MatrixX h1 = MatrixX::Zero(size, size);
+ MatrixX h2 = MatrixX::Zero(size, size);
+ MatrixX h3 = MatrixX::Zero(size, size);
+ MatrixX h4 = MatrixX::Zero(size, size);
+ MatrixX h5 = MatrixX::Zero(size, size);
+ MatrixX h6 = MatrixX::Zero(size, size);
+
+ MatrixX a = MatrixX::Zero(size, Dim);
+ MatrixX b = MatrixX::Zero(size, Dim);
+ MatrixX c = MatrixX::Zero(size, Dim);
+ MatrixX d = MatrixX::Zero(size, Dim);
+ MatrixX x = MatrixX::Zero(size, Dim);
+
+
+ In it(wayPointsBegin), next(wayPointsBegin);
+ ++next;
+ for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i)
+ {
+ num_t const dTi((*next).first - (*it).first);
+ num_t const dTi_sqr(dTi * dTi);
+ num_t const dTi_cube(dTi_sqr * dTi);
+ // filling matrices values
+ h3(i,i) = -3 / dTi_sqr;
+ h3(i,i+1) = 3 / dTi_sqr;
+ h4(i,i) = -2 / dTi;
+ h4(i,i+1) = -1 / dTi;
+ num_t coeff_h5(-2);
+ if(i == 0)
+ {
+ coeff_h5 = 1;
+ }
+ h5(i,i) = -coeff_h5 / dTi_cube;
+ h5(i,i+1) = coeff_h5 / dTi_cube;
+ h6(i,i) = 1 / dTi_sqr;
+ h6(i,i+1) = 1 / dTi_sqr;
+ if( i+2 < size)
+ {
+ In it2(next); ++ it2;
+ num_t const dTi_1((*it2).first - (*next).first);
+ num_t const dTi_1sqr(dTi_1 * dTi_1);
+ // this can be optimized but let's focus on clarity as long as not needed
+ h1(i+1, i) = 2 / dTi;
+ h1(i+1, i+1) = 4 / dTi + 4 / dTi_1;
+ h1(i+1, i+2) = 2 / dTi_1;
+ h2(i+1, i) = -6 / dTi_sqr;
+ h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
+ h2(i+1, i+2) = 6 / dTi_1sqr;
+ }
+ x.row(i)= (*it).second.transpose();
+ }
+ // adding last x
+ x.row(size-1)= (*it).second.transpose();
+ a= x;
+ h1(0,0)= 1;
+ h3(0,0)= 0;h3(0,1)= 0;
+ h4(0,0)= 0;h4(0,1)= 0;
+ h6(0,0)= 0;h6(0,1)= 0;
+ PseudoInverse(h1);
+ b = h1 * h2 * x; //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
+ c = h3 * x + h4 * b;
+ d = h5 * x + h6 * b;
+ it= wayPointsBegin, next=wayPointsBegin; ++ next;
+ for(int i=0; next != wayPointsEnd; ++i, ++it, ++next)
+ {
+ subSplines.push_back(
+ create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a.row(i), b.row(i), c.row(i), d.row(i),(*it).first, (*next).first));
+ }
+ subSplines.push_back(
+ create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first));
+ return subSplines;
+ }
+
+
private:
cubic_zero_vel(const cubic_zero_vel&);
cubic_zero_vel& operator=(const cubic_zero_vel&);
/* Constructors - destructors */
+
+ /*Operations*/
+ public:
+ /// \brief Evaluation of the cubic spline at time t.
+ /// \param t : the time when to evaluate the spine
+ /// \param return : the value x(t)
+ virtual point_t operator()(time_t t) const
+ {
+ if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())){throw std::out_of_range("TODO");}
+ for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it)
+ {
+ if(t >= (it->min()) && t <= (it->max()))
+ {
+ return it->operator()(t);
+ }
+ }
+ }
+ /*Operations*/
+
+ /*Helpers*/
+ public:
+ num_t virtual min() const{return subSplines_.front().min();}
+ num_t virtual max() const{return subSplines_.back().max();}
+ /*Helpers*/
+
+ /*Attributes*/
+ public:
+ const t_spline_t subSplines_;
+ /*Attributes*/
};
}
#endif //_CLASS_CUBICZEROVELACC
diff --git a/include/spline/spline_curve.h b/include/spline/spline_curve.h
index a01f937..12a53a4 100644
--- a/include/spline/spline_curve.h
+++ b/include/spline/spline_curve.h
@@ -44,12 +44,12 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/* Constructors - destructors */
public:
///\brief Constructor
- ///\param coefficients : a container containing all coefficients of the spline, starting
- /// with the zero order coefficient, up to the highest order. Spline order is given
- /// by the size of the coefficients
+ ///\param coefficients : a reference to an Eigen matrix where each column is a coefficient,
+ /// from the zero order coefficient, up to the highest order. Spline order is given
+ /// by the number of the columns -1.
///\param min: LOWER bound on interval definition of the spline
///\param max: UPPER bound on interval definition of the spline
- spline_curve(const coeff_t_ref& coefficients, const time_t min, const time_t max)
+ spline_curve(const coeff_t_ref coefficients, const time_t min, const time_t max)
: curve_abc_t(),
coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.cols()-1)
{
--
GitLab