Commit 686f698b authored by Steve Tonneau's avatar Steve Tonneau

vel acc constraints for bezier curves

parent 82884af0
......@@ -12,6 +12,7 @@
#include "curve_abc.h"
#include "bernstein.h"
#include "curve_constraint.h"
#include "MathDefs.h"
......@@ -34,7 +35,9 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef Point point_t;
typedef Time time_t;
typedef Numeric num_t;
typedef curve_constraints<point_t> curve_constraints_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef typename t_point_t::const_iterator cit_point_t;
typedef bezier_curve<Time, Numeric, Dim, Safe, Point > bezier_curve_t;
/* Constructors - destructors */
......@@ -49,18 +52,36 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
, size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>(degree_))
{
{
assert(bernstein_.size() == size_);
In it(PointsBegin);
if(Safe && (size_<1 || minBound >= maxBound))
{
throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
}
for(; it != PointsEnd; ++it)
{
pts_.push_back(*it);
}
}
for(; it != PointsEnd; ++it)
pts_.push_back(*it);
}
///\brief Constructor
/// This constructor will add 4 points (2 after the first one, 2 before the last one)
/// to ensure that velocity and acceleration constraints are respected
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///\param constraints : constraints applying on start / end velocities and acceleration
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t minBound=0, const time_t maxBound=1)
: minBound_(minBound)
, maxBound_(maxBound)
, size_(std::distance(PointsBegin, PointsEnd)+4)
, degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>(degree_))
{
if(Safe && (size_<1 || minBound >= maxBound))
throw std::out_of_range("can't create bezier min bound is higher than max bound");
t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
for(cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit)
pts_.push_back(*cit);
}
///\brief Destructor
~bezier_curve()
......@@ -86,8 +107,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
}
else
{
num_t dt = (1 - nT);
{
num_t dt = (1 - nT);
switch(size_)
{
case 1 :
......@@ -106,7 +127,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
+ 3 * pts_[2] * nT * nT * dt
+ pts_[3] * nT * nT *nT;
default :
return evalBernstein(dt);
return evalBernstein(nT);
break;
}
}
......@@ -154,7 +175,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param order : order of the derivative
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual point_t derivate(const time_t t, const std::size_t order) const
virtual point_t derivate(const time_t t, const std::size_t order) const
{
bezier_curve_t deriv =compute_derivate(order);
return deriv(t);
......@@ -169,17 +190,41 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
typename t_point_t::const_iterator pts_it = pts_.begin();
for(typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin();
cit !=bernstein_.end(); ++cit, ++pts_it)
{
res += cit->operator()(u) * (*pts_it);
}
return res;
}
const t_point_t& waypoints() const {return pts_;}
private:
template<typename In>
t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints)
{
t_point_t res;
point_t P0, P1, P2, P_n_2, P_n_1, PN;
P0 = *PointsBegin; PN = *(PointsEnd-1);
P1 = P0+ constraints.init_vel / degree_;
P_n_1 = PN -constraints.end_vel / degree_;
P2 = constraints.init_acc / (degree_ * (degree_-1)) + 2* P1 - P0;
P_n_2 = constraints.end_acc / (degree_ * (degree_-1)) + 2* P_n_1 - PN;
res.push_back(P0);
res.push_back(P1);
res.push_back(P2);
for(In it = PointsBegin+1; it != PointsEnd-1; ++it)
res.push_back(*it);
res.push_back(P_n_2);
res.push_back(P_n_1);
res.push_back(PN);
return res;
}
/*Operations*/
/*Helpers*/
public:
virtual time_t min() const{return minBound_;}
virtual time_t max() const{return maxBound_;}
/*Helpers*/
......
......@@ -38,6 +38,7 @@ typedef std::vector<waypoint_t, Eigen::aligned_allocator<point_t> > t_waypoint_t
typedef spline::spline_deriv_constraint <real, real, 3, true, point_t, t_point_t> spline_deriv_constraint_t;
typedef spline::curve_constraints<point_t> curve_constraints_t;
typedef spline::curve_constraints<point6_t> curve_constraints6_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
......@@ -72,6 +73,19 @@ bezier_t* wrapBezierConstructorBounds(const point_list_t& array, const real lb,
return new bezier_t(asVector.begin(), asVector.end(), lb, ub);
}
bezier_t* wrapBezierConstructorConstraints(const point_list_t& array, const curve_constraints_t& constraints)
{
t_point_t asVector = vectorFromEigenArray<point_list_t, t_point_t>(array);
return new bezier_t(asVector.begin(), asVector.end(), constraints);
}
bezier_t* wrapBezierConstructorConstraintsBounds(const point_list_t& array, const curve_constraints_t& constraints, const real lb, const real ub)
{
t_point_t asVector = vectorFromEigenArray<point_list_t, t_point_t>(array);
return new bezier_t(asVector.begin(), asVector.end(), constraints, lb, ub);
}
bezier6_t* wrapBezierConstructor6(const point_list6_t& array)
{
t_point6_t asVector = vectorFromEigenArray<point_list6_t, t_point6_t>(array);
......@@ -85,6 +99,19 @@ bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real l
return new bezier6_t(asVector.begin(), asVector.end(), lb, ub);
}
bezier6_t* wrapBezierConstructor6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints)
{
t_point6_t asVector = vectorFromEigenArray<point_list6_t, t_point6_t>(array);
return new bezier6_t(asVector.begin(), asVector.end(), constraints);
}
bezier6_t* wrapBezierConstructorBounds6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints, const real lb, const real ub)
{
t_point6_t asVector = vectorFromEigenArray<point_list6_t, t_point6_t>(array);
return new bezier6_t(asVector.begin(), asVector.end(), constraints, lb, ub);
}
spline_curve_t* wrapSplineConstructor(const coeff_t& array)
{
return new spline_curve_t(array, 0., 1.);
......
......@@ -60,7 +60,7 @@ void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const
if((pt1-pt2).norm() > margin && !notequal)
{
error = true;
std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
std::cout << errmsg << pt1.transpose() << " ; " << pt2.transpose() << std::endl;
}
}
......@@ -247,6 +247,37 @@ void BezierDerivativeCurveTest(bool& error)
ComparePoints(point_t::Zero(), cf3.derivate(0.,100), errMsg, error);
}
void BezierDerivativeCurveConstraintTest(bool& error)
{
std::string errMsg("In test BezierDerivativeCurveConstraintTest ; unexpected result for x ");
point_t a(1,2,3);
point_t b(2,3,4);
point_t c(3,4,5);
bezier_curve_t::curve_constraints_t constraints;
constraints.init_vel = point_t(-1,-1,-1);
constraints.init_acc = point_t(-2,-2,-2);
constraints.end_vel = point_t(-10,-10,-10);
constraints.end_acc = point_t(-20,-20,-20);
std::vector<point_t> params;
params.push_back(a);
params.push_back(b);
params.push_back(c);
bezier_curve_t cf3(params.begin(), params.end(), constraints);
assert(cf3.degree_ == params.size() + 3);
assert(cf3.size_ == params.size() + 4);
ComparePoints(a, cf3(0), errMsg, error);
ComparePoints(c, cf3(1), errMsg, error);
ComparePoints(constraints.init_vel, cf3.derivate(0.,1), errMsg, error);
ComparePoints(constraints.end_vel , cf3.derivate(1.,1), errMsg, error);
ComparePoints(constraints.init_acc, cf3.derivate(0.,2), errMsg, error);
ComparePoints(constraints.end_vel, cf3.derivate(1.,1), errMsg, error);
ComparePoints(constraints.end_acc, cf3.derivate(1.,2), errMsg, error);
}
/*Exact Cubic Function tests*/
......@@ -599,6 +630,7 @@ int main(int /*argc*/, char** /*argv[]*/)
EffectorSplineRotationWayPointRotationTest(error);
BezierCurveTest(error);
BezierDerivativeCurveTest(error);
BezierDerivativeCurveConstraintTest(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