effector_spline.h

Go to the documentation of this file.

 
#ifndef _CLASS_EFFECTORSPLINE
#define _CLASS_EFFECTORSPLINE
 
#include "spline/spline_deriv_constraint.h"
 
namespace spline {
namespace helpers {
typedef double Numeric;
typedef double Time;
typedef Eigen::Matrix<Numeric, 3, 1> Point;
typedef std::vector<Point, Eigen::aligned_allocator<Point> > T_Point;
typedef std::pair<double, Point> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
typedef spline_deriv_constraint<Time, Numeric, 3, true, Point, T_Point>
    spline_deriv_constraint_t;
typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
typedef spline_deriv_constraint_t::exact_cubic_t exact_cubic_t;
typedef spline_deriv_constraint_t::t_spline_t t_spline_t;
typedef spline_deriv_constraint_t::spline_t spline_t;
 
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
  Numeric norm = normal.norm();
  assert(norm > 0.);
  return std::make_pair(source.first + time_offset,
                        (source.second + normal / norm * offset));
}
 
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
  Numeric norm = normal.norm();
  assert(norm > 0.);
  Point n = normal / norm;
  Point d = offset / (time_offset * time_offset * time_offset) * -n;
  Point c = -3 * d * time_offset;
  Point b = -c * time_offset;
 
  T_Point points;
  points.push_back(from);
  points.push_back(b);
  points.push_back(c);
  points.push_back(d);
 
  return spline_t(points.begin(), points.end(), init_time,
                  init_time + time_offset);
}
 
spline_constraints_t compute_required_offset_velocity_acceleration(
    const spline_t& end_spline, const Time time_offset) {
  // computing end velocity: along landing normal and respecting time
  spline_constraints_t constraints;
  constraints.end_acc = end_spline.derivate(end_spline.min(), 2);
  constraints.end_vel = end_spline.derivate(end_spline.min(), 1);
  return constraints;
}
 
template <typename In>
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) {
  T_Waypoint waypoints;
  const Waypoint &inPoint = *wayPointsBegin, endPoint = *(wayPointsEnd - 1);
  waypoints.push_back(inPoint);
  // adding initial offset
  waypoints.push_back(
      compute_offset(inPoint, lift_normal, lift_offset, lift_offset_duration));
  // inserting all waypoints but last
  waypoints.insert(waypoints.end(), wayPointsBegin + 1, wayPointsEnd - 1);
  // inserting waypoint to start landing
  const Waypoint& landWaypoint =
      compute_offset(endPoint, land_normal, land_offset, -land_offset_duration);
  waypoints.push_back(landWaypoint);
  // specifying end velocity constraint such that landing will be in straight
  // line
  spline_t end_spline =
      make_end_spline(land_normal, landWaypoint.second, land_offset,
                      landWaypoint.first, land_offset_duration);
  spline_constraints_t constraints =
      compute_required_offset_velocity_acceleration(end_spline,
                                                    land_offset_duration);
  spline_deriv_constraint_t all_but_end(waypoints.begin(), waypoints.end(),
                                        constraints);
  t_spline_t splines = all_but_end.subSplines_;
  splines.push_back(end_spline);
  return new exact_cubic_t(splines);
}
}  // namespace helpers
}  // namespace spline
#endif  //_CLASS_EFFECTORSPLINE