effector_spline.h
Go to the documentation of this file.
1 
19 #ifndef _CLASS_EFFECTORSPLINE
20 #define _CLASS_EFFECTORSPLINE
21 
22 #include "spline/spline_deriv_constraint.h"
23 
24 namespace spline {
25 namespace helpers {
26 typedef double Numeric;
27 typedef double Time;
28 typedef Eigen::Matrix<Numeric, 3, 1> Point;
29 typedef std::vector<Point, Eigen::aligned_allocator<Point> > T_Point;
30 typedef std::pair<double, Point> Waypoint;
31 typedef std::vector<Waypoint> T_Waypoint;
32 typedef spline_deriv_constraint<Time, Numeric, 3, true, Point, T_Point> spline_deriv_constraint_t;
33 typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
37 
38 Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset) {
39  // compute time such that the equation from source to offsetpoint is necessarily a line
40  Numeric norm = normal.norm();
41  assert(norm > 0.);
42  return std::make_pair(source.first + time_offset, (source.second + normal / norm * offset));
43 }
44 
45 spline_t make_end_spline(const Point& normal, const Point& from, const Numeric offset, const Time init_time,
46  const Time time_offset) {
47  // compute spline from land way point to end point
48  // constraints are null velocity and acceleration
49  Numeric norm = normal.norm();
50  assert(norm > 0.);
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;
55 
56  T_Point points;
57  points.push_back(from);
58  points.push_back(b);
59  points.push_back(c);
60  points.push_back(d);
61 
62  return spline_t(points.begin(), points.end(), init_time, init_time + time_offset);
63 }
64 
66  const Time time_offset) {
67  // computing end velocity: along landing normal and respecting time
68  spline_constraints_t constraints;
69  constraints.end_acc = end_spline.derivate(end_spline.min(), 2);
70  constraints.end_vel = end_spline.derivate(end_spline.min(), 1);
71  return constraints;
72 }
73 
89 template <typename In>
90 exact_cubic_t* effector_spline(In wayPointsBegin, In wayPointsEnd, const Point& lift_normal = Eigen::Vector3d::UnitZ(),
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) {
94  T_Waypoint waypoints;
95  const Waypoint &inPoint = *wayPointsBegin, endPoint = *(wayPointsEnd - 1);
96  waypoints.push_back(inPoint);
97  // adding initial offset
98  waypoints.push_back(compute_offset(inPoint, lift_normal, lift_offset, lift_offset_duration));
99  // inserting all waypoints but last
100  waypoints.insert(waypoints.end(), wayPointsBegin + 1, wayPointsEnd - 1);
101  // inserting waypoint to start landing
102  const Waypoint& landWaypoint = compute_offset(endPoint, land_normal, land_offset, -land_offset_duration);
103  waypoints.push_back(landWaypoint);
104  // specifying end velocity constraint such that landing will be in straight line
105  spline_t end_spline =
106  make_end_spline(land_normal, landWaypoint.second, land_offset, landWaypoint.first, land_offset_duration);
107  spline_constraints_t constraints = compute_required_offset_velocity_acceleration(end_spline, land_offset_duration);
108  spline_deriv_constraint_t all_but_end(waypoints.begin(), waypoints.end(), constraints);
109  t_spline_t splines = all_but_end.subSplines_;
110  splines.push_back(end_spline);
111  return new exact_cubic_t(splines);
112 }
113 } // namespace helpers
114 } // namespace spline
115 #endif //_CLASS_EFFECTORSPLINE
spline::helpers::spline_constraints_t
spline_deriv_constraint_t::spline_constraints spline_constraints_t
Definition: effector_spline.h:33
spline::helpers::make_end_spline
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::helpers::compute_offset
Waypoint compute_offset(const Waypoint &source, const Point &normal, const Numeric offset, const Time time_offset)
Definition: effector_spline.h:38
spline::helpers::Numeric
double Numeric
Definition: effector_spline.h:26
spline::helpers::Time
double Time
Definition: effector_spline.h:27
spline::helpers::effector_spline
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
spline::helpers::Point
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
spline::helpers::spline_t
spline_deriv_constraint_t::spline_t spline_t
Definition: effector_spline.h:36
spline::helpers::t_spline_t
spline_deriv_constraint_t::t_spline_t t_spline_t
Definition: effector_spline.h:35
spline::helpers::Waypoint
std::pair< double, Point > Waypoint
Definition: effector_spline.h:30
spline::helpers::compute_required_offset_velocity_acceleration
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t &end_spline, const Time time_offset)
Definition: effector_spline.h:65
spline::helpers::exact_cubic_t
spline_deriv_constraint_t::exact_cubic_t exact_cubic_t
Definition: effector_spline.h:34
spline::helpers::spline_deriv_constraint_t
spline_deriv_constraint< Time, Numeric, 3, true, Point, T_Point > spline_deriv_constraint_t
Definition: effector_spline.h:32
spline::helpers::T_Waypoint
std::vector< Waypoint > T_Waypoint
Definition: effector_spline.h:31
spline
Definition: bernstein.h:20
spline::helpers::T_Point
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29