19 #ifndef _CLASS_EXACTCUBIC 20 #define _CLASS_EXACTCUBIC 37 template <
typename Time = double,
typename Numeric =
Time,
bool Safe =
false,
38 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
39 typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> >,
40 typename SplineBase = polynomial<Time, Numeric, Safe, Point, T_Point> >
45 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic>
MatrixX;
46 typedef Eigen::Matrix<Numeric, 3, 3>
Matrix3;
71 template <
typename In>
72 exact_cubic(In wayPointsBegin, In wayPointsEnd) : piecewise_curve_t() {
73 t_spline_t subSplines = computeWayPoints<In>(wayPointsBegin, wayPointsEnd);
74 for (cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
84 template <
typename In>
85 exact_cubic(In wayPointsBegin, In wayPointsEnd,
const spline_constraints& constraints) : piecewise_curve_t() {
86 t_spline_t subSplines = computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints);
87 for (cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
94 exact_cubic(
const t_spline_t& subSplines) : piecewise_curve_t() {
95 for (cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
100 exact_cubic(
const t_curve_ptr_t& subSplines) : piecewise_curve_t(subSplines) {}
116 bool isApprox(
const exact_cubic_t& other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
123 boost::shared_ptr<spline_t> s_ptr = boost::dynamic_pointer_cast<spline_t>(this->
curves_.at(index));
127 throw std::runtime_error(
128 "Parent piecewise curve do not contain only curves created from exact_cubic class methods");
133 static polynomial_t create_cubic(point_ref_t a,point_ref_t b, point_ref_t c, point_ref_t d,
134 const time_t t_min,
const time_t t_max){
140 return polynomial_t(coeffs.begin(), coeffs.end(), t_min, t_max);
142 static polynomial_t create_quintic(point_ref_t a,point_ref_t b, point_ref_t c, point_ref_t d,
143 point_ref_t e, point_ref_t f,
144 const time_t t_min,
const time_t t_max){
152 return polynomial_t(coeffs.begin(), coeffs.end(), t_min, t_max);
161 template <
typename In>
162 t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd)
const {
163 const std::size_t
dim = wayPointsBegin->second.size();
164 const std::size_t size = std::distance(wayPointsBegin, wayPointsEnd);
165 if (Safe && size < 1) {
166 throw std::length_error(
"size of waypoints must be superior to 0");
168 t_spline_t subSplines;
169 subSplines.reserve(size);
171 MatrixX h1 = MatrixX::Zero(size, size);
172 MatrixX h2 = MatrixX::Zero(size, size);
173 MatrixX h3 = MatrixX::Zero(size, size);
174 MatrixX h4 = MatrixX::Zero(size, size);
175 MatrixX h5 = MatrixX::Zero(size, size);
176 MatrixX h6 = MatrixX::Zero(size, size);
177 MatrixX a = MatrixX::Zero(size, dim);
178 MatrixX b = MatrixX::Zero(size, dim);
179 MatrixX c = MatrixX::Zero(size, dim);
180 MatrixX d = MatrixX::Zero(size, dim);
181 MatrixX x = MatrixX::Zero(size, dim);
182 In it(wayPointsBegin), next(wayPointsBegin);
185 for (std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i) {
186 num_t
const dTi((*next).first - (*it).first);
187 num_t
const dTi_sqr(dTi * dTi);
188 num_t
const dTi_cube(dTi_sqr * dTi);
190 h3(i, i) = -3 / dTi_sqr;
191 h3(i, i + 1) = 3 / dTi_sqr;
193 h4(i, i + 1) = -1 / dTi;
194 h5(i, i) = 2 / dTi_cube;
195 h5(i, i + 1) = -2 / dTi_cube;
196 h6(i, i) = 1 / dTi_sqr;
197 h6(i, i + 1) = 1 / dTi_sqr;
201 num_t
const dTi_1((*it2).first - (*next).first);
202 num_t
const dTi_1sqr(dTi_1 * dTi_1);
204 h1(i + 1, i) = 2 / dTi;
205 h1(i + 1, i + 1) = 4 / dTi + 4 / dTi_1;
206 h1(i + 1, i + 2) = 2 / dTi_1;
207 h2(i + 1, i) = -6 / dTi_sqr;
208 h2(i + 1, i + 1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
209 h2(i + 1, i + 2) = 6 / dTi_1sqr;
211 x.row(i) = (*it).second.transpose();
214 x.row(size - 1) = (*it).second.transpose();
222 it = wayPointsBegin, next = wayPointsBegin;
224 for (
int i = 0; next != wayPointsEnd; ++i, ++it, ++next) {
225 subSplines.push_back(create_cubic(a.row(i), b.row(i), c.row(i), d.row(i),
226 (*it).first, (*next).first));
231 template <
typename In>
232 t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd,
const spline_constraints& constraints)
const {
233 std::size_t
const size(std::distance(wayPointsBegin, wayPointsEnd));
234 if (Safe && size < 1) {
235 throw std::length_error(
"number of waypoints should be superior to one");
237 t_spline_t subSplines;
238 subSplines.reserve(size - 1);
239 spline_constraints cons = constraints;
240 In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd - 1);
242 for (std::size_t i(0); next != end; ++next, ++it, ++i) {
243 compute_one_spline<In>(it, next, cons, subSplines);
245 compute_end_spline<In>(it, next, cons, subSplines);
249 template <
typename In>
250 void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints,
251 t_spline_t& subSplines)
const {
252 const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
254 const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
255 const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
256 const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
257 subSplines.push_back(create_cubic(a0, b0, c0, d0, init_t, end_t));
258 constraints.
init_vel = subSplines.back().derivate(end_t, 1);
259 constraints.
init_acc = subSplines.back().derivate(end_t, 2);
262 template <
typename In>
263 void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints,
264 t_spline_t& subSplines)
const {
265 const std::size_t
dim = wayPointsBegin->second.size();
266 const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
269 const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
270 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;
272 const point_t alpha_0 = a1 - a0 - b0 * dt - c0 * dt_2;
273 const point_t alpha_1 = b1 - b0 - 2 * c0 * dt;
274 const point_t alpha_2 = c1 - 2 * c0;
275 const num_t x_d_0 = dt_3, x_d_1 = 3 * dt_2, x_d_2 = 6 * dt;
276 const num_t x_e_0 = dt_4, x_e_1 = 4 * dt_3, x_e_2 = 12 * dt_2;
277 const num_t x_f_0 = dt_5, x_f_1 = 5 * dt_4, x_f_2 = 20 * dt_3;
279 MatrixX rhs = MatrixX::Zero(3, dim);
280 rhs.row(0) = alpha_0;
281 rhs.row(1) = alpha_1;
282 rhs.row(2) = alpha_2;
283 Matrix3 eq = Matrix3::Zero(3, 3);
293 rhs = eq.inverse().eval() * rhs;
297 subSplines.push_back(create_quintic(a0, b0, c0, d, e, f, init_t, end_t));
302 friend class boost::serialization::access;
303 template <
class Archive>
304 void serialize(Archive& ar,
const unsigned int version) {
308 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(piecewise_curve_t);
313 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(
typename Time,
typename Numeric,
bool Safe,
typename Point,
314 typename T_Point,
typename SplineBase),
316 #endif //_CLASS_EXACTCUBIC exact_cubic(In wayPointsBegin, In wayPointsEnd)
Constructor.
Definition: exact_cubic.h:72
t_curve_ptr_t curves_
Definition: piecewise_curve.h:581
double Numeric
Definition: effector_spline.h:26
Eigen::Matrix< Numeric, 3, 3 > Matrix3
Definition: exact_cubic.h:46
point_t end_vel
Definition: curve_constraint.h:45
exact_cubic(const exact_cubic &other)
Copy Constructor.
Definition: exact_cubic.h:103
piecewise_curve< Time, Numeric, Safe, point_t > piecewise_curve_t
Definition: exact_cubic.h:57
exact_cubic()
Empty constructor. Add at least one curve to call other class functions.
Definition: exact_cubic.h:65
point_t init_acc
Definition: curve_constraint.h:43
T_Point t_point_t
Definition: polynomial.h:37
Definition of a cubic spline.
Definition: curve_constraint.h:21
bool isApprox(const piecewise_curve_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals. Only two curves of the same class can be ...
Definition: piecewise_curve.h:92
interface for a Curve of arbitrary dimension.
void add_curve(const Curve &curve)
Definition: piecewise_curve.h:142
t_spline_t::const_iterator cit_spline_t
Definition: exact_cubic.h:52
Definition: bernstein.h:20
curve_constraints< Point > spline_constraints
Definition: exact_cubic.h:53
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
Represents a polynomial of an arbitrary order defined on the interval . It follows the equation : ...
Definition: fwd.h:37
point_t init_vel
Definition: curve_constraint.h:42
exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints &constraints)
Constructor.
Definition: exact_cubic.h:85
point_t end_acc
Definition: curve_constraint.h:46
t_spline_t::iterator it_spline_t
Definition: exact_cubic.h:51
spline_t getSplineAt(std::size_t index)
Definition: exact_cubic.h:122
T_Point t_point_t
Definition: exact_cubic.h:44
std::size_t getNumberSplines()
Definition: exact_cubic.h:120
Definition: exact_cubic.h:41
virtual std::size_t dim() const
Get dimension of curve.
Definition: piecewise_curve.h:564
Numeric num_t
Definition: exact_cubic.h:48
void serialize(Archive &ar, const unsigned int version)
Definition: exact_cubic.h:304
std::vector< spline_t > t_spline_t
Definition: exact_cubic.h:50
polynomial< Time, Numeric, Safe, point_t > polynomial_t
Definition: exact_cubic.h:58
const Eigen::Ref< const point_t > point_ref_t
Definition: exact_cubic.h:43
exact_cubic< Time, Numeric, Safe, point_t, T_Point, SplineBase > exact_cubic_t
Definition: exact_cubic.h:55
Time time_t
Definition: exact_cubic.h:47
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: exact_cubic.h:45
std::size_t getNumberCurves()
Definition: piecewise_curve.h:576
virtual ~exact_cubic()
Destructor.
Definition: exact_cubic.h:106
exact_cubic(const t_spline_t &subSplines)
Constructor.
Definition: exact_cubic.h:94
exact_cubic(const t_curve_ptr_t &subSplines)
Definition: exact_cubic.h:100
curve_abc< Time, Numeric, Safe, point_t > curve_abc_t
Definition: exact_cubic.h:56
Point point_t
Definition: exact_cubic.h:42
struct to define constraints on start / end velocities and acceleration on a curve ...
bool isApprox(const exact_cubic_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals. Only two curves of the same class can be ...
Definition: exact_cubic.h:116
class allowing to create a piecewise curve.
void PseudoInverse(_Matrix_Type_ &pinvmat)
An inverse kinematics architecture enforcing an arbitrary number of strict priority levels (Reference...
Definition: MathDefs.h:26
double Time
Definition: effector_spline.h:27
piecewise_curve_t::t_curve_ptr_t t_curve_ptr_t
Definition: exact_cubic.h:59
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
std::vector< curve_ptr_t > t_curve_ptr_t
Definition: piecewise_curve.h:41
SplineBase spline_t
Definition: exact_cubic.h:49
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34