#include "curve_abc.h"
#include "MathDefs.h"
#include <math.h>
#include <vector>
#include <stdexcept>
T_Point t_point_t
Definition: exact_cubic.h:42
T_Point make_cubic_vector(Point const &a, Point const &b, Point const &c, Point const &d)
Creates coefficient vector of a cubic spline defined on the interval [tBegin, tEnd].
Definition: cubic_spline.h:28
t_point_t::const_iterator cit_point_t
Definition: bezier_curve.h:37
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t.
Definition: bezier_curve.h:127
Definition: curve_constraint.h:21
double max_
Definition: effector_spline_rotation.h:93
Point point_t
Definition: polynom.h:35
Definition: bernstein.h:39
~effector_spline_rotation()
Definition: effector_spline_rotation.h:178
std::vector< Bern< Numeric > > bernstein_
Definition: bezier_curve.h:322
std::vector< spline_t > t_spline_t
Definition: spline_deriv_constraint.h:51
Definition: bernstein.h:20
virtual time_t max() const
Returns the maximum time for wich curve is defined.
Definition: effector_spline_rotation.h:87
polynom(const coeff_t &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynom.h:51
Mosek connection to produce optimized splines.
Definition: OptimizeSpline.h:28
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
std::pair< Numeric, quat_t > waypoint_quat_t
Definition: effector_spline_rotation.h:34
const Eigen::Quaterniond land_quat_
Definition: effector_spline_rotation.h:237
Eigen::Quaterniond quat_from_
Definition: effector_spline_rotation.h:90
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluation of the derivative spline at time t.
Definition: polynom.h:150
const exact_cubic_t * spline_
Definition: effector_spline_rotation.h:235
bezier_curve_t extract(const Numeric t1, const Numeric t2)
Definition: bezier_curve.h:275
time_t T_
Definition: bezier_curve.h:318
spline_deriv_constraint(const spline_deriv_constraint &other)
Copy Constructor.
Definition: spline_deriv_constraint.h:71
virtual time_t min() const
Returns the minimum time for wich curve is defined.
Definition: effector_spline_rotation.h:86
const Eigen::Quaterniond to_quat_
Definition: effector_spline_rotation.h:236
~bezier_curve()
Destructor.
Definition: bezier_curve.h:113
virtual ~exact_cubic()
Destructor.
Definition: exact_cubic.h:69
Time time_t
Definition: spline_deriv_constraint.h:47
virtual time_t min() const =0
Returns the minimum time for wich curve is defined.
double min_
Definition: effector_spline_rotation.h:92
curve_abc()
Constructor.
Definition: curve_abc.h:31
Eigen::Ref< coeff_t > coeff_t_ref
Definition: polynom.h:41
T_Point t_point_t
Definition: spline_deriv_constraint.h:44
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
std::size_t order_
Definition: polynom.h:183
Numeric num_t
Definition: bezier_curve.h:34
t_point_t deCasteljauReduction(const t_point_t &pts, const Numeric u) const
deCasteljauReduction compute the de Casteljau's reduction of the given list of points at time t
Definition: bezier_curve.h:239
spline_deriv_constraint_t::spline_t spline_t
Definition: effector_spline.h:36
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: OptimizeSpline.h:29
double Numeric
Definition: effector_spline.h:26
Represents a curve of dimension Dim is Safe is false, no verification is made on the evaluation of th...
Definition: curve_abc.h:24
Point point_t
Definition: curve_constraint.h:22
bezier_curve< Time, Numeric, Dim, Safe, Point > bezier_curve_t
Definition: bezier_curve.h:38
curve_constraints()
Definition: curve_constraint.h:23
Time time_t
Definition: OptimizeSpline.h:31
std::vector< waypoint_quat_t > t_waypoint_quat_t
Definition: effector_spline_rotation.h:35
std::vector< Waypoint > T_Waypoint
Definition: effector_spline.h:31
spline_deriv_constraint_t::spline_constraints spline_constraints_t
Definition: effector_spline.h:33
polynom< time_t, Numeric, Dim, Safe, point_t, t_point_t > spline_t
Definition: spline_deriv_constraint.h:49
SplineOptimizer()
Initializes optimizer environment.
Definition: OptimizeSpline.h:39
exact_cubic(In wayPointsBegin, In wayPointsEnd)
Constructor.
Definition: exact_cubic.h:58
Point point_t
Definition: OptimizeSpline.h:30
Represents a set of cubic splines defining a continuous function crossing each of the waypoint given ...
Definition: spline_deriv_constraint.h:42
spline_deriv_constraint< Numeric, Numeric, 1, false, point_one_dim_t > spline_deriv_constraint_one_dim
Definition: effector_spline_rotation.h:37
virtual time_t max() const =0
Returns the maximum time for wich curve is defined.
quat_t interpolate_quat(const Numeric t) const
Definition: effector_spline_rotation.h:202
virtual ~spline_deriv_constraint()
Destructor.
Definition: spline_deriv_constraint.h:68
t_spline_t::iterator it_spline_t
Definition: exact_cubic.h:48
struct to define constraints on start / end velocities and acceleration on a curve
virtual time_t max() const
Definition: bezier_curve.h:314
const double time_lift_offset_
Definition: effector_spline_rotation.h:238
Polynom from_bezier(const Bezier &curve)
Provides methods for converting a curve from a bernstein representation to a polynom representation.
Definition: bezier_polynom_conversion.h:34
point_t end_vel
Definition: curve_constraint.h:28
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: exact_cubic.h:43
polynom< Time, Numeric, Dim, Safe, Point, T_Point > create_quintic(Point const &a, Point const &b, Point const &c, Point const &d, Point const &e, Point const &f, const Time min, const Time max)
Definition: quintic_spline.h:41
std::vector< spline_t > t_spline_t
Definition: exact_cubic.h:47
std::pair< double, Point > Waypoint
Definition: effector_spline.h:30
exact_cubic(const t_spline_t &subSplines)
Constructor.
Definition: exact_cubic.h:63
bezier_curve_t compute_derivate(const std::size_t order) const
Computes the derivative curve at order N.
Definition: bezier_curve.h:139
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t.
Definition: exact_cubic.h:153
const typedef Eigen::Ref< const quat_t > quat_ref_const_t
Definition: effector_spline_rotation.h:31
point_t end_acc
Definition: curve_constraint.h:29
spline_deriv_constraint_t::t_spline_t t_spline_t
Definition: effector_spline.h:35
Eigen::Matrix< Numeric, 7, 1 > config_t
Definition: effector_spline_rotation.h:32
Eigen::Matrix< double, Dim, Eigen::Dynamic > coeff_t
Definition: polynom.h:40
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: spline_deriv_constraint.h:45
rotation_spline & operator=(const rotation_spline &from)
Definition: effector_spline_rotation.h:55
exact_cubic< Time, Numeric, 4, false, quat_t, std::vector< quat_t, Eigen::aligned_allocator< quat_t > >, rotation_spline > exact_cubic_quat_t
Definition: effector_spline_rotation.h:99
point_t init_vel
Definition: curve_constraint.h:26
Time time_t
Definition: bezier_curve.h:33
coeff_t coefficients_
Definition: polynom.h:181
Time time_t
Definition: curve_abc.h:26
exact_cubic_t * effector_spline(In wayPointsBegin, In wayPointsEnd, const Point &lift_normal=Eigen::Vector3d::UnitZ(), const Point &land_normal=Eigen::Vector3d::UnitZ(), const Numeric lift_offset=0.02, const Numeric land_offset=0.02, const Time lift_offset_duration=0.02, const Time land_offset_duration=0.02)
Helper method to create a spline typically used to guide the 3d trajectory of a robot end effector.
Definition: effector_spline.h:90
~polynom()
Destructor.
Definition: polynom.h:94
config_t operator()(const Numeric t) const
Evaluation of the effector position and rotation at time t.
Definition: effector_spline_rotation.h:194
~curve_constraints()
Definition: curve_constraint.h:25
Definition of a cubic spline.
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluates the derivative at order N of the curve.
Definition: bezier_curve.h:174
rotation_spline(quat_ref_const_t quat_from=quat_t(0, 0, 0, 1), quat_ref_const_t quat_to=quat_t(0, 0, 0, 1), const double min=0., const double max=1.)
Definition: effector_spline_rotation.h:43
virtual num_t min() const
Definition: exact_cubic.h:182
std::pair< time_t, time_t > timeRange()
Definition: curve_abc.h:58
exact_cubic(const exact_cubic &other)
Copy Constructor.
Definition: exact_cubic.h:66
SplineOptimizer< time_t, Numeric, Dim, Safe, Point > splineOptimizer_t
Definition: OptimizeSpline.h:34
bezier_curve_t compute_primitive(const std::size_t order) const
Computes the primitive of the curve at order N.
Definition: bezier_curve.h:152
point_t evalDeCasteljau(const Numeric t) const
evalDeCasteljau evaluate the curve value at time t using deCasteljau algorithm
Definition: bezier_curve.h:221
point_t evalHorner(const Numeric t) const
Evaluates all Bernstein polynomes for a certain degree using horner's scheme.
Definition: bezier_curve.h:197
Eigen::Matrix< Numeric, 1, 1 > point_one_dim_t
Definition: effector_spline_rotation.h:36
polynom(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
Constructor.
Definition: polynom.h:84
Numeric num_t
Definition: exact_cubic.h:45
spline_deriv_constraint_one_dim time_reparam_
Definition: effector_spline_rotation.h:94
t_spline_t subSplines_
Definition: exact_cubic.h:188
Numeric max() const
Definition: effector_spline_rotation.h:184
virtual ~curve_abc()
Destructor.
Definition: curve_abc.h:34
spline_t make_end_spline(const Point &normal, const Point &from, const Numeric offset, const Time init_time, const Time time_offset)
Definition: effector_spline.h:45
spline_deriv_constraint< Time, Numeric, 3, true, Point, T_Point > spline_deriv_constraint_t
Definition: effector_spline.h:32
std::pair< Numeric, point_one_dim_t > waypoint_one_dim_t
Definition: effector_spline_rotation.h:38
bezier_curve(In PointsBegin, In PointsEnd, const time_t T)
Constructor.
Definition: bezier_curve.h:63
std::vector< Bern< Numeric > > makeBernstein(const unsigned int n)
Computes all Bernstein polynomes for a certain degree.
Definition: bernstein.h:58
T_Point make_quintic_vector(Point const &a, Point const &b, Point const &c, Point const &d, Point const &e, Point const &f)
Creates coefficient vector of a quintic spline defined on the interval [tBegin, tEnd].
Definition: quintic_spline.h:28
Definition: exact_cubic.h:40
double Time
Definition: effector_spline.h:27
polynom(const polynom &other)
Definition: polynom.h:98
Numeric i_
Definition: bernstein.h:50
Eigen::Matrix< Numeric, 4, 1 > quat_t
Definition: effector_spline_rotation.h:29
void PseudoInverse(_Matrix_Type_ &pinvmat)
Definition: MathDefs.h:28
std::size_t dim_
Definition: polynom.h:182
const exact_cubic_quat_t quat_spline_
Definition: effector_spline_rotation.h:240
interface for a Curve of arbitrary dimension.
Numeric bin_m_i_
Definition: bernstein.h:51
polynom(const T_Point &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynom.h:67
T_Point t_point_t
Definition: polynom.h:36
Numeric num_t
Definition: polynom.h:38
static bezier_curve_t zero(const time_t T=1.)
Definition: bezier_curve.h:328
Bern(const unsigned int m, const unsigned int i)
Definition: bernstein.h:40
curve_abc< Time, Numeric, Dim, Safe, Point > curve_abc_t
Definition: exact_cubic.h:50
virtual quat_t derivate(time_t, std::size_t) const
Definition: effector_spline_rotation.h:74
const t_point_t & waypoints() const
Definition: bezier_curve.h:214
Represents a trajectory for and end effector uses the method effector_spline to create a spline traje...
Definition: effector_spline_rotation.h:108
~SplineOptimizer()
Destructor.
Definition: OptimizeSpline.h:45
t_spline_t::const_iterator cit_spline_t
Definition: exact_cubic.h:49
unsigned int fact(const unsigned int n)
Computes factorial of a number.
Definition: bernstein.h:24
Point point_t
Definition: curve_abc.h:25
time_t mult_T_
Definition: bezier_curve.h:319
effector_spline_rotation(In wayPointsBegin, In wayPointsEnd, InQuat quatWayPointsBegin, InQuat quatWayPointsEnd, const Point &lift_normal=Eigen::Vector3d::UnitZ(), const Point &land_normal=Eigen::Vector3d::UnitZ(), const Numeric lift_offset=0.02, const Numeric land_offset=0.02, const Time lift_offset_duration=0.02, const Time land_offset_duration=0.02)
Constructor.
Definition: effector_spline_rotation.h:163
Definition of a cubic spline.
t_spline_t::iterator it_spline_t
Definition: spline_deriv_constraint.h:52
spline_deriv_constraint_t::exact_cubic_t exact_cubic_t
Definition: effector_spline.h:34
virtual point_t derivate(const time_t t, const std::size_t order) const =0
Evaluation of the derivative spline at time t.
virtual point_t operator()(const time_t t) const =0
Evaluation of the cubic spline at time t.
class allowing to create an Exact cubic spline.
point_t evalBernstein(const Numeric t) const
Evaluates all Bernstein polynomes for a certain degree Warning: the horner scheme is about 100 times ...
Definition: bezier_curve.h:184
t_point_t deCasteljauReduction(const Numeric t) const
Definition: bezier_curve.h:231
Definition: bezier_curve.h:31
unsigned int bin(const unsigned int n, const unsigned int k)
Computes a binomal coefficient.
Definition: bernstein.h:33
std::pair< bezier_curve_t, bezier_curve_t > split(const Numeric t)
split split the curve in 2 at time t
Definition: bezier_curve.h:255
virtual num_t max() const
Returns the maximum time for wich curve is defined.
Definition: polynom.h:176
bezier_curve(In PointsBegin, In PointsEnd, const time_t T, const time_t mult_T)
Constructor.
Definition: bezier_curve.h:80
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t &end_spline, const Time)
Definition: effector_spline.h:65
spline_deriv_constraint_one_dim computeWayPoints() const
Definition: effector_spline_rotation.h:78
virtual num_t min() const
Returns the minimum time for wich curve is defined.
Definition: polynom.h:174
curve_constraints< point_t > curve_constraints_t
Definition: bezier_curve.h:35
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluation of the derivative spline at time t.
Definition: exact_cubic.h:168
virtual num_t max() const
Definition: exact_cubic.h:183
~Bern()
Definition: bernstein.h:42
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:36
polynom< Time, Numeric, Dim, Safe, Point, T_Point > create_cubic(Point const &a, Point const &b, Point const &c, Point const &d, const Time min, const Time max)
Definition: cubic_spline.h:38
quat_t operator()(const Numeric t) const
Definition: effector_spline_rotation.h:65
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t using horner's scheme.
Definition: polynom.h:136
~rotation_spline()
Definition: effector_spline_rotation.h:52
Eigen::Quaterniond quat_to_
Definition: effector_spline_rotation.h:91
Numeric num_t
Definition: OptimizeSpline.h:32
Point point_t
Definition: bezier_curve.h:32
std::size_t size_
Definition: bezier_curve.h:320
effector_spline_rotation(In wayPointsBegin, In wayPointsEnd, quat_ref_const_t &to_quat=quat_t(0, 0, 0, 1), quat_ref_const_t &land_quat=quat_t(0, 0, 0, 1), const Point &lift_normal=Eigen::Vector3d::UnitZ(), const Point &land_normal=Eigen::Vector3d::UnitZ(), const Numeric lift_offset=0.02, const Numeric land_offset=0.02, const Time lift_offset_duration=0.02, const Time land_offset_duration=0.02)
Constructor.
Definition: effector_spline_rotation.h:128
exact_cubic_t * GenerateOptimizedCurve(In wayPointsBegin, In wayPointsEnd) const
Starts an optimization loop to create curve.
SplineBase spline_t
Definition: exact_cubic.h:46
curve_abc< Time, Numeric, Dim, Safe, Point > curve_abc_t
Definition: polynom.h:39
Point point_t
Definition: spline_deriv_constraint.h:43
Numeric m_minus_i
Definition: bernstein.h:49
spline_deriv_constraint(In wayPointsBegin, In wayPointsEnd, const spline_constraints &constraints=spline_constraints())
Constructor.
Definition: spline_deriv_constraint.h:63
point_t init_acc
Definition: curve_constraint.h:27
Time time_t
Definition: exact_cubic.h:44
Time time_t
Definition: polynom.h:37
const double time_land_offset_
Definition: effector_spline_rotation.h:239
exact_cubic< time_t, Numeric, Dim, Safe, Point > exact_cubic_t
Definition: OptimizeSpline.h:33
exact_cubic< time_t, Numeric, Dim, Safe, point_t, t_point_t > exact_cubic_t
Definition: spline_deriv_constraint.h:50
Point point_t
Definition: exact_cubic.h:41
std::size_t degree_
Definition: bezier_curve.h:321
bezier_curve(In PointsBegin, In PointsEnd)
Constructor.
Definition: bezier_curve.h:46
virtual time_t min() const
Definition: bezier_curve.h:313
Represents a polynomf arbitrary order defined on the interval [tBegin, tEnd]. It follows the equation...
Definition: polynom.h:34
t_spline_t::const_iterator cit_spline_t
Definition: spline_deriv_constraint.h:53
Numeric min() const
Definition: effector_spline_rotation.h:183
curve_constraints< point_t > spline_constraints
Definition: spline_deriv_constraint.h:54
Definition: effector_spline_rotation.h:41
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t &constraints, const time_t T=1.)
Constructor This constructor will add 4 points (2 after the first one, 2 before the last one) to ensu...
Definition: bezier_curve.h:100
Numeric num_t
Definition: spline_deriv_constraint.h:48
curve_abc< Time, Numeric, 4, false, quat_t > curve_abc_quat_t
Definition: effector_spline_rotation.h:33
Eigen::Ref< quat_t > quat_ref_t
Definition: effector_spline_rotation.h:30
Numeric operator()(const Numeric u) const
Definition: bernstein.h:44
Eigen::Matrix< Numeric, 3, 3 > Matrix3
Definition: spline_deriv_constraint.h:46
Waypoint compute_offset(const Waypoint &source, const Point &normal, const Numeric offset, const Time time_offset)
Definition: effector_spline.h:38
std::vector< waypoint_one_dim_t > t_waypoint_one_dim_t
Definition: effector_spline_rotation.h:39