#include "curve_abc.h"
#include "MathDefs.h"
#include <math.h>
|
template<typename Numeric > |
brief Computes all Bernstein polynomes for a certain degree std::vector< Bern< Numeric > > | makeBernstein (const unsigned int n) |
|
◆ makeBernstein()
template<typename Numeric >
brief Computes all Bernstein polynomes for a certain degree std::vector<Bern<Numeric> > makeBernstein |
( |
const unsigned int |
n | ) |
|
@ END_JERK
Definition: definitions.h:28
std::vector< var_t > variables_
Definition: details.h:31
curve_abc< Time, Numeric, Safe, point_t > curve_abc_t
Definition: cubic_hermite_spline.h:41
@ INIT_POS
Definition: definitions.h:21
Eigen::Quaternion< Scalar > quaternion_t
Definition: so3_linear.h:24
BezierFixed evaluateLinear(const BezierLinear &bIn, const X x)
Definition: linear_variable.h:143
curve_abc< double, double, true, transform_t, point6_t > curve_SE3_t
Definition: fwd.h:72
virtual bool operator==(const SO3Linear_t &other) const
Definition: so3_linear.h:142
Eigen::VectorXd time_waypoints_t
Definition: python_definitions.h:16
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: so3_linear.h:133
point_t evalBernstein(const Numeric t) const
Evaluate all Bernstein polynomes for a certain degree.
Definition: bezier_curve.h:245
Eigen::MatrixXd coeff() const
Definition: polynomial.h:322
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > ineqMatrix
Definition: definitions.h:35
Eigen::Matrix< double, Eigen::Dynamic, 1, 0, Eigen::Dynamic, 1 > ret_pointX_t
Definition: python_definitions.h:18
@ END_VEL
Definition: definitions.h:26
static void evalCoeffs(const Numeric t, Numeric &h00, Numeric &h10, Numeric &h01, Numeric &h11, std::size_t degree_derivative)
Evaluate coefficient for polynom of cubic hermite spline.
Definition: cubic_hermite_spline.h:263
curve_ptr_t curve_at_index(const std::size_t idx) const
Definition: piecewise_curve.h:217
@ INIT_VEL
Definition: definitions.h:22
linear_variable_t & operator-=(const linear_variable_t &w1)
Definition: linear_variable.h:54
bool isApprox(const linear_variable_t &other, const double prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: linear_variable.h:84
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:42
virtual quat_t derivate(time_t, std::size_t) const
Definition: effector_spline_rotation.h:103
PointList vectorToEigenArray(const T_point &vect)
Definition: python_definitions.h:51
quadratic_variable< Numeric > compute_integral_cost_internal(const problem_data< Point, Numeric > &pData, const std::size_t num_derivate)
Definition: integral_cost.h:30
virtual std::size_t degree() const
Get the degree of the curve.
Definition: effector_spline_rotation.h:127
double Time
Definition: effector_spline.h:27
bezier_t * bezier
Definition: details.h:36
virtual time_t min() const =0
Get the minimum time for which the curve is defined.
quadratic_variable< Numeric > quadratic_variable_t
Definition: quadratic_variable.h:28
std::size_t dim_
Definition: polynomial.h:375
Time time_t
Definition: piecewise_curve.h:34
double max_
Definition: effector_spline_rotation.h:134
exact_cubic< double, double, true, pointX_t, t_pointX_t, polynomial_t > exact_cubic_t
Definition: fwd.h:82
SO3Linear()
Empty constructor.
Definition: so3_linear.h:33
curve_abc_quat_t * compute_derivate_ptr(const std::size_t) const
Compute the derived curve at order N.
Definition: effector_spline_rotation.h:110
Numeric Scalar
Definition: so3_linear.h:21
@ VELOCITY
Definition: integral_cost.h:22
exact_cubic_constraint_one_dim computeWayPoints() const
Initialize time reparametrization for spline.
Definition: effector_spline_rotation.h:115
Time time_t
Definition: OptimizeSpline.h:31
friend class boost::serialization::access
Definition: se3_curve.h:229
boost::shared_ptr< curve_t > curve_ptr_t
Definition: curve_abc.h:40
virtual std::size_t dim() const
Get dimension of curve.
Definition: se3_curve.h:205
std::size_t degree
Definition: definitions.h:75
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
cubic_hermite_spline< Time, Numeric, Safe, point_t > cubic_hermite_spline_t
Definition: cubic_hermite_spline.h:42
t_spline_t::iterator it_spline_t
Definition: exact_cubic.h:51
virtual ~curve_abc()
Destructor.
Definition: curve_abc.h:48
constraint_flag operator~(constraint_flag a)
Definition: details.h:312
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > vector_x_t
Definition: linear_variable.h:27
SE3Curve(const pointX_t &init_pos, const pointX_t &end_pos, const matrix3_t &init_rot, const matrix3_t &end_rot, const time_t &t_min, const time_t &t_max)
Constructor from init/end pose, with rotation matrix.
Definition: se3_curve.h:82
std::size_t dim_
Definition: effector_spline_rotation.h:132
exact_cubic< Time, Numeric, Safe, Point, T_Point, SplineBase > exact_cubic_t
Definition: exact_cubic.h:55
quadratic_variable & operator*=(const double d)
Definition: quadratic_variable.h:94
virtual Time min() const
Get the minimum time for which the curve is defined.
Definition: cubic_hermite_spline.h:372
quat_t operator()(const Numeric t) const
Definition: effector_spline_rotation.h:66
curve_abc< Time, Numeric, Safe, point_t > curve_abc_t
Definition: exact_cubic.h:56
~polynomial()
Destructor.
Definition: polynomial.h:214
SO3Linear(const matrix3_t &init_rot, const matrix3_t &end_rot)
constructor with initial and final rotation expressed as rotation matrix, time bounds are set to [0;1...
Definition: so3_linear.h:72
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.
Definition: exact_cubic.h:115
Numeric num_t
Definition: curve_abc.h:38
polynomial< Time, Numeric, Safe, Point, T_Point > polynomial_t
Definition: polynomial.h:43
SE3Curve< Time, Numeric, Safe > SE3Curve_t
Definition: se3_curve.h:39
void add_curve_ptr(const curve_ptr_t &cf)
Add a new curve to piecewise curve, which should be defined in where is equal to of the actual pie...
Definition: piecewise_curve.h:148
constraint_flag operator|(constraint_flag a, constraint_flag b)
Definition: details.h:316
virtual bool operator!=(const rotation_spline &other) const
Definition: effector_spline_rotation.h:101
matrix3_t getEndRotation()
Definition: so3_linear.h:189
Eigen::Matrix< double, 3, 3 > matrix3_t
Definition: fwd.h:59
time_t T_max_
Definition: so3_linear.h:197
Definition: exact_cubic.h:42
std::vector< var_t > T_var_t
Definition: details.h:28
exact_cubic(const t_spline_t &subSplines)
Constructor.
Definition: exact_cubic.h:93
virtual ~cubic_hermite_spline()
Destructor.
Definition: cubic_hermite_spline.h:85
bool isApprox(const rotation_spline &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals.
Definition: effector_spline_rotation.h:83
Numeric max() const
Definition: effector_spline_rotation.h:225
@ JERK
Definition: integral_cost.h:24
point3_t angular_vel_
Definition: so3_linear.h:196
quat_t interpolate_quat(const Numeric t) const
Definition: effector_spline_rotation.h:241
Eigen::MatrixXd coeff_t
Definition: polynomial.h:41
std::vector< waypoint_one_dim_t > t_waypoint_one_dim_t
Definition: effector_spline_rotation.h:38
time_t max() const
Get the maximum time for which the curve is defined.
Definition: so3_linear.h:182
bezier_curve< Numeric, Numeric, true, linear_variable< Numeric > > bezier_t
Definition: details.h:29
transform_t point_t
Definition: se3_curve.h:26
exact_cubic()
Empty constructor.
Definition: exact_cubic.h:64
curve_ptr_t curve_at_time(const time_t t) const
Definition: piecewise_curve.h:215
std::vector< real > t_time_t
Definition: python_definitions.h:15
quadratic_variable & operator-=(const quadratic_variable &w1)
Definition: quadratic_variable.h:70
Eigen::Quaterniond quat_to_
Definition: effector_spline_rotation.h:131
piecewise_curve< double, double, true, point3_t, point3_t, curve_3_t > piecewise3_t
Definition: fwd.h:92
piecewise_curve_t * compute_derivate_ptr(const std::size_t order) const
compute_derivate return a piecewise_curve which is the derivative of this at given order
Definition: piecewise_curve.h:127
T_vector_x_t::const_iterator CIT_vector_x_t
Definition: definitions.h:50
Hermite hermite_from_curve(const typename Hermite::curve_abc_t &curve)
Converts a polynomial of order 3 or less/cubic bezier curve to a cubic hermite spline.
Definition: curve_conversion.h:70
t_point_t::const_iterator cit_point_t
Definition: bezier_curve.h:40
Definition: definitions.h:41
static piecewise_curve_t convert_discrete_points_to_polynomial(t_point_t points, t_point_derivate_t points_derivative, t_time_t time_points)
Definition: piecewise_curve.h:294
const point_t & b() const
Definition: quadratic_variable.h:110
Eigen::Matrix< num_t, Eigen::Dynamic, 1 > vector_x_t
Definition: definitions.h:45
Eigen::Matrix< Numeric, 4, 1 > quat_t
Definition: effector_spline_rotation.h:28
point_t end_jerk
Definition: curve_constraint.h:47
@ INIT_JERK
Definition: definitions.h:24
@ ALL
Definition: definitions.h:29
polynomial< double, double, true, pointX_t, t_pointX_t > polynomial_t
Definition: fwd.h:81
virtual point_derivate_t derivate(const Time t, const std::size_t order) const
Evaluate the derivative of order N of curve at time t.
Definition: piecewise_curve.h:114
cubic_hermite_spline< double, double, true, pointX_t > cubic_hermite_spline_t
Definition: fwd.h:84
curve_ptr_t translation_curve_
Definition: se3_curve.h:223
bezier_curve_t compute_primitive(const std::size_t order) const
Compute the primitive of the curve at order N.
Definition: bezier_curve.h:209
SE3Curve(curve_ptr_t translation_curve, const Quaternion &init_rot, const Quaternion &end_rot)
Constructor from curve for the translation and init/end rotation, with quaternion.
Definition: se3_curve.h:96
quadratic_variable & operator+=(const quadratic_variable &w1)
Definition: quadratic_variable.h:56
Eigen::Matrix< double, 4, 4 > matrix4_t
Definition: fwd.h:60
polynomial< double, double, true, point3_t, t_point3_t > polynomial3_t
Definition: fwd.h:88
linear_variable< Numeric > linear_variable_t
Definition: linear_variable.h:29
T_matrix_x_t::const_iterator CIT_matrix_x_t
Definition: definitions.h:49
boost::shared_ptr< bezier_curve_t > bezier_curve_ptr_t
Definition: bezier_curve.h:42
T_Point t_point_t
Definition: polynomial.h:37
@ FOURTH
Definition: integral_cost.h:25
piecewise_curve< Time, Numeric, Safe, point_t, point_t, bezier_curve_t > piecewise_curve_t
Definition: bezier_curve.h:43
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > vector_x_t
Definition: bezier_curve.h:34
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
point_t init_jerk
Definition: curve_constraint.h:44
problem_data(const problem_data &other)
Definition: details.h:39
problem_definition(const curve_constraints_t &parent)
Definition: definitions.h:62
void initInequalityMatrix(const problem_definition< Point, Numeric > &pDef, problem_data< Point, Numeric > &pData, quadratic_problem< Point, Numeric > &prob)
Definition: details.h:231
virtual bool operator!=(const piecewise_curve_t &other) const
Definition: piecewise_curve.h:107
Point point_t
Definition: cubic_hermite_spline.h:36
Bezier bezier_from_curve(const typename Bezier::curve_abc_t &curve)
Converts a cubic hermite spline or polynomial of order 3 or less to a cubic bezier curve.
Definition: curve_conversion.h:38
bool isApprox(const cubic_hermite_spline_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals.
Definition: cubic_hermite_spline.h:113
bool is_continuous(const std::size_t order)
Check if the curve is continuous of order given.
Definition: piecewise_curve.h:181
Represents a trajectory for and end effector. uses the method effector_spline to create a spline traj...
Definition: effector_spline_rotation.h:150
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluation of the derivative of order N of spline at time t.
Definition: polynomial.h:290
Numeric num_t
Definition: piecewise_curve.h:35
virtual std::size_t dim() const
Get dimension of curve.
Definition: polynomial.h:362
Represents a polynomial of an arbitrary order defined on the interval . It follows the equation : ...
Definition: fwd.h:34
linear_variable(const vector_x_t &c)
Definition: linear_variable.h:32
quadratic_variable()
Definition: quadratic_variable.h:30
matrix3_t point_t
Definition: so3_linear.h:22
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > matrix_x_t
Definition: quadratic_variable.h:26
struct to define constraints on start / end velocities and acceleration on a curve
constraint_flag & operator&=(constraint_flag &a, constraint_flag b)
Definition: details.h:332
constraint_flag flag
Definition: definitions.h:72
bezier_curve(const bezier_curve &other)
Definition: bezier_curve.h:114
~effector_spline_rotation()
Definition: effector_spline_rotation.h:220
curve_abc< Time, Numeric, Safe, point_t > curve_abc_t
Definition: bezier_curve.h:44
Point_derivate point_derivate_t
Definition: curve_abc.h:36
point3_t computeAngularVelocity(const matrix3_t &init_rot, const matrix3_t &end_rot, const double t_min, const double t_max)
Definition: so3_linear.h:96
std::size_t degree_
Degree (Cubic so degree 3)
Definition: cubic_hermite_spline.h:402
Definition of a cubic spline.
SE3Curve_t compute_derivate(const std::size_t) const
Definition: se3_curve.h:193
config_t operator()(const Numeric t) const
Evaluation of the effector position and rotation at time t.
Definition: effector_spline_rotation.h:234
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: se3_curve.h:166
T_vector_x_t inequalityVectors_
Definition: definitions.h:79
@ DISTANCE
Definition: integral_cost.h:21
virtual std::size_t dim() const
Get dimension of curve.
Definition: effector_spline_rotation.h:122
matrix3_t getInitRotation()
Definition: so3_linear.h:188
virtual bool operator==(const rotation_spline &other) const
Definition: effector_spline_rotation.h:99
const std::size_t dim_
Definition: definitions.h:80
time_t T_min_
Definition: se3_curve.h:225
std::size_t dim_
Dim of curve.
Definition: cubic_hermite_spline.h:383
virtual ~piecewise_curve()
Definition: piecewise_curve.h:68
Mosek connection to produce optimized splines.
Definition: OptimizeSpline.h:28
piecewise_curve(const curve_ptr_t &cf)
Constructor.
Definition: piecewise_curve.h:52
num_t totalTime
Definition: definitions.h:76
std::vector< matrix_x_t, Eigen::aligned_allocator< matrix_x_t > > T_matrix_x_t
Definition: definitions.h:47
curve_abc< double, double, true, matrix3_t, point3_t > curve_rotation_t
Definition: fwd.h:70
curve_abc< Time, Numeric, false, quat_t > curve_abc_quat_t
Definition: effector_spline_rotation.h:32
bool isApprox(const SE3Curve_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals.
Definition: se3_curve.h:159
void serialize(Archive &ar, const unsigned int version)
Definition: cubic_hermite_spline.h:409
std::vector< bezier_curve< Numeric, Numeric, true, linear_variable< Numeric > > > split(const problem_definition< Point, Numeric > &pDef, problem_data< Point, Numeric > &pData)
Definition: details.h:208
piecewise_curve_t convert_piecewise_curve_to_bezier()
Definition: piecewise_curve.h:226
virtual bool isApprox(const curve_abc_quat_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: effector_spline_rotation.h:90
double Numeric
Definition: effector_spline.h:26
virtual curve_t * compute_derivate_ptr(const std::size_t order) const =0
Compute the derived curve at order N.
virtual bool operator!=(const bezier_curve_t &other) const
Definition: bezier_curve.h:175
rotation_spline & operator=(const rotation_spline &from)
Definition: effector_spline_rotation.h:55
Point point_t
Definition: definitions.h:42
bezier_curve< double, double, true, point3_t > bezier3_t
Definition: fwd.h:90
piecewise_curve_t convert_piecewise_curve_to_polynomial()
Definition: piecewise_curve.h:256
problem_data(const std::size_t dim)
Definition: details.h:22
exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints &constraints)
Constructor.
Definition: exact_cubic.h:84
const exact_cubic_quat_t quat_spline_
Definition: effector_spline_rotation.h:280
curve_constraints< point_t > curve_constraints_t
Definition: definitions.h:44
void save(Archive &ar, const unsigned int version) const
Definition: so3_linear.h:221
curve_abc< Time, Numeric, Safe, Point > curve_abc_t
Definition: polynomial.h:40
const vector_x_t & c() const
Definition: linear_variable.h:90
exact_cubic(In wayPointsBegin, In wayPointsEnd)
Constructor.
Definition: exact_cubic.h:71
vector_x_t splitTimes_
Definition: definitions.h:77
curve_abc< Time, Numeric, Safe, point_t, point_derivate_t > curve_t
Definition: curve_abc.h:39
@ NONE
Definition: definitions.h:30
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:39
std::vector< pair_pointX_tangent_t, Eigen::aligned_allocator< pair_pointX_tangent_t > > t_pair_pointX_tangent_t
Definition: python_definitions.h:21
Point point_t
Definition: curve_abc.h:35
static linear_variable_t Zero(size_t dim=0)
Definition: linear_variable.h:76
boost::shared_ptr< curve_t > curve_ptr_t
Definition: piecewise_curve.h:38
Definition: cubic_hermite_spline.h:35
problem_definition(const std::size_t dim)
Definition: definitions.h:52
const matrix_x_t & B() const
Definition: linear_variable.h:89
curve_constraints< Point > spline_constraints
Definition: exact_cubic.h:53
SO3Linear(const quaternion_t &init_rot, const quaternion_t &end_rot, const time_t t_min, const time_t t_max)
constructor with initial and final rotation and time bounds
Definition: so3_linear.h:36
void serialize(Archive &ar, const unsigned int version)
Definition: se3_curve.h:232
SE3Curve_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: se3_curve.h:200
constraint_flag & operator|=(constraint_flag &a, constraint_flag b)
Definition: details.h:328
virtual std::size_t dim() const
Get dimension of curve.
Definition: cubic_hermite_spline.h:369
Time time_t
Definition: bezier_curve.h:36
bezier_curve_t compute_derivate(const std::size_t order) const
Compute the derived curve at order N.
Definition: bezier_curve.h:181
virtual bool operator==(const polynomial_t &other) const
Definition: polynomial.h:282
boost::shared_ptr< curve_3_t > curve3_ptr_t
Definition: fwd.h:76
Eigen::Matrix< double, 6, 1 > point6_t
Definition: fwd.h:57
polynomial(const Point &init, const Point &d_init, const Point &dd_init, const Point &end, const Point &d_end, const Point &dd_end, const time_t min, const time_t max)
Constructor from boundary condition with C2 : create a polynomial that connect exactly init and end a...
Definition: polynomial.h:172
std::size_t numControlPoints
Definition: details.h:33
SO3Linear(const quaternion_t &init_rot, const quaternion_t &end_rot)
constructor with initial and final rotation, time bounds are set to [0;1]
Definition: so3_linear.h:60
Definition: effector_spline_rotation.h:40
linear_variable_t & operator*=(const double d)
Definition: linear_variable.h:70
Definition: bernstein.h:20
void load(Archive &ar, const unsigned int version)
Definition: so3_linear.h:204
std::size_t dim_
Definition: so3_linear.h:194
curve_abc< double, double, true, pointX_t, pointX_t > curve_abc_t
Definition: fwd.h:67
point3_t point_derivate_t
Definition: so3_linear.h:23
boost::shared_ptr< curve_SE3_t > curve_SE3_ptr_t
Definition: fwd.h:78
~problem_data()
Definition: details.h:23
virtual time_t min() const
Get the minimum time for which the curve is defined.
Definition: effector_spline_rotation.h:123
virtual Time max() const
Get the maximum time for which the curve is defined.
Definition: piecewise_curve.h:408
bool isApprox(const T a, const T b, const T eps=1e-6)
Definition: curve_abc.h:25
bool isApprox(const bezier_curve_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals.
Definition: bezier_curve.h:153
SE3Curve()
Empty constructor.
Definition: se3_curve.h:45
Eigen::Vector3d point3_t
Definition: fwd.h:52
bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
Constructor.
Definition: bezier_curve.h:61
void add_curve(const Curve &curve)
Definition: piecewise_curve.h:137
curve_rotation_ptr_t rotation_curve_
Definition: se3_curve.h:224
const Numeric c() const
Definition: quadratic_variable.h:116
Point evalCubicHermiteSpline(const Numeric t, std::size_t degree_derivative) const
Evaluate value of cubic hermite spline or its derivate at specified order at time .
Definition: cubic_hermite_spline.h:211
vector_time_t getTime()
Get vector of Time corresponding to Time for each control point.
Definition: cubic_hermite_spline.h:187
Definition of a cubic spline.
virtual std::size_t dim() const
Get dimension of curve.
Definition: piecewise_curve.h:402
Eigen::Matrix< N, Eigen::Dynamic, Eigen::Dynamic > to_diagonal(const Eigen::Ref< const Eigen::Matrix< N, Eigen::Dynamic, 1 > > vec)
Transforms a vector into a diagonal matrix.
Definition: quadratic_variable.h:134
curve_abc_t::curve_ptr_t curve_ptr_t
Definition: bezier_curve.h:45
exact_cubic_constraint_one_dim time_reparam_
Definition: effector_spline_rotation.h:135
curve_abc_t::curve_ptr_t curve_ptr_t
Definition: polynomial.h:44
std::vector< Waypoint > T_Waypoint
Definition: effector_spline.h:31
cubic_hermite_spline_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: cubic_hermite_spline.h:157
double min_
Definition: effector_spline_rotation.h:133
std::pair< Numeric, quat_t > waypoint_quat_t
Definition: effector_spline_rotation.h:33
boost::shared_ptr< curve_X_t > curve_ptr_t
Definition: se3_curve.h:34
time_t min() const
Get the minimum time for which the curve is defined.
Definition: so3_linear.h:179
Eigen::VectorXd pointX_t
Definition: fwd.h:58
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:205
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: polynomial.h:273
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > ineqVector
Definition: definitions.h:36
friend class boost::serialization::access
Definition: so3_linear.h:201
Point point_t
Definition: exact_cubic.h:43
virtual bool operator!=(const cubic_hermite_spline_t &other) const
Definition: cubic_hermite_spline.h:138
brief Computes all Bernstein polynomes for a certain degree std::vector< Bern< Numeric > > makeBernstein(const unsigned int n)
Definition: bernstein.h:79
piecewise_curve_t convert_piecewise_curve_to_cubic_hermite()
Definition: piecewise_curve.h:241
virtual std::size_t degree() const
Get the degree of the curve.
Definition: cubic_hermite_spline.h:378
Numeric num_t
Definition: exact_cubic.h:48
Eigen::Quaterniond quat_from_
Definition: effector_spline_rotation.h:130
curve_constraints< point_t > curve_constraints_t
Definition: bezier_curve.h:38
double real
Definition: python_definitions.h:14
bezier_curve< double, double, true, pointX_t > bezier_t
Definition: fwd.h:83
polynomial< Time, Numeric, Safe, Point, T_Point > create_cubic(Point const &a, Point const &b, Point const &c, Point const &d, const Time t_min, const Time t_max)
Definition: cubic_spline.h:39
quadratic_variable< Numeric > bezier_product(In PointsBegin1, In PointsEnd1, In PointsBegin2, In PointsEnd2, const std::size_t)
Definition: details.h:279
class allowing to create a Bezier curve of dimension 1 <= n <= 3.
static quadratic_variable_t Zero(size_t dim=0)
Definition: quadratic_variable.h:46
std::vector< point3_t, Eigen::aligned_allocator< point3_t > > t_point3_t
Definition: fwd.h:63
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t &end_spline, const Time)
Compute end velocity : along landing normal and respecting time.
Definition: effector_spline.h:67
SE3Curve(const pointX_t &init_pos, const pointX_t &end_pos, const Quaternion &init_rot, const Quaternion &end_rot, const time_t &t_min, const time_t &t_max)
Constructor from init/end pose, with quaternion.
Definition: se3_curve.h:69
std::vector< waypoint_quat_t > t_waypoint_quat_t
Definition: effector_spline_rotation.h:34
Eigen::Matrix< Numeric, 1, 1 > point_one_dim_t
Definition: effector_spline_rotation.h:35
linear_variable(const matrix_x_t &B, const vector_x_t &c)
Definition: linear_variable.h:33
std::size_t size() const
Definition: linear_variable.h:80
Eigen::Matrix< Numeric, 7, 1 > config_t
Definition: effector_spline_rotation.h:31
virtual point_t operator()(const time_t t) const
Evaluation of the bezier curve at time t.
Definition: bezier_curve.h:133
bool isEquivalent(const curve_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision(), const size_t order=5) const
isEquivalent check if other and *this are approximately equal by values, given a precision treshold.
Definition: curve_abc.h:76
void serialize(Archive &ar, const unsigned int version)
Definition: linear_variable.h:97
Numeric num_t
Definition: definitions.h:43
piecewise_curve(const t_curve_ptr_t &curves_list)
Definition: piecewise_curve.h:54
Numeric operator()(const Eigen::Ref< const point_t > &val) const
Definition: quadratic_variable.h:49
const Eigen::Quaterniond land_quat_
Definition: effector_spline_rotation.h:277
Eigen::MatrixXd pointX_list_t
Definition: python_definitions.h:20
integral_cost_flag
Definition: integral_cost.h:20
piecewise_curve< double, double, true, transform_t, point6_t, curve_SE3_t > piecewise_SE3_t
Definition: fwd.h:97
Time T_max_
Definition: piecewise_curve.h:422
Eigen::Matrix< real, 3, Eigen::Dynamic > point_list3_t
Definition: python_definitions.h:27
Represents a linear interpolation in SO3, using the slerp method provided by Eigen::Quaternion.
Definition: fwd.h:40
virtual std::size_t dim() const
Get dimension of curve.
Definition: so3_linear.h:176
Eigen::Matrix< real, 6, Eigen::Dynamic > point_list6_t
Definition: python_definitions.h:28
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluate the derivative order N of curve at time t.
Definition: bezier_curve.h:235
Numeric num_t
Definition: cubic_hermite_spline.h:40
boost::shared_ptr< curve_rotation_t > curve_rotation_ptr_t
Definition: se3_curve.h:35
curve_constraints(const size_t dim=3)
Definition: curve_constraint.h:23
virtual num_t max() const
Get the maximum time for which the curve is defined.
Definition: polynomial.h:368
cubic_hermite_spline()
Empty constructor.
Definition: cubic_hermite_spline.h:47
LinearVar fill_with_zeros(const LinearVar &var, const std::size_t i, const std::size_t startVariableIndex, const std::size_t numVariables, const std::size_t Dim)
Definition: details.h:62
t_curve_ptr_t curves_
Definition: piecewise_curve.h:419
virtual time_t max() const
Get the maximum time for which the curve is defined.
Definition: effector_spline_rotation.h:124
std::vector< Time > vector_time_t
Definition: cubic_hermite_spline.h:39
time_t T_min_
Definition: so3_linear.h:197
virtual Time min() const
Get the minimum time for which the curve is defined.
Definition: piecewise_curve.h:405
const matrix_x_t & A() const
Definition: quadratic_variable.h:104
std::vector< vector_x_t, Eigen::aligned_allocator< vector_x_t > > T_vector_x_t
Definition: definitions.h:48
curve_constraints(const curve_constraints &other)
Definition: curve_constraint.h:32
Eigen::Quaternion< Scalar > Quaternion
Definition: se3_curve.h:28
CurveType curve_t
Definition: piecewise_curve.h:37
linear_variable_t & operator/=(const double d)
Definition: linear_variable.h:65
polynomial(const Point &init, const Point &end, const time_t min, const time_t max)
Constructor from boundary condition with C0 : create a polynomial that connect exactly init and end (...
Definition: polynomial.h:107
virtual Time max() const
Get the maximum time for which the curve is defined.
Definition: cubic_hermite_spline.h:375
virtual std::size_t degree() const
Get the degree of the curve.
Definition: piecewise_curve.h:411
t_pair_point_tangent_t getControlPoints()
Get vector of pair (positition, derivative) corresponding to control points.
Definition: cubic_hermite_spline.h:182
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 .
Definition: cubic_spline.h:29
polynomial(const Point &init, const Point &d_init, const Point &end, const Point &d_end, const time_t min, const time_t max)
Constructor from boundary condition with C1 : create a polynomial that connect exactly init and end a...
Definition: polynomial.h:128
time_t T_min_
Definition: polynomial.h:378
@ FIFTH
Definition: integral_cost.h:26
Eigen::Transform< double, 3, Eigen::Affine > transform_t
Definition: fwd.h:62
std::size_t numVariables
Definition: details.h:32
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: exact_cubic.h:45
const curve_rotation_ptr_t rotation_curve() const
const accessor to the rotation curve
Definition: se3_curve.h:218
forward declaration of all curves class
long compute_num_ineq_control_points(const problem_definition< Point, Numeric > &pDef, const problem_data< Point, Numeric > &pData)
Definition: details.h:196
exact_cubic_t::t_spline_t t_spline_t
Definition: effector_spline.h:34
piecewise_curve< double, double, true, pointX_t, pointX_t, curve_abc_t > piecewise_t
Definition: fwd.h:85
Time time_t
Definition: so3_linear.h:25
static piecewise_curve_t convert_discrete_points_to_polynomial(t_point_t points, t_time_t time_points)
Definition: piecewise_curve.h:271
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t using horner's scheme.
Definition: polynomial.h:246
std::pair< pointX_t, pointX_t > pair_pointX_tangent_t
Definition: python_definitions.h:19
const curve_ptr_t translation_curve() const
const accessor to the translation curve
Definition: se3_curve.h:216
piecewise_curve_t::t_curve_ptr_t t_curve_ptr_t
Definition: exact_cubic.h:58
polynomial(const coeff_t &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynomial.h:58
polynomial()
Empty constructor.
Definition: polynomial.h:50
constraint_flag
Definition: definitions.h:20
polynomial(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
Constructor.
Definition: polynomial.h:92
SO3Linear_t compute_derivate(const std::size_t) const
Definition: so3_linear.h:164
virtual point_t operator()(const time_t t) const
Evaluation of the SO3Linear at time t using Eigen slerp.
Definition: so3_linear.h:117
curve_abc< Time, Numeric, Safe, point_t, point_derivate_t > base_curve_t
Definition: piecewise_curve.h:36
interface for a Curve of arbitrary dimension.
exact_cubic< time_t, Numeric, Dim, Safe, Point > exact_cubic_t
Definition: OptimizeSpline.h:33
virtual bool operator==(const SE3Curve_t &other) const
Definition: se3_curve.h:175
const std::size_t dim_
Definition: details.h:37
class allowing to create a piecewise curve.
virtual Point derivate(const Time t, const std::size_t order) const
Evaluate the derivative of order N of spline at time t.
Definition: cubic_hermite_spline.h:145
Definition: curve_constraint.h:21
matrix3_t getInitRotation() const
Definition: so3_linear.h:186
quadratic_variable< Numeric > compute_integral_cost(const problem_data< Point, Numeric > &pData, const integral_cost_flag flag)
Definition: integral_cost.h:43
point_t end_vel
Definition: curve_constraint.h:45
time_t T_max_
Definition: se3_curve.h:225
virtual point_derivate_t derivate(const time_t t, const std::size_t order) const =0
Evaluate the derivative of order N of curve at time t.
friend class boost::serialization::access
Definition: cubic_hermite_spline.h:406
Eigen::Ref< const vector_x_t > vector_x_ref_t
Definition: bezier_curve.h:35
const exact_cubic_t * spline_
Definition: effector_spline_rotation.h:275
SO3Linear< Time, Numeric, Safe > SO3Linear_t
Definition: so3_linear.h:27
curve_abc< Time, Numeric, Safe, matrix3_t, point3_t > curve_rotation_t
Definition: se3_curve.h:33
size_t dim_
Definition: curve_constraint.h:48
Definition: bezier_curve.h:32
bool isZero() const
Definition: linear_variable.h:91
virtual point_derivate_t derivate(const time_t t, const std::size_t order) const
Evaluation of the derivative of order N of spline at time t.
Definition: se3_curve.h:183
cubic_hermite_spline< double, double, true, point3_t > cubic_hermite_spline3_t
Definition: fwd.h:91
exact_cubic< double, double, true, point3_t, t_point3_t, polynomial_t > exact_cubic3_t
Definition: fwd.h:89
quadratic_variable & operator/=(const double d)
Definition: quadratic_variable.h:85
bool isZero() const
Definition: quadratic_variable.h:122
std::pair< real, pointX_t > waypoint_t
Definition: python_definitions.h:24
Numeric num_t
Definition: OptimizeSpline.h:32
std::pair< Numeric, point_one_dim_t > waypoint_one_dim_t
Definition: effector_spline_rotation.h:37
Time T_min_
Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
Definition: cubic_hermite_spline.h:396
Eigen::Quaternion< double > quaternion_t
Definition: fwd.h:61
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t &constraints, const time_t T_min=0., const time_t T_max=1., const time_t mult_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:93
Composition of a curve of any type of dimension 3 and a curve representing an rotation (in current im...
Definition: fwd.h:37
const double time_land_offset_
Definition: effector_spline_rotation.h:279
vector_time_t time_control_points_
Vector of Time corresponding to time of each N control points : time at .
Definition: cubic_hermite_spline.h:389
SE3Curve(curve_ptr_t translation_curve, const matrix3_t &init_rot, const matrix3_t &end_rot)
Constructor from curve for the translation and init/end rotation, with rotation matrix.
Definition: se3_curve.h:107
cubic_hermite_spline_t compute_derivate(const std::size_t) const
Definition: cubic_hermite_spline.h:150
std::vector< point_derivate_t, Eigen::aligned_allocator< point_derivate_t > > t_point_derivate_t
Definition: piecewise_curve.h:33
SO3Linear(const SO3Linear &other)
Definition: so3_linear.h:88
point_t init_pos
Definition: definitions.h:73
linear_variable< Numeric > var_t
Definition: details.h:27
cubic_hermite_spline(const cubic_hermite_spline &other)
Definition: cubic_hermite_spline.h:74
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:170
Numeric norm() const
Definition: linear_variable.h:82
Eigen::Ref< quat_t > quat_ref_t
Definition: effector_spline_rotation.h:29
std::size_t getNumberSplines()
Definition: exact_cubic.h:119
Time time_t
Definition: curve_abc.h:37
Point point_t
Definition: piecewise_curve.h:30
exact_cubic< Time, Numeric, true, Point, T_Point > exact_cubic_t
Definition: effector_spline.h:32
Time T_min_
Definition: piecewise_curve.h:422
SO3Linear(const matrix3_t &init_rot, const matrix3_t &end_rot, const time_t t_min, const time_t t_max)
constructor with initial and final rotation expressed as rotation matrix and time bounds
Definition: so3_linear.h:48
curve_abc< double, double, true, point3_t, point3_t > curve_3_t
Definition: fwd.h:68
virtual std::size_t degree() const
Get the degree of the curve.
Definition: polynomial.h:371
std::vector< curve_ptr_t > t_curve_ptr_t
Definition: piecewise_curve.h:39
Eigen::Matrix< Scalar, 6, 1 > point_derivate_t
Definition: se3_curve.h:27
virtual ~exact_cubic()
Destructor.
Definition: exact_cubic.h:105
t_pair_point_tangent_t control_points_
Vector of pair < Point, Tangent >.
Definition: cubic_hermite_spline.h:385
curves::curve_constraints< pointX_t > curve_constraints_t
Definition: python_definitions.h:22
Point point_t
Definition: polynomial.h:36
Numeric min() const
Definition: effector_spline_rotation.h:224
@ END_ACC
Definition: definitions.h:27
const typedef Eigen::Ref< const quat_t > quat_ref_const_t
Definition: effector_spline_rotation.h:30
quadratic_variable(const point_t &b, const Numeric c=0)
Definition: quadratic_variable.h:43
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: cubic_hermite_spline.h:127
polynomial(const polynomial &other)
Definition: polynomial.h:218
friend class boost::serialization::access
Definition: linear_variable.h:94
T_Point t_point_t
Definition: exact_cubic.h:44
std::size_t startVariableIndex
Definition: details.h:34
quadratic_variable< Numeric > cost
Definition: definitions.h:37
friend class boost::serialization::access
Definition: polynomial.h:395
polynomial_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: polynomial.h:318
std::size_t num_curves() const
Definition: piecewise_curve.h:213
vector_x_t operator()(const Eigen::Ref< const vector_x_t > &val) const
Definition: linear_variable.h:36
storage for variable points of the form p_i = B_i x + c_i
Eigen::Matrix< real, Eigen::Dynamic, Eigen::Dynamic > point_listX_t
Definition: python_definitions.h:26
polynomial_t::coeff_t coeff_t
Definition: python_definitions.h:29
virtual point_derivate_t derivate(const time_t t, const std::size_t order) const
Evaluation of the derivative of order N of spline at time t.
Definition: so3_linear.h:150
virtual bool operator==(const bezier_curve_t &other) const
Definition: bezier_curve.h:173
std::vector< waypoint_t > t_waypoint_t
Definition: python_definitions.h:25
std::vector< Time > t_time_t
Definition: piecewise_curve.h:40
Point point_t
Definition: bezier_curve.h:33
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
Definition: definitions.h:34
polynomial(const T_Point &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynomial.h:74
T_matrix_x_t inequalityMatrices_
Definition: definitions.h:78
exact_cubic< Numeric, Numeric, false, point_one_dim_t > exact_cubic_constraint_one_dim
Definition: effector_spline_rotation.h:36
static piecewise_curve_t convert_discrete_points_to_polynomial(t_point_t points, t_point_derivate_t points_derivative, t_point_derivate_t points_second_derivative, t_time_t time_points)
Definition: piecewise_curve.h:325
boost::shared_ptr< curve_rotation_t > curve_rotation_ptr_t
Definition: fwd.h:77
linear_variable_t & operator+=(const linear_variable_t &w1)
Definition: linear_variable.h:43
Time time_t
Definition: exact_cubic.h:47
exact_cubic_t::spline_constraints spline_constraints_t
Definition: effector_spline.h:33
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: bezier_curve.h:164
~rotation_spline()
Definition: effector_spline_rotation.h:52
Time time_t
Definition: polynomial.h:38
Time time_t
Definition: se3_curve.h:29
virtual bool operator!=(const polynomial_t &other) const
Definition: polynomial.h:284
curves::Bern< double > bernstein_t
Definition: python_definitions.h:31
curves::curve_constraints< point3_t > curve_constraints3_t
Definition: python_definitions.h:23
piecewise_curve< Time, Numeric, Safe, Point, Point_derivate, CurveType > piecewise_curve_t
Definition: piecewise_curve.h:41
vector_time_t duration_splines_
Vector of Time corresponding to time duration of each subspline.
Definition: cubic_hermite_spline.h:394
void setTime(const vector_time_t &time_control_points)
Set time of each control point of cubic hermite spline.
Definition: cubic_hermite_spline.h:166
constraint_flag & operator^=(constraint_flag &a, constraint_flag b)
Definition: details.h:336
virtual std::size_t degree() const
Get the degree of the curve.
Definition: so3_linear.h:185
point_t end_acc
Definition: curve_constraint.h:46
time_t max() const
Get the maximum time for which the curve is defined.
Definition: se3_curve.h:211
piecewise_curve(const piecewise_curve &other)
Definition: piecewise_curve.h:60
std::vector< pair_point_tangent_t, Eigen::aligned_allocator< Point > > t_pair_point_tangent_t
Definition: cubic_hermite_spline.h:38
t_time_t time_curves_
Definition: piecewise_curve.h:420
linear_variable< N, S > operator-(const linear_variable< N, S > &w1, const linear_variable< N, S > &w2)
Definition: linear_variable.h:119
quaternion_t computeAsQuaternion(const time_t t) const
Definition: so3_linear.h:104
virtual bool operator!=(const SO3Linear_t &other) const
Definition: so3_linear.h:144
SE3Curve(const transform_t &init_transform, const transform_t &end_transform, const time_t &t_min, const time_t &t_max)
Constructor from init/end transform use polynomial of degree 1 for position and SO3Linear for rotatio...
Definition: se3_curve.h:55
const double time_lift_offset_
Definition: effector_spline_rotation.h:278
~curve_constraints()
Definition: curve_constraint.h:41
quaternion_t end_rot_
Definition: so3_linear.h:195
exact_cubic_t * GenerateOptimizedCurve(In wayPointsBegin, In wayPointsEnd) const
Start an optimization loop to create curve.
bezier_curve_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: bezier_curve.h:200
exact_cubic_t::spline_t spline_t
Definition: effector_spline.h:35
std::pair< double, Point > Waypoint
Definition: effector_spline.h:30
~bezier_curve()
Destructor.
Definition: bezier_curve.h:125
Numeric num_t
Definition: bezier_curve.h:37
quadratic_variable(const matrix_x_t &A, const point_t &b, const Numeric c=0)
Definition: quadratic_variable.h:37
constraint_flag operator&(constraint_flag a, constraint_flag b)
Definition: details.h:320
bool isApprox(const SO3Linear_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals.
Definition: so3_linear.h:127
matrix3_t getEndRotation() const
Definition: so3_linear.h:187
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: piecewise_curve.h:32
SE3Curve< double, double, true > SE3Curve_t
Definition: fwd.h:96
exact_cubic(const t_curve_ptr_t &subSplines)
Definition: exact_cubic.h:99
void serialize(Archive &ar, const unsigned int version)
Definition: piecewise_curve.h:430
SO3Linear< double, double, true > SO3Linear_t
Definition: fwd.h:95
void serialize(Archive &, const unsigned int version)
Definition: curve_abc.h:138
Bezier * compute_linear_control_points(const problem_data< Point, Numeric > &pData, const std::vector< LinearVar > &linearVars, const Numeric totalTime)
Definition: details.h:73
piecewise_curve()
Empty constructor.
Definition: piecewise_curve.h:46
std::pair< time_t, time_t > timeRange()
Definition: curve_abc.h:132
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:91
Eigen::Matrix< Numeric, 3, 3 > Matrix3
Definition: exact_cubic.h:46
virtual std::size_t dim() const =0
Get dimension of curve.
virtual std::size_t degree() const =0
Get the degree of the curve.
SE3Curve(curve_ptr_t translation_curve, curve_rotation_ptr_t rotation_curve)
Constructor from from translation and rotation curves object.
Definition: se3_curve.h:119
~SplineOptimizer()
Destructor.
Definition: OptimizeSpline.h:47
curve_abc< Time, Numeric, Safe, point_t, point_derivate_t > curve_abc_t
Definition: so3_linear.h:26
quadratic_problem< Point, Numeric > generate_problem(const problem_definition< Point, Numeric > &pDef, const quadratic_variable< Numeric > &cost)
Definition: quadratic_problem.h:22
utils for defining optimization problems
virtual bool operator==(const cubic_hermite_spline_t &other) const
Definition: cubic_hermite_spline.h:136
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > matrix_x_t
Definition: linear_variable.h:28
virtual point_t operator()(const Time t) const
Definition: piecewise_curve.h:70
virtual bool isApprox(const base_curve_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: piecewise_curve.h:96
void PseudoInverse(_Matrix_Type_ &pinvmat)
Definition: MathDefs.h:26
Point_derivate point_derivate_t
Definition: piecewise_curve.h:31
cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t &time_control_points)
Constructor.
Definition: cubic_hermite_spline.h:55
std::size_t getNumberCurves()
Definition: piecewise_curve.h:414
polynomial_t compute_derivate(const std::size_t order) const
Definition: polynomial.h:305
std::vector< spline_t > t_spline_t
Definition: exact_cubic.h:50
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.
Definition: piecewise_curve.h:87
SO3Linear< Time, Numeric, Safe > SO3Linear_t
Definition: se3_curve.h:37
point_t end_pos
Definition: definitions.h:74
virtual point_t operator()(const time_t t) const =0
Evaluation of the cubic spline at time t.
linear_variable()
Definition: linear_variable.h:31
SplineBase spline_t
Definition: exact_cubic.h:49
std::pair< Point, Point > pair_point_tangent_t
Definition: cubic_hermite_spline.h:37
polynomial< Time, Numeric, Safe, pointX_t > polynomial_t
Definition: se3_curve.h:38
Waypoint compute_offset(const Waypoint &source, const Point &normal, const Numeric offset, const Time time_offset)
Compute time such that the equation from source to offsetpoint is necessarily a line.
Definition: effector_spline.h:38
virtual bool operator!=(const SE3Curve_t &other) const
Definition: se3_curve.h:177
std::size_t size_
Definition: piecewise_curve.h:421
std::size_t numStateConstraints
Definition: details.h:35
T_Point vectorFromEigenArray(const PointList &array)
Definition: python_definitions.h:34
void serialize(Archive &ar, const unsigned int version)
Definition: polynomial.h:398
bezier_curve< Time, Numeric, Safe, Point > bezier_curve_t
Definition: bezier_curve.h:41
virtual bool isApprox(const curve_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const =0
isApprox check if other and *this are approximately equals given a precision treshold Only two curves...
linear_variable< N, S > operator/(const linear_variable< N, S > &w, const double k)
Definition: linear_variable.h:137
bezier_curve()
Empty constructor.
Definition: bezier_curve.h:51
Time T_max_
Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
Definition: cubic_hermite_spline.h:398
@ END_POS
Definition: definitions.h:25
Numeric num_t
Definition: polynomial.h:39
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: OptimizeSpline.h:29
virtual point_t operator()(const time_t t) const
Evaluation of the SE3Curve at time t.
Definition: se3_curve.h:141
SplineOptimizer()
Initializes optimizer environment.
Definition: OptimizeSpline.h:39
class allowing to create an Exact cubic spline.
exact_cubic(const exact_cubic &other)
Copy Constructor.
Definition: exact_cubic.h:102
spline_t getSplineAt(std::size_t index)
Definition: exact_cubic.h:121
curve_abc< Time, Numeric, Safe, point_t, point_derivate_t > curve_abc_t
Definition: se3_curve.h:30
storage for variable points of the form p_i = x' A_i x + B_i x + c_i
Eigen::Transform< Numeric, 3, Eigen::Affine > transform_t
Definition: se3_curve.h:25
coeff_t coefficients_
Definition: polynomial.h:376
std::vector< pointX_t, Eigen::aligned_allocator< pointX_t > > t_pointX_t
Definition: fwd.h:64
piecewise_curve< Time, Numeric, Safe, Point > piecewise_curve_t
Definition: exact_cubic.h:57
point_t init_vel
Definition: curve_constraint.h:42
quaternion_t init_rot_
Definition: so3_linear.h:195
exact_cubic< Time, Numeric, false, quat_t, std::vector< quat_t, Eigen::aligned_allocator< quat_t > >, rotation_spline > exact_cubic_quat_t
Definition: effector_spline_rotation.h:141
@ INIT_ACC
Definition: definitions.h:23
void serialize(Archive &ar, const unsigned int version)
Definition: exact_cubic.h:280
point_t coeffAtDegree(const std::size_t degree) const
Definition: polynomial.h:324
Point point_t
Definition: OptimizeSpline.h:30
SO3Linear_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: so3_linear.h:171
SplineOptimizer< time_t, Numeric, Dim, Safe, Point > splineOptimizer_t
Definition: OptimizeSpline.h:34
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 .
Definition: quintic_spline.h:29
std::size_t size() const
Get number of control points contained in the trajectory.
Definition: cubic_hermite_spline.h:192
std::size_t dim_
Definition: se3_curve.h:222
spline_t make_end_spline(const Point &normal, const Point &from, const Numeric offset, const Time init_time, const Time time_offset)
Compute spline from land way point to end point.
Definition: effector_spline.h:48
problem_data< Point, Numeric, Safe > setup_control_points(const problem_definition< Point, Numeric > &pDef)
Definition: details.h:85
polynomial< Time, Numeric, 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 t_min, const Time t_max)
Definition: quintic_spline.h:42
std::size_t size_
Number of control points (pairs).
Definition: cubic_hermite_spline.h:400
boost::shared_ptr< curve_abc_t > curve_ptr_t
Definition: fwd.h:75
virtual Point operator()(const Time t) const
Evaluation of the cubic hermite spline at time t.
Definition: cubic_hermite_spline.h:93
std::size_t size() const
Definition: quadratic_variable.h:123
point_t init_acc
Definition: curve_constraint.h:43
~SE3Curve()
Destructor.
Definition: se3_curve.h:48
virtual std::size_t degree() const
Get the degree of the curve.
Definition: se3_curve.h:214
virtual time_t max() const =0
Get the maximum time for which the curve is defined.
T_Point vectorFromEigenVector(const PointList &vector)
Definition: python_definitions.h:42
constraint_flag operator^(constraint_flag a, constraint_flag b)
Definition: details.h:324
Point point_t
Definition: curve_constraint.h:22
curve_abc< Time, Numeric, Safe, pointX_t > curve_X_t
Definition: se3_curve.h:31
std::size_t degree_
Definition: polynomial.h:377
@ ACCELERATION
Definition: integral_cost.h:23
std::size_t dim_
Definition: piecewise_curve.h:418
Numeric Scalar
Definition: se3_curve.h:24
point3_t log3(const matrix3_t &R)
Log: SO3 -> so3.
Definition: so3_linear.h:247
Eigen::Ref< coeff_t > coeff_t_ref
Definition: polynomial.h:42
~SO3Linear()
Destructor.
Definition: so3_linear.h:84
t_spline_t::const_iterator cit_spline_t
Definition: exact_cubic.h:52
Polynomial polynomial_from_curve(const typename Polynomial::curve_abc_t &curve)
Converts a cubic hermite spline or a bezier curve to a polynomial.
Definition: curve_conversion.h:20
Eigen::Matrix< num_t, Eigen::Dynamic, Eigen::Dynamic > matrix_x_t
Definition: definitions.h:46
const Eigen::Quaterniond to_quat_
Definition: effector_spline_rotation.h:276
virtual bool operator==(const piecewise_curve_t &other) const
Definition: piecewise_curve.h:105
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > point_t
Definition: quadratic_variable.h:27
curve_abc()
Constructor.
Definition: curve_abc.h:45
bool isApprox(const polynomial_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals.
Definition: polynomial.h:268
std::size_t numIntervals() const
Get number of intervals (subsplines) contained in the trajectory.
Definition: cubic_hermite_spline.h:197
std::size_t num_active_constraints(const constraint_flag &flag)
Definition: details.h:51
linear_variable< N, S > operator*(const double k, const linear_variable< N, S > &w)
Definition: linear_variable.h:125
static const double MARGIN
Definition: piecewise_curve.h:423
virtual num_t min() const
Get the minimum time for which the curve is defined.
Definition: polynomial.h:365
linear_variable< N, S > operator+(const linear_variable< N, S > &w1, const linear_variable< N, S > &w2)
Definition: linear_variable.h:113
time_t T_max_
Definition: polynomial.h:378
time_t min() const
Get the minimum time for which the curve is defined.
Definition: se3_curve.h:208