19 #ifndef _CLASS_CUBICZEROVELACC 20 #define _CLASS_CUBICZEROVELACC 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> >
45 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic>
MatrixX;
46 typedef Eigen::Matrix<Numeric, 3, 3>
Matrix3;
62 template <
typename In>
65 : exact_cubic_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}
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;
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);
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;
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;
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;
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);
118 rhs = eq.inverse().eval() * rhs;
123 subSplines.push_back(create_quintic<Time, Numeric, Dim, Safe, Point, T_Point>(a0, b0, c0, d, e, f, init_t, end_t));
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;
131 t_spline_t subSplines;
132 subSplines.reserve(size - 1);
133 spline_constraints cons = constraints;
134 In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd - 1);
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);
145 #endif //_CLASS_CUBICZEROVELACC Definition: curve_constraint.h:21
t_spline_t::const_iterator cit_spline_t
Definition: spline_deriv_constraint.h:53
t_spline_t::iterator it_spline_t
Definition: spline_deriv_constraint.h:52
class allowing to create an Exact cubic spline.
Definition: bernstein.h:20
curve_constraints< point_t > spline_constraints
Definition: spline_deriv_constraint.h:54
Point point_t
Definition: spline_deriv_constraint.h:43
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: spline_deriv_constraint.h:45
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
point_t init_vel
Definition: curve_constraint.h:26
double Time
Definition: effector_spline.h:27
double Numeric
Definition: effector_spline.h:26
Definition: exact_cubic.h:40
point_t end_acc
Definition: curve_constraint.h:29
T_Point t_point_t
Definition: spline_deriv_constraint.h:44
polynom< time_t, Numeric, Dim, Safe, point_t, t_point_t > spline_t
Definition: spline_deriv_constraint.h:49
exact_cubic< time_t, Numeric, Dim, Safe, point_t, t_point_t > exact_cubic_t
Definition: spline_deriv_constraint.h:50
Numeric num_t
Definition: spline_deriv_constraint.h:48
point_t init_acc
Definition: curve_constraint.h:27
point_t end_vel
Definition: curve_constraint.h:28
Represents a set of cubic splines defining a continuous function crossing each of the waypoint given ...
Definition: spline_deriv_constraint.h:42
t_spline_t subSplines_
Definition: exact_cubic.h:188
std::vector< spline_t > t_spline_t
Definition: spline_deriv_constraint.h:51
spline_deriv_constraint(const spline_deriv_constraint &other)
Copy Constructor.
Definition: spline_deriv_constraint.h:71
Eigen::Matrix< Numeric, 3, 3 > Matrix3
Definition: spline_deriv_constraint.h:46
Represents a polynomf arbitrary order defined on the interval [tBegin, tEnd]. It follows the equation...
Definition: polynom.h:34
virtual ~spline_deriv_constraint()
Destructor.
Definition: spline_deriv_constraint.h:68
struct to define constraints on start / end velocities and acceleration on a curve ...
spline_deriv_constraint(In wayPointsBegin, In wayPointsEnd, const spline_constraints &constraints=spline_constraints())
Constructor.
Definition: spline_deriv_constraint.h:63
Time time_t
Definition: spline_deriv_constraint.h:47
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28