hpp-spline
4.10.0
template based classes for creating and manipulating spline and bezier curves. Comes with extra options specific to end-effector trajectories in robotics.
|
Go to the documentation of this file.
19 #ifndef _CLASS_EFFECTORSPLINE
20 #define _CLASS_EFFECTORSPLINE
28 typedef Eigen::Matrix<Numeric, 3, 1>
Point;
29 typedef std::vector<Point, Eigen::aligned_allocator<Point> >
T_Point;
42 return std::make_pair(source.first + time_offset, (source.second + normal / norm * offset));
46 const Time time_offset) {
51 Point n = normal / norm;
52 Point d = offset / (time_offset * time_offset * time_offset) * -n;
53 Point c = -3 * d * time_offset;
54 Point b = -c * time_offset;
57 points.push_back(from);
62 return spline_t(points.begin(), points.end(), init_time, init_time + time_offset);
89 template <
typename In>
91 const Point& land_normal = Eigen::Vector3d::UnitZ(),
const Numeric lift_offset = 0.02,
92 const Numeric land_offset = 0.02,
const Time lift_offset_duration = 0.02,
93 const Time land_offset_duration = 0.02) {
95 const Waypoint &inPoint = *wayPointsBegin, endPoint = *(wayPointsEnd - 1);
96 waypoints.push_back(inPoint);
98 waypoints.push_back(
compute_offset(inPoint, lift_normal, lift_offset, lift_offset_duration));
100 waypoints.insert(waypoints.end(), wayPointsBegin + 1, wayPointsEnd - 1);
103 waypoints.push_back(landWaypoint);
106 make_end_spline(land_normal, landWaypoint.second, land_offset, landWaypoint.first, land_offset_duration);
110 splines.push_back(end_spline);
115 #endif //_CLASS_EFFECTORSPLINE
spline_deriv_constraint_t::spline_constraints spline_constraints_t
Definition: effector_spline.h:33
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t &end_spline, const Time)
Definition: effector_spline.h:65
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
point_t end_vel
Definition: curve_constraint.h:28
t_spline_t subSplines_
Definition: exact_cubic.h:188
Represents a set of cubic splines defining a continuous function crossing each of the waypoint given ...
Definition: spline_deriv_constraint.h:42
virtual num_t min() const
Returns the minimum time for wich curve is defined.
Definition: polynom.h:174
point_t end_acc
Definition: curve_constraint.h:29
Waypoint compute_offset(const Waypoint &source, const Point &normal, const Numeric offset, const Time time_offset)
Definition: effector_spline.h:38
double Numeric
Definition: effector_spline.h:26
double Time
Definition: effector_spline.h:27
std::vector< spline_t > t_spline_t
Definition: spline_deriv_constraint.h:51
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
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
spline_deriv_constraint_t::spline_t spline_t
Definition: effector_spline.h:36
spline_deriv_constraint_t::t_spline_t t_spline_t
Definition: effector_spline.h:35
std::pair< double, Point > Waypoint
Definition: effector_spline.h:30
Represents a polynomf arbitrary order defined on the interval [tBegin, tEnd]. It follows the equation...
Definition: polynom.h:34
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
spline_deriv_constraint_t::exact_cubic_t exact_cubic_t
Definition: effector_spline.h:34
spline_deriv_constraint< Time, Numeric, 3, true, Point, T_Point > spline_deriv_constraint_t
Definition: effector_spline.h:32
std::vector< Waypoint > T_Waypoint
Definition: effector_spline.h:31
Definition: bernstein.h:20
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
Definition: exact_cubic.h:40
Definition: curve_constraint.h:21