From 686f698bea58dcc943fc63e7ff9cbe755a5f1cf0 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@laas.fr>
Date: Wed, 5 Apr 2017 13:33:03 +0200
Subject: [PATCH] vel acc constraints for bezier curves
---
include/spline/bezier_curve.h | 73 +++++++++++++++++++++++++++-------
python/spline_python.cpp | 27 +++++++++++++
src/tests/spline_test/Main.cpp | 34 +++++++++++++++-
3 files changed, 119 insertions(+), 15 deletions(-)
diff --git a/include/spline/bezier_curve.h b/include/spline/bezier_curve.h
index 73fe61b..5285c4d 100644
--- a/include/spline/bezier_curve.h
+++ b/include/spline/bezier_curve.h
@@ -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*/
diff --git a/python/spline_python.cpp b/python/spline_python.cpp
index 2762f7d..3dfeb50 100644
--- a/python/spline_python.cpp
+++ b/python/spline_python.cpp
@@ -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.);
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index 2f2cd2d..6579c1a 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/src/tests/spline_test/Main.cpp
@@ -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";
--
GitLab