From e9882b85e101d1d6a9b3d6299cfb1a0c619b8393 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@laas.fr>
Date: Mon, 28 Nov 2016 16:10:48 +0100
Subject: [PATCH] start and end velocity constraints
---
.gitignore | 2 +
include/spline/curve_abc.h | 2 +-
include/spline/exact_cubic.h | 25 +++++-
include/spline/exact_cubic_vel_acc_cons.h | 101 +++++++++++-----------
include/spline/spline_curve.h | 27 ++++++
src/tests/spline_test/Main.cpp | 69 ++++++++++++---
6 files changed, 163 insertions(+), 63 deletions(-)
diff --git a/.gitignore b/.gitignore
index cb577ba..491eaf4 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,6 +1,8 @@
# Build and Release Folders
bin/
lib/
+build/
# temp files
.*~
+*.user
diff --git a/include/spline/curve_abc.h b/include/spline/curve_abc.h
index 8aedfec..0eb17f0 100644
--- a/include/spline/curve_abc.h
+++ b/include/spline/curve_abc.h
@@ -42,7 +42,7 @@ struct curve_abc : std::unary_function<Time, Point>
/// \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 = 0;
+ virtual point_t operator()(time_t t) const = 0;
/*Operations*/
/*Helpers*/
diff --git a/include/spline/exact_cubic.h b/include/spline/exact_cubic.h
index c87863b..e55d4d0 100644
--- a/include/spline/exact_cubic.h
+++ b/include/spline/exact_cubic.h
@@ -59,10 +59,16 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
: curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
+
+ ///\brief Constructor
+ ///\param subSplines: vector of subsplines
+ exact_cubic(const t_spline_t& subSplines)
+ : curve_abc_t(), subSplines_(subSplines) {}
+
///\brief Destructor
~exact_cubic(){}
- protected:
+ private:
template<typename In>
t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const
{
@@ -158,7 +164,22 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
return it->operator()(t);
}
}
- }
+ }
+
+ /// \brief Evaluation of the derivative spline at time t.
+ /// \param t : the time when to evaluate the spine
+ /// \param return : the value x(t)
+ virtual point_t derivate(time_t t, std::size_t order) 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->derivate(t, order);
+ }
+ }
+ }
/*Operations*/
/*Helpers*/
diff --git a/include/spline/exact_cubic_vel_acc_cons.h b/include/spline/exact_cubic_vel_acc_cons.h
index 7b64109..aa825f3 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 curve_abc<Time, Numeric, Dim, Safe, Point>
+struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point>
{
typedef Point point_t;
typedef T_Point t_point_t;
@@ -47,26 +47,65 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef Time time_t;
typedef Numeric num_t;
typedef spline_curve<time_t, Numeric, Dim, Safe, point_t, t_point_t> spline_t;
- typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
+ typedef exact_cubic<time_t, Numeric, Dim, Safe, point_t, t_point_t> exact_cubic_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;
+ public:
+ struct spline_constraints
+ {
+ spline_constraints():
+ init_vel(point_t::Zero()),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel),
+ init_normal(init_vel),end_normal(init_vel) {}
+
+ spline_constraints(const point_t& n0, point_t& n1):
+ init_vel(point_t::Zero()),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel),
+ init_normal(n0),end_normal(n1) {}
+
+ ~spline_constraints(){}
+ point_t init_vel;
+ point_t init_acc;
+ point_t end_vel;
+ point_t end_acc;
+ point_t init_normal;
+ point_t end_normal;
+ };
+
/* Constructors - destructors */
public:
///\brief Constructor
///\param wayPointsBegin : an iterator pointing to the first element of a waypoint container
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
- template<typename In>
- cubic_zero_vel(In wayPointsBegin, In wayPointsEnd)
- : curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
+ template<typename In>
+ cubic_zero_vel(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints = spline_constraints())
+ : exact_cubic_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}
///\brief Destructor
~cubic_zero_vel(){}
- protected:
+ private:
+ MatrixX setVelConstraintsAndComputeB(const spline_constraints& constraints,
+ const Eigen::Ref<MatrixX> x,
+ Eigen::Ref<MatrixX> h1, Eigen::Ref<MatrixX> h2) const
+ {
+ std::size_t size(h1.rows());
+ MatrixX cons = MatrixX::Zero(h1.rows(), Dim); // constraint matrix on b
+ cons.row(0) = constraints.init_vel;
+ cons.row(size-1) = constraints.end_vel;
+
+
+ h1(0,0)= 1; // activating init velocity constraint
+ h1(size-1,size-1)= 1; // activating end velocity constraint
+
+ MatrixX b = MatrixX::Zero(size,Dim);
+ MatrixX h1inv = h1.inverse();
+ b = h1inv * (h2 *x + cons); //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
+ return b;
+ }
+
template<typename In>
- t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const
+ t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) const
{
std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
if(Safe && size < 1)
@@ -84,12 +123,10 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
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)
@@ -102,13 +139,8 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
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;
+ h5(i,i) = 2 / dTi_cube;
+ h5(i,i+1) = -2 / dTi_cube;
h6(i,i) = 1 / dTi_sqr;
h6(i,i+1) = 1 / dTi_sqr;
if( i+2 < size)
@@ -129,12 +161,8 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
// 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
+ // init velocity given by a constraint on b0
+ MatrixX b = setVelConstraintsAndComputeB(constraints, x, h1, h2);
c = h3 * x + h4 * b;
d = h5 * x + h6 * b;
it= wayPointsBegin, next=wayPointsBegin; ++ next;
@@ -143,8 +171,8 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
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));
+ //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;
}
@@ -153,31 +181,6 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
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_;
diff --git a/include/spline/spline_curve.h b/include/spline/spline_curve.h
index 12a53a4..7e831f6 100644
--- a/include/spline/spline_curve.h
+++ b/include/spline/spline_curve.h
@@ -129,6 +129,33 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
currentPoint_ += cdt *coefficients_.col(i);
return currentPoint_;
}
+
+
+ /// \brief Evaluation of the derivative spline at time t.
+ /// \param t : the time when to evaluate the spine
+ /// \param return : the value x(t)
+ point_t derivate(time_t t, std::size_t order) const
+ {
+ if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
+ time_t const dt (t-t_min_);
+ time_t cdt(1);
+ point_t currentPoint_ = point_t::Zero();
+ for(int i = order; i < order_+1; ++i, cdt*=dt)
+ {
+ currentPoint_ += cdt *coefficients_.col(i) * fact(i, order);
+ }
+ return currentPoint_;
+ }
+
+ private:
+ num_t fact(const std::size_t n, const std::size_t order) const
+ {
+ std::size_t res(1);
+ for(std::size_t i = 0; i < order; ++i)
+ res *= n-i;
+ return res;
+ }
+
/*Operations*/
/*Helpers*/
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index 64cc78b..3d2434a 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/src/tests/spline_test/Main.cpp
@@ -16,7 +16,9 @@ typedef Eigen::Vector3d point_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef spline_curve <double, double, 3, true, point_t, t_point_t> cubic_function_t;
typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
+typedef cubic_zero_vel <double, double, 3, true, point_t> cubic_zero_vel_t;
typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t;
+typedef cubic_zero_vel_t::spline_constraints spline_constraints_t;
typedef std::pair<double, point_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
@@ -311,6 +313,24 @@ void ExactCubicOneDimTest(bool& error)
ComparePoints(one, res1, errmsg, error);
}
+void CheckWayPointConstraint(const std::string& errmsg, const double step, const spline::T_Waypoint& wayPoints, const exact_cubic_t* curve, bool& error )
+{
+ point_t res1;
+ for(double i = 0; i <= 1; i = i + step)
+ {
+ res1 = (*curve)(i);
+ ComparePoints(point_t(i,i,i), res1, errmsg, error);
+ }
+}
+
+void CheckDerivative(const std::string& errmsg, const double eval_point, const std::size_t order, const point_t& target, const exact_cubic_t* curve, bool& error )
+{
+ point_t res1;
+ res1 = curve->derivate(eval_point,order);
+ ComparePoints(target, res1, errmsg, error);
+}
+
+
void ExactCubicPointsCrossedTest(bool& error)
{
spline::T_Waypoint waypoints;
@@ -318,25 +338,52 @@ void ExactCubicPointsCrossedTest(bool& error)
{
waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
}
- exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
- point_t res1;
- for(double i = 0; i <= 1; i = i + 0.2)
- {
- res1 = exactCubic(i);
- std::string errmsg("Error While checking that given wayPoints are crossed (expected / obtained)");
- ComparePoints(point_t(i,i,i), res1, errmsg, error);
- }
+ exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
+ std::string errmsg("Error While checking that given wayPoints are crossed (expected / obtained)");
+ CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
+
}
-int main(int argc, char *argv[])
+void ExactCubicVelocityConstraintsTest(bool& error)
+{
+ spline::T_Waypoint waypoints;
+ for(double i = 0; i <= 1; i = i + 0.2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ std::string errmsg("Error in ExactCubicVelocityConstraintsTest (1); while checking that given wayPoints are crossed (expected / obtained)");
+ spline_constraints_t constraints;
+ cubic_zero_vel_t exactCubic(waypoints.begin(), waypoints.end());
+ // now check that init and end velocity are 0
+ CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
+ std::string errmsg3("Error in ExactCubicVelocityConstraintsTest (2); while checking derivative (expected / obtained)");
+ // now check derivatives
+ CheckDerivative(errmsg3,0,1,constraints.init_vel,&exactCubic, error);
+ CheckDerivative(errmsg3,1,1,constraints.end_vel,&exactCubic, error);
+
+ constraints.end_vel = point_t(1,2,3);
+ constraints.init_vel = point_t(-1,-2,-3);
+ std::string errmsg2("Error in ExactCubicVelocityConstraintsTest (3); while checking that given wayPoints are crossed (expected / obtained)");
+ cubic_zero_vel_t exactCubic2(waypoints.begin(), waypoints.end(),constraints);
+ CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error);
+
+ std::string errmsg4("Error in ExactCubicVelocityConstraintsTest (4); while checking derivative (expected / obtained)");
+ // now check derivatives
+ CheckDerivative(errmsg4,0,1,constraints.init_vel,&exactCubic2, error);
+ CheckDerivative(errmsg4,1,1,constraints.end_vel,&exactCubic2, error);
+}
+
+
+int main(int /*argc*/, char* /*argv[]*/)
{
std::cout << "performing tests... \n";
bool error = false;
- CubicFunctionTest(error);
+ CubicFunctionTest(error);
ExactCubicNoErrorTest(error);
ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
ExactCubicTwoPointsTest(error);
- ExactCubicOneDimTest(error);
+ ExactCubicOneDimTest(error);
+ ExactCubicVelocityConstraintsTest(error);
//BezierCurveTest(error);
if(error)
{
--
GitLab