From c1bb699c9b1e711b2e1cb67555082dc34f689a86 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Fri, 2 Dec 2016 17:17:14 +0100
Subject: [PATCH] spline with start and end vel and acc constraints working and
tested
---
include/spline/exact_cubic.h | 2 +-
include/spline/exact_cubic_vel_acc_cons.h | 130 +++++-----------------
include/spline/spline_curve.h | 8 --
src/tests/spline_test/Main.cpp | 15 ++-
4 files changed, 39 insertions(+), 116 deletions(-)
diff --git a/include/spline/exact_cubic.h b/include/spline/exact_cubic.h
index e55d4d0..8cb96c4 100644
--- a/include/spline/exact_cubic.h
+++ b/include/spline/exact_cubic.h
@@ -155,7 +155,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \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)
{
diff --git a/include/spline/exact_cubic_vel_acc_cons.h b/include/spline/exact_cubic_vel_acc_cons.h
index 37859dd..cb64418 100644
--- a/include/spline/exact_cubic_vel_acc_cons.h
+++ b/include/spline/exact_cubic_vel_acc_cons.h
@@ -44,6 +44,7 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
typedef Point point_t;
typedef T_Point t_point_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+ typedef Eigen::Matrix<Numeric, 3, 3> Matrix3;
typedef Time time_t;
typedef Numeric num_t;
typedef spline_curve<time_t, Numeric, Dim, Safe, point_t, t_point_t> spline_t;
@@ -84,33 +85,13 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
///\brief Destructor
~cubic_zero_vel(){}
- /*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;
- }*/
-
private:
template<typename In>
- void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines)
+ void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
{
const point_t& a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
- const point_t& b0 = constraints.init_vel , c0 = constraints.init_acc;
- const num_t& init_t = wayPointsBegin->first, end_t = - wayPointsNext->first;
+ const point_t& b0 = constraints.init_vel , c0 = constraints.init_acc / 2.;
+ const num_t& init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
subSplines.push_back(create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>
@@ -120,106 +101,51 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
}
template<typename In>
- void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines)
+ void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
{
const point_t& a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
const point_t& b0 = constraints.init_vel, b1 = constraints.end_vel,
- c0 = constraints.init_acc, c1 = constraints.end_acc;
- const num_t& init_t = wayPointsBegin->first, end_t = - wayPointsNext->first;
- const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
+ c0 = constraints.init_acc / 2., c1 = constraints.end_acc;
+ const num_t& init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
+ const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt, dt_4 = dt_3 * dt, dt_5 = dt_4 * dt;
//solving a system of four linear eq with 4 unknows: d0, e0
- const num_t alpha_0 = a1 - a0 + b0 *dt + c0 * dt_2;
- const num_t alpha_1 = b1 - b0 + b0 *dt + c0 * dt_2;
-
- /*subSplines.push_back(create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>
- (a0,b0,c0,d0,e0, init_t, end_t));
- constraints.init_vel = subSplines.back().derivate(end_t, 1);
- constraints.init_acc = subSplines.back().derivate(end_t, 2);*/
-
- //solving
+ const point_t alpha_0 = a1 - a0 - b0 *dt - c0 * dt_2;
+ const point_t alpha_1 = b1 - b0 - 2 *c0 * dt;
+ const point_t alpha_2 = c1 - 2 *c0;
+ const num_t x_d_0 = dt_3, x_d_1 = 3*dt_2, x_d_2 = 6*dt;
+ const num_t x_e_0 = dt_4, x_e_1 = 4*dt_3, x_e_2 = 12*dt_2;
+ const num_t x_f_0 = dt_5, x_f_1 = 5*dt_4, x_f_2 = 20*dt_3;
+
+ point_t d, e, f;
+ MatrixX rhs = MatrixX::Zero(3,Dim);
+ rhs.row(0) = alpha_0; rhs.row(1) = alpha_1; rhs.row(2) = alpha_2;
+ Matrix3 eq = Matrix3::Zero(3,3);
+ eq(0,0) = x_d_0; eq(0,1) = x_e_0; eq(0,2) = x_f_0;
+ eq(1,0) = x_d_1; eq(1,1) = x_e_1; eq(1,2) = x_f_1;
+ eq(2,0) = x_d_2; eq(2,1) = x_e_2; eq(2,2) = x_f_2;
+ rhs = eq.inverse().eval() * rhs;
+ d = rhs.row(0); e = rhs.row(1); f = rhs.row(2);
+
+ subSplines.push_back(create_quintic<Time,Numeric,Dim,Safe,Point,T_Point>
+ (a0,b0,c0,d,e,f, init_t, end_t));
}
+ private:
template<typename In>
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)
- {
throw; // TODO
- }
t_spline_t subSplines; subSplines.reserve(size-1);
spline_constraints cons = constraints;
In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd-1);
++next;
for(std::size_t i(0); next != end; ++next, ++it, ++i)
- {
compute_one_spline<In>(it, next, cons, subSplines);
- }
compute_end_spline<In>(it, next,cons, subSplines);
return subSplines;
- // then solving last step
-
- /*// 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 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;
- 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)
- {
- 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;
- // 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;
- 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;*/
}
diff --git a/include/spline/spline_curve.h b/include/spline/spline_curve.h
index 7e831f6..5e12363 100644
--- a/include/spline/spline_curve.h
+++ b/include/spline/spline_curve.h
@@ -102,13 +102,9 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
if(Safe)
{
if(t_min_ > t_max_)
- {
std::out_of_range("TODO");
- }
if(coefficients_.size() != order_+1)
- {
std::runtime_error("Spline order and coefficients do not match");
- }
}
}
@@ -141,9 +137,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
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_;
}
@@ -183,9 +177,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
std::size_t size = std::distance(zeroOrderCoefficient, highestOrderCoefficient);
coeff_t res = coeff_t(Dim, size); int i = 0;
for(In cit = zeroOrderCoefficient; cit != highestOrderCoefficient; ++cit, ++i)
- {
res.col(i) = *cit;
- }
return res;
}
}; //class spline_curve
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index 3d2434a..cb8cbda 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/src/tests/spline_test/Main.cpp
@@ -325,8 +325,7 @@ void CheckWayPointConstraint(const std::string& errmsg, const double step, const
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);
+ point_t res1 = curve->derivate(eval_point,order);
ComparePoints(target, res1, errmsg, error);
}
@@ -360,9 +359,13 @@ void ExactCubicVelocityConstraintsTest(bool& error)
// now check derivatives
CheckDerivative(errmsg3,0,1,constraints.init_vel,&exactCubic, error);
CheckDerivative(errmsg3,1,1,constraints.end_vel,&exactCubic, error);
+ CheckDerivative(errmsg3,0,2,constraints.init_acc,&exactCubic, error);
+ CheckDerivative(errmsg3,1,2,constraints.end_acc,&exactCubic, error);
constraints.end_vel = point_t(1,2,3);
constraints.init_vel = point_t(-1,-2,-3);
+ constraints.end_acc = point_t(4,5,6);
+ constraints.init_acc = point_t(-4,-4,-6);
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);
@@ -370,11 +373,13 @@ void ExactCubicVelocityConstraintsTest(bool& 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);
+ CheckDerivative(errmsg4,1,1,constraints.end_vel ,&exactCubic2, error);
+ CheckDerivative(errmsg4,0,2,constraints.init_acc,&exactCubic2, error);
+ CheckDerivative(errmsg4,1,2,constraints.end_acc ,&exactCubic2, error);
}
-int main(int /*argc*/, char* /*argv[]*/)
+int main(int /*argc*/, char** /*argv[]*/)
{
std::cout << "performing tests... \n";
bool error = false;
@@ -384,7 +389,7 @@ int main(int /*argc*/, char* /*argv[]*/)
ExactCubicTwoPointsTest(error);
ExactCubicOneDimTest(error);
ExactCubicVelocityConstraintsTest(error);
- //BezierCurveTest(error);
+ //BezierCurveTest(error);
if(error)
{
std::cout << "There were some errors\n";
--
GitLab