#include "curve_abc.h"
#include "MathDefs.h"
#include <math.h>
#include <vector>
#include <stdexcept>
quat_t operator()(const Numeric t) const
Definition: effector_spline_rotation.h:64
const exact_cubic_quat_t quat_spline_
Definition: effector_spline_rotation.h:239
Numeric min() const
Definition: effector_spline_rotation.h:182
std::pair< double, Point > Waypoint
Definition: effector_spline.h:30
virtual time_t max() const
Definition: effector_spline_rotation.h:86
config_t operator()(const Numeric t) const
Evaluation of the effector position and rotation at time t.
Definition: effector_spline_rotation.h:193
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:127
Eigen::Matrix< Numeric, 7, 1 > config_t
Definition: effector_spline_rotation.h:32
~rotation_spline()
Definition: effector_spline_rotation.h:52
virtual time_t max() const
Definition: bezier_curve.h:224
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t &constraints, const time_t minBound=0, const time_t maxBound=1)
Constructor This constructor will add 4 points (2 after the first one, 2 before the last one) to ensu...
Definition: bezier_curve.h:66
Numeric num_t
Definition: bezier_curve.h:34
bezier_curve(In PointsBegin, In PointsEnd, const time_t minBound=0, const time_t maxBound=1)
Constructor.
Definition: bezier_curve.h:46
Eigen::Quaterniond quat_from_
Definition: effector_spline_rotation.h:89
std::vector< Bern< Numeric > > makeBernstein(const unsigned int n)
Computes all Bernstein polynomes for a certain degree.
Definition: bernstein.h:59
Eigen::Ref< quat_t > quat_ref_t
Definition: effector_spline_rotation.h:30
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:36
spline_deriv_constraint_t::spline_t spline_t
Definition: effector_spline.h:36
Waypoint compute_offset(const Waypoint &source, const Point &normal, const Numeric offset, const Time time_offset)
Definition: effector_spline.h:38
Numeric i_
Definition: bernstein.h:51
Numeric operator()(const Numeric u) const
Definition: bernstein.h:45
Bern(const unsigned int m, const unsigned int i)
Definition: bernstein.h:41
~effector_spline_rotation()
Definition: effector_spline_rotation.h:177
Numeric num_t
Definition: OptimizeSpline.h:32
Time time_t
Definition: bezier_curve.h:33
const std::vector< Bern< Numeric > > bernstein_
Definition: bezier_curve.h:231
bezier_curve_t compute_primitive(const std::size_t order) const
Computes the primitive of the curve at order N.
Definition: bezier_curve.h:135
Time time_t
Definition: OptimizeSpline.h:31
spline_deriv_constraint_t::t_spline_t t_spline_t
Definition: effector_spline.h:35
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
double Time
Definition: effector_spline.h:27
~SplineOptimizer()
Destructor.
Definition: OptimizeSpline.h:45
point_t evalBernstein(const Numeric u) const
Evaluates all Bernstein polynomes for a certain degree.
Definition: bezier_curve.h:165
const Eigen::Quaterniond land_quat_
Definition: effector_spline_rotation.h:236
Eigen::Quaterniond quat_to_
Definition: effector_spline_rotation.h:90
std::vector< waypoint_quat_t > t_waypoint_quat_t
Definition: effector_spline_rotation.h:35
Mosek connection to produce optimized splines.
Definition: OptimizeSpline.h:28
const std::size_t size_
Definition: bezier_curve.h:229
std::pair< Numeric, quat_t > waypoint_quat_t
Definition: effector_spline_rotation.h:34
const Eigen::Quaterniond to_quat_
Definition: effector_spline_rotation.h:235
Eigen::Matrix< Numeric, 1, 1 > point_one_dim_t
Definition: effector_spline_rotation.h:36
SplineOptimizer()
Initializes optimizer environment.
Definition: OptimizeSpline.h:39
spline_deriv_constraint_one_dim computeWayPoints() const
Definition: effector_spline_rotation.h:77
const exact_cubic_t * spline_
Definition: effector_spline_rotation.h:234
unsigned int bin(const unsigned int n, const unsigned int k)
Computes a binomal coefficient.
Definition: bernstein.h:34
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:162
point_t evalHorner(const Numeric t) const
Evaluates all Bernstein polynomes for a certain degree using horner's scheme.
Definition: bezier_curve.h:177
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t.
Definition: bezier_curve.h:94
double Numeric
Definition: effector_spline.h:26
Definition: bernstein.h:40
Numeric m_minus_i
Definition: bernstein.h:50
~bezier_curve()
Destructor.
Definition: bezier_curve.h:80
SplineOptimizer< time_t, Numeric, Dim, Safe, Point > splineOptimizer_t
Definition: OptimizeSpline.h:34
exact_cubic< time_t, Numeric, Dim, Safe, Point > exact_cubic_t
Definition: OptimizeSpline.h:33
const time_t maxBound_
Definition: bezier_curve.h:228
rotation_spline & operator=(const rotation_spline &from)
Definition: effector_spline_rotation.h:55
const t_point_t & waypoints() const
Definition: bezier_curve.h:193
Numeric max() const
Definition: effector_spline_rotation.h:183
std::vector< waypoint_one_dim_t > t_waypoint_one_dim_t
Definition: effector_spline_rotation.h:39
spline_deriv_constraint< Numeric, Numeric, 1, false, point_one_dim_t > spline_deriv_constraint_one_dim
Definition: effector_spline_rotation.h:37
spline_deriv_constraint_one_dim time_reparam_
Definition: effector_spline_rotation.h:93
exact_cubic_t * GenerateOptimizedCurve(In wayPointsBegin, In wayPointsEnd) const
Starts an optimization loop to create curve.
std::pair< Numeric, point_one_dim_t > waypoint_one_dim_t
Definition: effector_spline_rotation.h:38
bezier_curve_t compute_derivate(const std::size_t order) const
Computes the derivative curve at order N.
Definition: bezier_curve.h:122
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
spline_deriv_constraint_t::exact_cubic_t exact_cubic_t
Definition: effector_spline.h:34
const double time_lift_offset_
Definition: effector_spline_rotation.h:237
~Bern()
Definition: bernstein.h:43
Definition: effector_spline_rotation.h:41
curve_constraints< point_t > curve_constraints_t
Definition: bezier_curve.h:35
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: OptimizeSpline.h:29
t_point_t::const_iterator cit_point_t
Definition: bezier_curve.h:37
const double time_land_offset_
Definition: effector_spline_rotation.h:238
virtual quat_t derivate(time_t, std::size_t) const
Definition: effector_spline_rotation.h:73
spline_deriv_constraint_t::spline_constraints spline_constraints_t
Definition: effector_spline.h:33
const time_t minBound_
Definition: bezier_curve.h:228
quat_t interpolate_quat(const Numeric t) const
Definition: effector_spline_rotation.h:201
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t &end_spline, const Time time_offset)
Definition: effector_spline.h:65
unsigned int fact(const unsigned int n)
Computes factorial of a number.
Definition: bernstein.h:24
bezier_curve< Time, Numeric, Dim, Safe, Point > bezier_curve_t
Definition: bezier_curve.h:38
Point point_t
Definition: OptimizeSpline.h:30
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:98
const typedef Eigen::Ref< const quat_t > quat_ref_const_t
Definition: effector_spline_rotation.h:31
Definition: bezier_curve.h:31
spline_deriv_constraint< Time, Numeric, 3, true, Point, T_Point > spline_deriv_constraint_t
Definition: effector_spline.h:32
virtual time_t min() const
Definition: bezier_curve.h:223
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:157
virtual time_t min() const
Definition: effector_spline_rotation.h:85
Point point_t
Definition: bezier_curve.h:32
Definition: MathDefs.h:24
Numeric bin_m_i_
Definition: bernstein.h:52
const std::size_t degree_
Definition: bezier_curve.h:230
double min_
Definition: effector_spline_rotation.h:91
Definition: bernstein.h:20
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
std::vector< Waypoint > T_Waypoint
Definition: effector_spline.h:31
curve_abc< Time, Numeric, 4, false, quat_t > curve_abc_quat_t
Definition: effector_spline_rotation.h:33
double max_
Definition: effector_spline_rotation.h:92
Eigen::Matrix< Numeric, 4, 1 > quat_t
Definition: effector_spline_rotation.h:29
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
Represents a trajectory for and end effector uses the method effector_spline to create a spline traje...
Definition: effector_spline_rotation.h:107
void PseudoInverse(_Matrix_Type_ &pinvmat)
Definition: MathDefs.h:28