effector_spline.h

Go to the documentation of this file.

 
#ifndef _CLASS_EFFECTORSPLINE
#define _CLASS_EFFECTORSPLINE
 
#include "ndcurves/exact_cubic.h"
 
namespace ndcurves {
namespace helpers {
typedef double Numeric;
typedef double Time;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 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 exact_cubic<Time, Numeric, true, Point, T_Point> exact_cubic_t;
typedef exact_cubic_t::spline_constraints spline_constraints_t;
typedef exact_cubic_t::t_spline_t t_spline_t;
typedef exact_cubic_t::spline_t spline_t;
 
Waypoint compute_offset(const Waypoint& source, const Point& normal,
                        const Numeric offset, const Time time_offset) {
  Numeric norm = normal.norm();
  if (norm < 0.) {
    throw std::runtime_error("Norm of normal is less than 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) {
  Numeric norm = normal.norm();
  if (norm < 0.) {
    throw std::runtime_error("Norm of normal is less than 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*/) {
  spline_constraints_t constraints(end_spline.dim());
  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);
  exact_cubic_t splines(waypoints.begin(), waypoints.end(), constraints);
  splines.add_curve(end_spline);
  return new exact_cubic_t(splines);
}
}  // namespace helpers
}  // namespace ndcurves
#endif  //_CLASS_EFFECTORSPLINE