Loading...
Searching...
No Matches
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
24namespace spline {
25namespace helpers {
26typedef double Numeric;
27typedef double Time;
28typedef Eigen::Matrix<Numeric, 3, 1> Point;
29typedef std::vector<Point, Eigen::aligned_allocator<Point> > T_Point;
30typedef std::pair<double, Point> Waypoint;
31typedef std::vector<Waypoint> T_Waypoint;
32typedef spline_deriv_constraint<Time, Numeric, 3, true, Point, T_Point>
34typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
35typedef spline_deriv_constraint_t::exact_cubic_t exact_cubic_t;
36typedef spline_deriv_constraint_t::t_spline_t t_spline_t;
37typedef spline_deriv_constraint_t::spline_t spline_t;
38
39Waypoint compute_offset(const Waypoint& source, const Point& normal,
40 const Numeric offset, const Time time_offset) {
41 // compute time such that the equation from source to offsetpoint is
42 // necessarily a line
43 Numeric norm = normal.norm();
44 assert(norm > 0.);
45 return std::make_pair(source.first + time_offset,
46 (source.second + normal / norm * offset));
47}
48
49spline_t make_end_spline(const Point& normal, const Point& from,
50 const Numeric offset, const Time init_time,
51 const Time time_offset) {
52 // compute spline from land way point to end point
53 // constraints are null velocity and acceleration
54 Numeric norm = normal.norm();
55 assert(norm > 0.);
56 Point n = normal / norm;
57 Point d = offset / (time_offset * time_offset * time_offset) * -n;
58 Point c = -3 * d * time_offset;
59 Point b = -c * time_offset;
60
61 T_Point points;
62 points.push_back(from);
63 points.push_back(b);
64 points.push_back(c);
65 points.push_back(d);
66
67 return spline_t(points.begin(), points.end(), init_time,
68 init_time + time_offset);
69}
70
72 const spline_t& end_spline, const Time time_offset) {
73 // computing end velocity: along landing normal and respecting time
74 spline_constraints_t constraints;
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
96template <typename In>
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 spline_deriv_constraint_t all_but_end(waypoints.begin(), waypoints.end(),
125 constraints);
126 t_spline_t splines = all_but_end.subSplines_;
127 splines.push_back(end_spline);
128 return new exact_cubic_t(splines);
129}
130} // namespace helpers
131} // namespace spline
132#endif //_CLASS_EFFECTORSPLINE
Waypoint compute_offset(const Waypoint &source, const Point &normal, const Numeric offset, const Time time_offset)
Definition effector_spline.h:39
spline_deriv_constraint_t::spline_t spline_t
Definition effector_spline.h:37
std::vector< Waypoint > T_Waypoint
Definition effector_spline.h:31
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
double Time
Definition effector_spline.h:27
spline_deriv_constraint_t::t_spline_t t_spline_t
Definition effector_spline.h:36
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t &end_spline, const Time time_offset)
Definition effector_spline.h:71
spline_deriv_constraint< Time, Numeric, 3, true, Point, T_Point > spline_deriv_constraint_t
Definition effector_spline.h:33
spline_deriv_constraint_t::spline_constraints spline_constraints_t
Definition effector_spline.h:34
Eigen::Matrix< Numeric, 3, 1 > Point
Definition effector_spline.h:28
std::pair< double, Point > Waypoint
Definition effector_spline.h:30
double Numeric
Definition effector_spline.h:26
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition effector_spline.h:29
spline_deriv_constraint_t::exact_cubic_t exact_cubic_t
Definition effector_spline.h:35
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:49
Definition bernstein.h:20