hpp-spline  4.10.0
template based classes for creating and manipulating spline and bezier curves. Comes with extra options specific to end-effector trajectories in robotics.
spline_deriv_constraint.h
Go to the documentation of this file.
1 
19 #ifndef _CLASS_CUBICZEROVELACC
20 #define _CLASS_CUBICZEROVELACC
21 
22 #include "exact_cubic.h"
23 #include "curve_constraint.h"
24 
25 #include "MathDefs.h"
26 
27 #include <functional>
28 #include <vector>
29 
30 namespace spline {
38 template <typename Time = double, typename Numeric = Time, std::size_t Dim = 3, bool Safe = false,
39  typename Point = Eigen::Matrix<Numeric, Dim, 1>,
40  typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> >,
41  typename SplineBase = polynom<Time, Numeric, Dim, Safe, Point, T_Point> >
42 struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase> {
43  typedef Point point_t;
44  typedef T_Point t_point_t;
45  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
46  typedef Eigen::Matrix<Numeric, 3, 3> Matrix3;
47  typedef Time time_t;
48  typedef Numeric num_t;
51  typedef typename std::vector<spline_t> t_spline_t;
52  typedef typename t_spline_t::iterator it_spline_t;
53  typedef typename t_spline_t::const_iterator cit_spline_t;
55 
56  /* Constructors - destructors */
57  public:
62  template <typename In>
63  spline_deriv_constraint(In wayPointsBegin, In wayPointsEnd,
64  const spline_constraints& constraints = spline_constraints())
65  : exact_cubic_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}
66 
69 
72 
73  private:
74  template <typename In>
75  void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints,
76  t_spline_t& subSplines) const {
77  const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
78  const point_t &b0 = constraints.init_vel, c0 = constraints.init_acc / 2.;
79  const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
80  const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
81  const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
82  subSplines.push_back(create_cubic<Time, Numeric, Dim, Safe, Point, T_Point>(a0, b0, c0, d0, init_t, end_t));
83  constraints.init_vel = subSplines.back().derivate(end_t, 1);
84  constraints.init_acc = subSplines.back().derivate(end_t, 2);
85  }
86 
87  template <typename In>
88  void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints,
89  t_spline_t& subSplines) const {
90  const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
91  const point_t &b0 = constraints.init_vel, b1 = constraints.end_vel, c0 = constraints.init_acc / 2.,
92  c1 = constraints.end_acc;
93  const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
94  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;
95  // solving a system of four linear eq with 4 unknows: d0, e0
96  const point_t alpha_0 = a1 - a0 - b0 * dt - c0 * dt_2;
97  const point_t alpha_1 = b1 - b0 - 2 * c0 * dt;
98  const point_t alpha_2 = c1 - 2 * c0;
99  const num_t x_d_0 = dt_3, x_d_1 = 3 * dt_2, x_d_2 = 6 * dt;
100  const num_t x_e_0 = dt_4, x_e_1 = 4 * dt_3, x_e_2 = 12 * dt_2;
101  const num_t x_f_0 = dt_5, x_f_1 = 5 * dt_4, x_f_2 = 20 * dt_3;
102 
103  point_t d, e, f;
104  MatrixX rhs = MatrixX::Zero(3, Dim);
105  rhs.row(0) = alpha_0;
106  rhs.row(1) = alpha_1;
107  rhs.row(2) = alpha_2;
108  Matrix3 eq = Matrix3::Zero(3, 3);
109  eq(0, 0) = x_d_0;
110  eq(0, 1) = x_e_0;
111  eq(0, 2) = x_f_0;
112  eq(1, 0) = x_d_1;
113  eq(1, 1) = x_e_1;
114  eq(1, 2) = x_f_1;
115  eq(2, 0) = x_d_2;
116  eq(2, 1) = x_e_2;
117  eq(2, 2) = x_f_2;
118  rhs = eq.inverse().eval() * rhs;
119  d = rhs.row(0);
120  e = rhs.row(1);
121  f = rhs.row(2);
122 
123  subSplines.push_back(create_quintic<Time, Numeric, Dim, Safe, Point, T_Point>(a0, b0, c0, d, e, f, init_t, end_t));
124  }
125 
126  private:
127  template <typename In>
128  t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) const {
129  std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
130  if (Safe && size < 1) throw; // TODO
131  t_spline_t subSplines;
132  subSplines.reserve(size - 1);
133  spline_constraints cons = constraints;
134  In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd - 1);
135  ++next;
136  for (std::size_t i(0); next != end; ++next, ++it, ++i) compute_one_spline<In>(it, next, cons, subSplines);
137  compute_end_spline<In>(it, next, cons, subSplines);
138  return subSplines;
139  }
140 
141  private:
142  /* Constructors - destructors */
143 };
144 } // namespace spline
145 #endif //_CLASS_CUBICZEROVELACC
spline::spline_deriv_constraint::exact_cubic_t
exact_cubic< time_t, Numeric, Dim, Safe, point_t, t_point_t > exact_cubic_t
Definition: spline_deriv_constraint.h:50
spline::spline_deriv_constraint::num_t
Numeric num_t
Definition: spline_deriv_constraint.h:48
spline::spline_deriv_constraint::cit_spline_t
t_spline_t::const_iterator cit_spline_t
Definition: spline_deriv_constraint.h:53
spline::spline_deriv_constraint::spline_t
polynom< time_t, Numeric, Dim, Safe, point_t, t_point_t > spline_t
Definition: spline_deriv_constraint.h:49
spline::spline_deriv_constraint::~spline_deriv_constraint
virtual ~spline_deriv_constraint()
Destructor.
Definition: spline_deriv_constraint.h:68
spline::exact_cubic< double, double, 3, false, Eigen::Matrix< double, 3, 1 >, std::vector< Eigen::Matrix< double, 3, 1 >, Eigen::aligned_allocator< Eigen::Matrix< double, 3, 1 > > >, polynom< double, double, 3, false, Eigen::Matrix< double, 3, 1 >, std::vector< Eigen::Matrix< double, 3, 1 >, Eigen::aligned_allocator< Eigen::Matrix< double, 3, 1 > > > > >::subSplines_
t_spline_t subSplines_
Definition: exact_cubic.h:188
spline::spline_deriv_constraint
Represents a set of cubic splines defining a continuous function crossing each of the waypoint given ...
Definition: spline_deriv_constraint.h:42
exact_cubic.h
class allowing to create an Exact cubic spline.
spline::spline_deriv_constraint::spline_deriv_constraint
spline_deriv_constraint(const spline_deriv_constraint &other)
Copy Constructor.
Definition: spline_deriv_constraint.h:71
spline::spline_deriv_constraint::spline_deriv_constraint
spline_deriv_constraint(In wayPointsBegin, In wayPointsEnd, const spline_constraints &constraints=spline_constraints())
Constructor.
Definition: spline_deriv_constraint.h:63
spline::helpers::Numeric
double Numeric
Definition: effector_spline.h:26
spline::helpers::Time
double Time
Definition: effector_spline.h:27
spline::spline_deriv_constraint::t_spline_t
std::vector< spline_t > t_spline_t
Definition: spline_deriv_constraint.h:51
spline::helpers::Point
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
spline::spline_deriv_constraint::t_point_t
T_Point t_point_t
Definition: spline_deriv_constraint.h:44
spline::polynom
Represents a polynomf arbitrary order defined on the interval [tBegin, tEnd]. It follows the equation...
Definition: polynom.h:34
MathDefs.h
spline::spline_deriv_constraint::MatrixX
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: spline_deriv_constraint.h:45
curve_constraint.h
struct to define constraints on start / end velocities and acceleration on a curve
spline::spline_deriv_constraint::spline_constraints
curve_constraints< point_t > spline_constraints
Definition: spline_deriv_constraint.h:54
spline::spline_deriv_constraint::point_t
Point point_t
Definition: spline_deriv_constraint.h:43
spline
Definition: bernstein.h:20
spline::spline_deriv_constraint::it_spline_t
t_spline_t::iterator it_spline_t
Definition: spline_deriv_constraint.h:52
spline::helpers::T_Point
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
spline::spline_deriv_constraint::time_t
Time time_t
Definition: spline_deriv_constraint.h:47
spline::spline_deriv_constraint::Matrix3
Eigen::Matrix< Numeric, 3, 3 > Matrix3
Definition: spline_deriv_constraint.h:46
spline::exact_cubic
Definition: exact_cubic.h:40
spline::curve_constraints
Definition: curve_constraint.h:21