Trajectory Manager

In this section, we explain how the define parametric paths or trajectories to be followed by the vehicle.

0. Mathematical Background

Consider an arbitrary path/trajectory parameterized by a parameter \(\gamma(t)\) given by the Trajectory Manager. Consider the desired position to be given by \(p_d(\gamma) \in \mathbb{R}^3\), such that:

  1. The desired velocity to be tracked in the inertial frame is given by

    \[\dot{p}_d(\gamma) = \frac{\partial p_d(\gamma)}{\partial \gamma}\dot{\gamma}\]

  2. The desired acceleration to be tracked in the inertial frame is given by

    \[\begin{split}\ddot{p}_d(\gamma) &=\frac{d}{dt} \left(\frac{\partial p_d(\gamma)}{\partial \gamma}\dot{\gamma}\right) \\ &= \frac{d}{dt} \left(\frac{\partial p_d(\gamma)}{\partial \gamma}\right)\dot{\gamma} + \frac{\partial p_d(\gamma)}{\partial \gamma} \ddot{\gamma}\\ &= \frac{\partial^2 p_d(\gamma)}{\partial \gamma^2}\dot{\gamma}^2 + \frac{\partial p_d(\gamma)}{\partial \gamma} \ddot{\gamma}\end{split}\]

  3. The desired jerk to be tracked in the inertial frame is given by

    \[\begin{split}\dddot{p}_d(\gamma) &= \frac{d}{dt}\left(\frac{\partial^2 p_d(\gamma)}{\partial \gamma^2}\dot{\gamma}^2 + \frac{\partial p_d(\gamma)}{\partial \gamma} \ddot{\gamma}\right) \\ &= \frac{d}{dt}\left(\frac{\partial^2 p_d(\gamma)}{\partial \gamma^2}\dot{\gamma}^2\right) + \frac{d}{dt}\left(\frac{\partial p_d(\gamma)}{\partial \gamma} \ddot{\gamma}\right) \\ &= \frac{\partial^3 p_d(\gamma)}{\partial \gamma^3}\dot{\gamma}^3 + 2\frac{\partial^2 p_d(\gamma)}{\partial \gamma^2}\dot{\gamma}\ddot{\gamma} + \frac{\partial^2 p_d(\gamma)}{\partial \gamma^2} \dot{\gamma}\ddot{\gamma} + \frac{\partial p_d(\gamma)}{\partial \gamma} \dddot{\gamma} \\ &= \frac{\partial^3 p_d(\gamma)}{\partial \gamma^3}\dot{\gamma}^3 + 3\frac{\partial^2 p_d(\gamma)}{\partial \gamma^2}\dot{\gamma}\ddot{\gamma} + \frac{\partial p_d(\gamma)}{\partial \gamma} \dddot{\gamma}\end{split}\]

1. Definition of the Trajectory Interface

  1namespace autopilot {
  2
  3/**
  4 * @brief The Trajectory class is an abstract class that defines the interface
  5 * to create a trajectory section that can be followed by a trajectory or path following controller
  6*/
  7class StaticTrajectory {
  8
  9public:
 10
 11    using SharedPtr = std::shared_ptr<StaticTrajectory>;
 12    using UniquePtr = std::unique_ptr<StaticTrajectory>;
 13    using WeakPtr = std::weak_ptr<StaticTrajectory>;
 14
 15    /**
 16     * @brief This function returns the desired position of the vehicle at a given time
 17     * provided the parameter gamma which paramaterizes the trajectory
 18     * @param gamma The parameter that paramaterizes the trajectory
 19     * @return The desired position of the vehicle at a given time (Eigen::Vector3d)
 20     */ 
 21    virtual Eigen::Vector3d pd(const double gamma) const = 0;
 22
 23    /**
 24     * @brief This function returns the desired velocity of the vehicle at a given time
 25     * provided the parameter gamma which paramaterizes the trajectory
 26     * @param gamma The parameter that paramaterizes the trajectory
 27     * @return The desired velocity of the vehicle at a given time (Eigen::Vector3d)
 28     */
 29    virtual Eigen::Vector3d d_pd(const double gamma) const { return Eigen::Vector3d::Zero(); }
 30
 31    /**
 32     * @brief This function returns the desired acceleration of the vehicle at a given time
 33     * provided the parameter gamma which paramaterizes the trajectory
 34     * @param gamma The parameter that paramaterizes the trajectory
 35     * @return The desired acceleration of the vehicle at a given time (Eigen::Vector3d)
 36     */
 37    virtual Eigen::Vector3d d2_pd(const double gamma) const { return Eigen::Vector3d::Zero(); }
 38
 39    /**
 40     * @brief This function returns the desired jerk of the vehicle at a given time
 41     * provided the parameter gamma which paramaterizes the trajectory
 42     * @param gamma The parameter that paramaterizes the trajectory
 43     * @return The desired jerk of the vehicle at a given time (Eigen::Vector3d)
 44     */
 45    virtual Eigen::Vector3d d3_pd(const double gamma) const { return Eigen::Vector3d::Zero(); }
 46
 47    /**
 48     * @brief This function returns the desired snap of the vehicle at a given time
 49     * provided the parameter gamma which paramaterizes the trajectory
 50     * @param gamma The parameter that paramaterizes the trajectory
 51     * @return The desired snap of the vehicle at a given time (Eigen::Vector3d)
 52     */
 53    virtual Eigen::Vector3d d4_pd(const double gamma) const { return Eigen::Vector3d::Zero(); }
 54
 55    /**
 56     * @brief This function returns the desired yaw angle (in radians) of the vehicle at a given time
 57     * provided the parameter gamma which paramaterizes the trajectory
 58     * @param gamma The parameter that paramaterizes the trajectory
 59     * @return The desired yaw angle of the vehicle at a given time (radians)
 60     */
 61    virtual double yaw(const double gamma) const { return 0.0; }
 62
 63    /**
 64     * @brief This function returns the desired yaw rate (in radians/s) of the vehicle at a given time
 65     * provided the parameter gamma which paramaterizes the trajectory
 66     * @param gamma The parameter that paramaterizes the trajectory
 67     * @return The desired yaw rate of the vehicle at a given time (radians/s)
 68     */
 69    virtual double d_yaw(const double gamma) const { return 0.0; }
 70
 71    /**
 72     * @brief This function returns the vehicle speed in m/s at any given position in the trajectory
 73     * @param gamma The parameter that paramaterizes the trajectory
 74     * @return The desired speed of the vehicle at a given time (double)
 75     */
 76    virtual double vehicle_speed(double gamma) const = 0;
 77
 78    /**
 79     * @brief This function returns the desired vehicle speed in the trajectory frame
 80     * (Note that this is not expressed in m/s as the trajectory can be normalized between 0-1)
 81     * @param gamma The parameter that paramaterizes the trajectory
 82     * @return The desired speed of the vehicle at a given time (double)
 83     */
 84    virtual double vd(const double gamma) const = 0;
 85
 86    /**
 87     * @brief This function returns the desired vehicle acceleration in the trajectory frame
 88     * (Note that this is not expressed in m/s^2 as the trajectory can be normalized between 0-1)
 89     * @param gamma The parameter that paramaterizes the trajectory
 90     * @return The desired acceleration of the vehicle at a given time (double)
 91     */
 92    virtual double d_vd(const double gamma) const { return 0.0; };
 93
 94    /**
 95     * @brief This function returns the desired vehicle jerk in the trajectory frame
 96     * (Note that this is not expressed in m/s^3 as the trajectory can be normalized between 0-1)
 97     * @param gamma The parameter that paramaterizes the trajectory
 98     * @return The desired jerk of the vehicle at a given time (double)
 99     */
100    virtual double d2_vd(const double gamma) const { return 0.0; };
101
102    /**
103     * @brief Getter for the minimum value of the variable that parameterizes the trajectory
104     */
105    inline double min_gamma() const { return min_gamma_; }
106
107    /**
108     * @brief Getter for the maximum value of the variable that parameterizes the trajectory
109     */
110    inline double max_gamma() const { return max_gamma_; }
111
112protected:
113
114    /**
115     * @brief Construct a new Trajectory object
116     */
117    StaticTrajectory(double min_gamma=0.0, double max_gamma=1.0) : 
118        min_gamma_(min_gamma), max_gamma_(max_gamma) {}
119
120    /**
121     * @brief The minimum and maximum value of the variable that parameterizes the trajectory
122     */
123    double min_gamma_;
124    double max_gamma_;
125
126};
127
128} // namespace autopilot

2. Provided Trajectories

TODO

3. Adding Custom Static Trajectories

TODO