effector_spline.h
Go to the documentation of this file.
1 
19 #ifndef _CLASS_EFFECTORSPLINE
20 #define _CLASS_EFFECTORSPLINE
21 
22 #include "ndcurves/exact_cubic.h"
23 
24 namespace ndcurves {
25 namespace helpers {
26 typedef double Numeric;
27 typedef double Time;
28 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 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;
36 
39 Waypoint compute_offset(const Waypoint& source, const Point& normal,
40  const Numeric offset, const Time time_offset) {
41  Numeric norm = normal.norm();
42  if (norm < 0.) {
43  throw std::runtime_error("Norm of normal is less than 0!");
44  }
45  return std::make_pair(source.first + time_offset,
46  (source.second + normal / norm * offset));
47 }
48 
51 spline_t make_end_spline(const Point& normal, const Point& from,
52  const Numeric offset, const Time init_time,
53  const Time time_offset) {
54  Numeric norm = normal.norm();
55  if (norm < 0.) {
56  throw std::runtime_error("Norm of normal is less than 0!");
57  }
58  Point n = normal / norm;
59  Point d = offset / (time_offset * time_offset * time_offset) * -n;
60  Point c = -3 * d * time_offset;
61  Point b = -c * time_offset;
62  T_Point points;
63  points.push_back(from);
64  points.push_back(b);
65  points.push_back(c);
66  points.push_back(d);
67  return spline_t(points.begin(), points.end(), init_time,
68  init_time + time_offset);
69 }
70 
73  const spline_t& end_spline, const Time /*time_offset*/) {
74  spline_constraints_t constraints(end_spline.dim());
75  constraints.end_acc = end_spline.derivate(end_spline.min(), 2);
76  constraints.end_vel = end_spline.derivate(end_spline.min(), 1);
77  return constraints;
78 }
79 
96 template <typename In>
97 exact_cubic_t* effector_spline(
98  In wayPointsBegin, In wayPointsEnd,
99  const Point& lift_normal = Eigen::Vector3d::UnitZ(),
100  const Point& land_normal = Eigen::Vector3d::UnitZ(),
101  const Numeric lift_offset = 0.02, const Numeric land_offset = 0.02,
102  const Time lift_offset_duration = 0.02,
103  const Time land_offset_duration = 0.02) {
104  T_Waypoint waypoints;
105  const Waypoint &inPoint = *wayPointsBegin, endPoint = *(wayPointsEnd - 1);
106  waypoints.push_back(inPoint);
107  // adding initial offset
108  waypoints.push_back(
109  compute_offset(inPoint, lift_normal, lift_offset, lift_offset_duration));
110  // inserting all waypoints but last
111  waypoints.insert(waypoints.end(), wayPointsBegin + 1, wayPointsEnd - 1);
112  // inserting waypoint to start landing
113  const Waypoint& landWaypoint =
114  compute_offset(endPoint, land_normal, land_offset, -land_offset_duration);
115  waypoints.push_back(landWaypoint);
116  // specifying end velocity constraint such that landing will be in straight
117  // line
118  spline_t end_spline =
119  make_end_spline(land_normal, landWaypoint.second, land_offset,
120  landWaypoint.first, land_offset_duration);
121  spline_constraints_t constraints =
123  land_offset_duration);
124  exact_cubic_t splines(waypoints.begin(), waypoints.end(), constraints);
125  splines.add_curve(end_spline);
126  return new exact_cubic_t(splines);
127 }
128 } // namespace helpers
129 } // namespace ndcurves
130 #endif //_CLASS_EFFECTORSPLINE
Definition: bernstein.h:20
class allowing to create an Exact cubic spline.
exact_cubic_t::spline_constraints spline_constraints_t
Definition: effector_spline.h:33
Definition: exact_cubic.h:41
std::vector< spline_t > t_spline_t
Definition: exact_cubic.h:50
SplineBase spline_t
Definition: exact_cubic.h:49
std::vector< Waypoint > T_Waypoint
Definition: effector_spline.h:31
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. Constraints are null velocity and acceleration...
Definition: effector_spline.h:51
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
exact_cubic_t::spline_t spline_t
Definition: effector_spline.h:35
double Time
Definition: effector_spline.h:27
exact_cubic_t::t_spline_t t_spline_t
Definition: effector_spline.h:34
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:72
point_t end_acc
Definition: curve_constraint.h:62
exact_cubic< Time, Numeric, true, Point, T_Point > exact_cubic_t
Definition: effector_spline.h:32
std::pair< double, Point > Waypoint
Definition: effector_spline.h:30
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
double Numeric
Definition: effector_spline.h:26
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:39
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:97
Definition: curve_constraint.h:20