Skip to content
Snippets Groups Projects
Commit e9882b85 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

start and end velocity constraints

parent 48067afc
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
.gitignore
+ 2
− 0
View file @ e9882b85
# Build and Release Folders
bin/
lib/
build/
# temp files
.*~
*.user
include/spline/curve_abc.h
+ 1
− 1
View file @ e9882b85
......@@ -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*/
......
include/spline/exact_cubic.h
+ 23
− 2
View file @ e9882b85
......@@ -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*/
......
include/spline/exact_cubic_vel_acc_cons.h
+ 52
− 49
View file @ e9882b85
......@@ -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_;
......
include/spline/spline_curve.h
+ 27
− 0
View file @ e9882b85
......@@ -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*/
......
src/tests/spline_test/Main.cpp
+ 58
− 11
View file @ e9882b85
......@@ -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)
{
......
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