Skip to content

GitLab

Commit c1bb699c authored by Steve Tonneau's avatar Steve Tonneau
Browse files

spline with start and end vel and acc constraints working and tested

parent 727b52ea
Hide whitespace changes
Inline Side-by-side
include/spline/exact_cubic.h
View file @ c1bb699c
......@@ -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)
{
......
include/spline/exact_cubic_vel_acc_cons.h
View file @ c1bb699c
......@@ -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;*/
}
......
include/spline/spline_curve.h
View file @ c1bb699c
......@@ -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
......
src/tests/spline_test/Main.cpp
View file @ c1bb699c
......@@ -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";
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment