Template Class Path4D

Class Documentation

template<int _N>
class mav_planning_utils::path_planning::Path4D

Public Types

enum [anonymous]

Values:

enumerator N
typedef Segment<N> SegmentType
typedef SegmentType::Vector SegmentVector
typedef Aligned<std::vector, mav_planning_utils::path_planning::Vertex4D> Vertex4DList

Public Functions

inline Path4D()

standard constructor, doesn’t initialize anything

inline Path4D(const SegmentVector &sx, const SegmentVector &sy, const SegmentVector &sz, const SegmentVector &syaw)

initializes the internal segments with the arguments, so that sample() can be used

inline bool optimize(const std::vector<Vertex4D, Eigen::aligned_allocator<Vertex4D>> &vertices, int continuity, double tol = 1e12)

plans a path through the points given in vertices.

For obtaining path data

See

void sample(), getSegmentsX(), getVertices()

Parameters

  • vertices – Vertices to plan the path through

  • continuity – Defines up to which derivative the path should be continuous. Use DerivativeP for that.

  • tol – Tolerance for Pseudo Inverse

inline std::vector<double> computeTimeScales(const Vertex1D &max_p, const Vertex1D &max_yaw)

computes a scaling factor by which every segment time has to be scaled in order to stay within the given bounds

Parameters

  • max_p – Vertex with “constraints” for the motion derivatives. At least a constraint for velocity is required.

  • max_yaw – Vertex with “constraints” for the motion derivatives. At least a constraint for angular velocity is required.

Returns

vector with time scaling factors for each segment

inline bool optimizeWithTime(const std::vector<Vertex4D, Eigen::aligned_allocator<Vertex4D>> &vertices, int continuity, const Vertex1D &max_p, const Vertex1D &max_yaw, double time_multiplier = 0.1, double tol = 1e12)

plans a path through the points given in vertices and optimized the segment times according to the given limits in max_p and max_yaw The time_to_next members in vertices are ignored.

In a first step, rough guesses of the segment times are made based on the maximum velocity and a first path is planned. Then, it gets checked for limit violations, segment times are scaled accordingly and the path gets re-planned based on the new times. For obtaining path data

See

sample(), getSegmentsX(), getVertices()

Parameters

  • vertices – Vertices to plan the path through

  • continuity – Defines up to which derivative the path should be continuous. Use DerivativeP for that.

  • max_p – Vertex with “constraints” for the motion derivatives. At least a constraint for velocity is required.

  • max_yaw – Vertex with “constraints” for the motion derivatives. At least a constraint for angular velocity is required.

  • time_multiplier – If not 0, the optimized segment times get ceiled to a multiple of time_multiplier. To ease sampling, it’s useful to set this to the same value as dt in sample().

  • tol – Tolerance for Pseudo Inverse

template<int n_p, int n_y>
inline void sample(typename Motion4D<n_p, n_y>::Vector *result, double dt, Eigen::VectorXd *timestamps_seconds) const

samples the whole path obtained by optimize() or optimizeWithTime() in steps of dt

Template Parameters

  • n_p – number of derivatives to sample for position. Implicitly obtained by result

  • n_y – number of derivatives to sample for position. Implicitly obtained by result

Parameters

  • result – [out] vector with a Motion4D element for each sample

  • dt – [in] timestep to sample

inline const Vertex4DList &getVertices() const

returns the time-optimized vertices after a optimizeWithTime() call

inline Vertex4DList *getVerticesPtr()
inline const SegmentVector &getSegmentsX() const

returns a vector of polynomial segments for the x-axis

inline SegmentVector *getSegmentsXPtr()
inline const SegmentVector &getSegmentsY() const

returns a vector of polynomial segments for the y-axis

inline SegmentVector *getSegmentsYPtr()
inline const SegmentVector &getSegmentsZ() const

returns a vector of polynomial segments for the z-axis

inline SegmentVector *getSegmentsZPtr()
inline const SegmentVector &getSegmentsYaw() const

returns a vector of polynomial segments for yaw

inline SegmentVector *getSegmentsYawPtr()