Commit e7701e1f authored by JasonChmn's avatar JasonChmn Committed by Pierre Fernbach
Browse files

[curve_constraint/ exact_cubic] Add dim to curve_constraint template to make...

[curve_constraint/ exact_cubic] Add dim to curve_constraint template to make it works with dynamic size + fix exact_cubic with dynamic size => Tests OK
parent 172bceb4
......@@ -19,12 +19,12 @@
namespace curves
{
template <typename Point>
template <typename Point, std::size_t Dim=3>
struct curve_constraints
{
typedef Point point_t;
curve_constraints():
init_vel(point_t::Zero()),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel){}
init_vel(point_t::Zero(Dim)),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel){}
~curve_constraints(){}
......
......@@ -51,8 +51,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Safe, Point>
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;
typedef curve_abc<Time, Numeric, Safe, Point> curve_abc_t;
typedef curve_constraints<point_t> spline_constraints;
typedef curve_constraints<Point, Dim> spline_constraints;
/* Constructors - destructors */
public:
......@@ -62,7 +61,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Safe, Point>
///
template<typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
: curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
: subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
......@@ -71,16 +70,16 @@ struct exact_cubic : public curve_abc<Time, Numeric, Safe, Point>
///
template<typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints)
: curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}
: subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}
/// \brief Constructor.
/// \param subSplines: vector of subsplines.
exact_cubic(const t_spline_t& subSplines)
: curve_abc_t(), subSplines_(subSplines) {}
: subSplines_(subSplines) {}
/// \brief Copy Constructor.
exact_cubic(const exact_cubic& other)
: curve_abc_t(), subSplines_(other.subSplines_) {}
: subSplines_(other.subSplines_) {}
/// \brief Destructor.
virtual ~exact_cubic(){}
......
......@@ -1318,7 +1318,8 @@ void curveAbcDimDynamicTest(bool& error)
{
typedef curve_abc<double,double,true> curve_abc_test_t;
typedef polynomial <double, double, 3, true> polynomial_test_t;
//typedef exact_cubic <double, double, 3, true, Eigen::Matrix<double, Eigen::Dynamic, 1> > exact_cubic_test_t;
typedef exact_cubic <double, double, 3, true> exact_cubic_test_t;
typedef exact_cubic_test_t::spline_constraints spline_constraints_test_t;
typedef bezier_curve <double, double, 3, true> bezier_curve_test_t;
typedef cubic_hermite_spline <double, double, 3, true> cubic_hermite_spline_test_t;
curve_abc_test_t * pt_curve_abc;
......@@ -1331,7 +1332,8 @@ void curveAbcDimDynamicTest(bool& error)
polynomial_test_t pol(vec.begin(), vec.end(), 0, 1);
try
{
std::cout<<"pol : "<<pol(0).transpose()<<" / "<<pol(1).transpose()<<std::endl;
pol(0);
pol(1);
}
catch(...)
{
......@@ -1341,7 +1343,8 @@ void curveAbcDimDynamicTest(bool& error)
bezier_curve_test_t bc = bezier_from_curve<bezier_curve_test_t, polynomial_test_t>(pol);
try
{
std::cout<<"bc : "<<bc(0).transpose()<<" / "<<bc(1).transpose()<<std::endl;
bc(0);
bc(1);
}
catch(...)
{
......@@ -1351,39 +1354,51 @@ void curveAbcDimDynamicTest(bool& error)
cubic_hermite_spline_test_t chs = hermite_from_curve<cubic_hermite_spline_test_t, polynomial_test_t>(pol);
try
{
std::cout<<"chs : "<<chs(0).transpose()<<" / "<<chs(1).transpose()<<std::endl;
chs(0);
chs(1);
}
catch(...)
{
error = false;
}
// EXACT CUBIC : NOT SUPPORTED, problem to fix later
/*
curves::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;
spline_constraints_test_t constraints;
constraints.end_vel = point_t(0,0,0);
constraints.init_vel = point_t(0,0,0);
constraints.end_acc = point_t(0,0,0);
constraints.init_acc = point_t(0,0,0);
exact_cubic_test_t ec(waypoints.begin(), waypoints.end(), constraints);
//std::cout<<"ec : "<<ec(0).transpose()<<" / "<<ec(1).transpose()<<std::endl;
*/
try
{
ec(0);
ec(1);
}
catch(...)
{
error = false;
}
// Test with pointer to curve_abc type
try
{
pt_curve_abc = &pol;
std::cout<<"curve_abc - pol : "<<(*pt_curve_abc)(0).transpose()<<" / "<<(*pt_curve_abc)(1).transpose()<<std::endl;
(*pt_curve_abc)(0);
(*pt_curve_abc)(1);
pt_curve_abc = &bc;
std::cout<<"curve_abc - bc : "<<(*pt_curve_abc)(0).transpose()<<" / "<<(*pt_curve_abc)(1).transpose()<<std::endl;
(*pt_curve_abc)(0);
(*pt_curve_abc)(1);
pt_curve_abc = &chs;
std::cout<<"curve_abc - chs : "<<(*pt_curve_abc)(0).transpose()<<" / "<<(*pt_curve_abc)(1).transpose()<<std::endl;
//pt_curve_abc = &ec;
//std::cout<<"curve_abc - ec : "<<(*pt_curve_abc)(0).transpose()<<" / "<<(*pt_curve_abc)(1).transpose()<<std::endl;
(*pt_curve_abc)(0);
(*pt_curve_abc)(1);
pt_curve_abc = &ec;
(*pt_curve_abc)(0);
(*pt_curve_abc)(1);
}
catch(...)
{
......
Supports Markdown
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