From 9daa937ebe33e3ce65942fa1656de4b90f2e11d1 Mon Sep 17 00:00:00 2001
From: JasonChmn <jason.chemin@hotmail.fr>
Date: Tue, 21 May 2019 14:15:11 +0200
Subject: [PATCH] Modif for spline deriv constraint to exact cubic : delete
spline_deriv_constraint.h and modifications in other classes and tests
---
include/curves/curve_conversion.h | 2 +-
include/curves/helpers/effector_spline.h | 13 +-
.../curves/helpers/effector_spline_rotation.h | 10 +-
include/curves/piecewise_curve.h | 21 ++-
include/curves/spline_deriv_constraint.h | 148 ------------------
tests/Main.cpp | 2 +-
6 files changed, 33 insertions(+), 163 deletions(-)
delete mode 100644 include/curves/spline_deriv_constraint.h
diff --git a/include/curves/curve_conversion.h b/include/curves/curve_conversion.h
index d042b35..58266c7 100644
--- a/include/curves/curve_conversion.h
+++ b/include/curves/curve_conversion.h
@@ -66,7 +66,7 @@ Bezier bezier_from_hermite(const Hermite& curve)
// Convert to bezier control points
// for t in [0,1] : x'(0)=3(b_p1-b_p0) and x'(1)=3(b_p3-b_p2)
// so : h_m0=3(b_p1-b_p0) and h_m1=3(b_p3-b_p2)
- // <=> b_p1=(h_m0/3)+b_p0 and b_p2=(h_m1/3)+b_p3
+ // <=> b_p1=(h_m0/3)+b_p0 and b_p2=-(h_m1/3)+b_p3
point_t b_p0 = h_p0;
point_t b_p3 = h_p1;
point_t b_p1 = (h_m0/3)+b_p0;
diff --git a/include/curves/helpers/effector_spline.h b/include/curves/helpers/effector_spline.h
index 6e26d6b..2a8fb59 100644
--- a/include/curves/helpers/effector_spline.h
+++ b/include/curves/helpers/effector_spline.h
@@ -20,7 +20,7 @@
#ifndef _CLASS_EFFECTORSPLINE
#define _CLASS_EFFECTORSPLINE
-#include "curves/spline_deriv_constraint.h"
+#include "curves/exact_cubic.h"
namespace curves
{
@@ -32,11 +32,10 @@ typedef Eigen::Matrix<Numeric, 3, 1> Point;
typedef std::vector<Point,Eigen::aligned_allocator<Point> > T_Point;
typedef std::pair<double, Point> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
-typedef spline_deriv_constraint<Time, Numeric, 3, true, Point, T_Point> spline_deriv_constraint_t;
-typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
-typedef spline_deriv_constraint_t::exact_cubic_t exact_cubic_t;
-typedef spline_deriv_constraint_t::t_spline_t t_spline_t;
-typedef spline_deriv_constraint_t::spline_t spline_t;
+typedef exact_cubic<Time, Numeric, 3, true, Point, T_Point> exact_cubic_t;
+typedef exact_cubic_t::spline_constraints spline_constraints_t;
+typedef exact_cubic_t::t_spline_t t_spline_t;
+typedef exact_cubic_t::spline_t spline_t;
/// \brief Compute time such that the equation from source to offsetpoint is necessarily a line.
Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset)
@@ -111,7 +110,7 @@ exact_cubic_t* effector_spline(
//specifying end velocity constraint such that landing will be in straight line
spline_t end_spline=make_end_spline(land_normal,landWaypoint.second,land_offset,landWaypoint.first,land_offset_duration);
spline_constraints_t constraints = compute_required_offset_velocity_acceleration(end_spline,land_offset_duration);
- spline_deriv_constraint_t all_but_end(waypoints.begin(), waypoints.end(),constraints);
+ exact_cubic_t all_but_end(waypoints.begin(), waypoints.end(),constraints);
t_spline_t splines = all_but_end.subSplines_;
splines.push_back(end_spline);
return new exact_cubic_t(splines);
diff --git a/include/curves/helpers/effector_spline_rotation.h b/include/curves/helpers/effector_spline_rotation.h
index 4e7430f..38e9266 100644
--- a/include/curves/helpers/effector_spline_rotation.h
+++ b/include/curves/helpers/effector_spline_rotation.h
@@ -37,7 +37,7 @@ typedef curve_abc<Time, Numeric, 4, false, quat_t> curve_abc_quat_t;
typedef std::pair<Numeric, quat_t > waypoint_quat_t;
typedef std::vector<waypoint_quat_t> t_waypoint_quat_t;
typedef Eigen::Matrix<Numeric, 1, 1> point_one_dim_t;
-typedef spline_deriv_constraint <Numeric, Numeric, 1, false, point_one_dim_t> spline_deriv_constraint_one_dim;
+typedef exact_cubic <Numeric, Numeric, 1, false, point_one_dim_t> exact_cubic_constraint_one_dim;
typedef std::pair<Numeric, point_one_dim_t > waypoint_one_dim_t;
typedef std::vector<waypoint_one_dim_t> t_waypoint_one_dim_t;
@@ -60,7 +60,7 @@ class rotation_spline: public curve_abc_quat_t
quat_from_ = from.quat_from_;
quat_to_ = from.quat_to_;
min_ =from.min_; max_ = from.max_;
- time_reparam_ = spline_deriv_constraint_one_dim(from.time_reparam_);
+ time_reparam_ = exact_cubic_constraint_one_dim(from.time_reparam_);
return *this;
}
/* Copy Constructors / operator=*/
@@ -81,12 +81,12 @@ class rotation_spline: public curve_abc_quat_t
}
/// \brief Initialize time reparametrization for spline.
- spline_deriv_constraint_one_dim computeWayPoints() const
+ exact_cubic_constraint_one_dim computeWayPoints() const
{
t_waypoint_one_dim_t waypoints;
waypoints.push_back(std::make_pair(0,point_one_dim_t::Zero()));
waypoints.push_back(std::make_pair(1,point_one_dim_t::Ones()));
- return spline_deriv_constraint_one_dim(waypoints.begin(), waypoints.end());
+ return exact_cubic_constraint_one_dim(waypoints.begin(), waypoints.end());
}
virtual time_t min() const{return min_;}
@@ -97,7 +97,7 @@ class rotation_spline: public curve_abc_quat_t
Eigen::Quaterniond quat_to_; //const
double min_; //const
double max_; //const
- spline_deriv_constraint_one_dim time_reparam_; //const
+ exact_cubic_constraint_one_dim time_reparam_; //const
};
diff --git a/include/curves/piecewise_curve.h b/include/curves/piecewise_curve.h
index 066df07..2d21931 100644
--- a/include/curves/piecewise_curve.h
+++ b/include/curves/piecewise_curve.h
@@ -1,10 +1,13 @@
#ifndef _CLASS_PIECEWISE_CURVE
#define _CLASS_PIECEWISE_CURVE
+#include <type_traits>
+
#include "curve_abc.h"
#include "cubic_hermite_spline.h"
#include "bezier_curve"
#include "polynomial.h"
+#include "curve_conversion.h"
namespace curves
@@ -14,12 +17,28 @@ namespace curves
///
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> > >
+ typename T_Point= std::vector<Point,Eigen::aligned_allocator<Point>,
+ typename Polynomial= polynomial <double, double, 3, true, point_t, t_point_t> > >
struct piecewise_curve
{
+ typedef Point point_t;
+ typedef T_Point t_point_t;
+ typedef Time time_t;
+ typedef Numeric num_t;
+
+ //typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t;
+ typedef std::vector < Polynomial > t_polynomial_t;
+ typedef std::vector<Time> t_vector_time_t;
+
+ public:
+ piecewise_curve
+ private:
+ t_polynomial_t polynomial_curves_; // for curves 0/1/2 : [ curve0, curve1, curve2 ]
+ t_vector_time_t time_polynomial_curves_; // for curves 0/1/2 : [ (Tmin0,Tmax0),(Tmin1,Tmax1),(Tmin2,Tmax2) ]
+ Numeric size_;
}
} // end namespace
diff --git a/include/curves/spline_deriv_constraint.h b/include/curves/spline_deriv_constraint.h
deleted file mode 100644
index 9051525..0000000
--- a/include/curves/spline_deriv_constraint.h
+++ /dev/null
@@ -1,148 +0,0 @@
-/**
-* \file exact_cubic.h
-* \brief class allowing to create an Exact cubic spline.
-* \author Steve T.
-* \version 0.1
-* \date 06/17/2013
-*
-* This file contains definitions for the ExactCubic class.
-* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique set of
-* cubic splines fulfulling those 4 restrictions :
-* - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
-* - x_i(t_i+1) = x_i+1* ;
-* - its derivative is continous at t_i+1
-* - its acceleration is continous at t_i+1
-* more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
-* By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
-*/
-
-
-#ifndef _CLASS_CUBICZEROVELACC
-#define _CLASS_CUBICZEROVELACC
-
-#include "exact_cubic.h"
-#include "curve_constraint.h"
-
-#include "MathDefs.h"
-
-#include <functional>
-#include <vector>
-
-namespace curves
-{
-/// \class spline_deriv_constraint.
-/// \brief Represents a set of cubic splines defining a continuous function
-/// crossing each of the waypoint given in its initialization. <br>
-/// Additional constraints +are used to increase the order of the last spline, to start and finish
-/// trajectory with user defined velocity and acceleration.
-///
-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> >,
- typename SplineBase=polynomial<Time, Numeric, Dim, Safe, Point, T_Point> >
-struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase>
-{
- 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 polynomial<time_t, Numeric, Dim, Safe, point_t, t_point_t> spline_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;
- typedef curve_constraints<point_t> spline_constraints;
-
- /* Constructors - destructors */
- public:
- /// \brief Constructor.
- /// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
- /// \param wayPointsEnd : an iterator pointing to the last element of a waypoint container.
- /// \param constraints : constraints on the init and end velocity / accelerations of the spline.
- ///
- template<typename In>
- spline_deriv_constraint(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints = spline_constraints())
- : exact_cubic_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}
-
- ///\brief Destructor
- virtual ~spline_deriv_constraint(){}
-
- ///\brief Copy Constructor
- spline_deriv_constraint(const spline_deriv_constraint& other)
- : exact_cubic_t(other.subSplines_) {}
-
- private:
- template<typename In>
- 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 / 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>
- (a0,b0,c0,d0,init_t, end_t));
- constraints.init_vel = subSplines.back().derivate(end_t, 1);
- constraints.init_acc = subSplines.back().derivate(end_t, 2);
- }
-
- template<typename In>
- 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 / 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 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 std::length_error("number of waypoints should be superior to one"); // 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;
- }
-
- private:
- /* Constructors - destructors */
-};
-} // namespace curves
-#endif //_CLASS_CUBICZEROVELACC
-
diff --git a/tests/Main.cpp b/tests/Main.cpp
index 2d908cf..de6f665 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -793,7 +793,7 @@ void EffectorSplineRotationWayPointRotationTest(bool& error)
void TestReparametrization(bool& error)
{
helpers::rotation_spline s;
- const helpers::spline_deriv_constraint_one_dim& sp = s.time_reparam_;
+ const helpers::exact_cubic_constraint_one_dim& sp = s.time_reparam_;
if(sp.min() != 0)
{
std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() << std::endl;
--
GitLab