include/a/trajtrap.h

   1 /*!
   2  @file trajtrap.h
   3  @brief trapezoidal velocity trajectory
   4  @details Trajectory Planning for Automatic Machines and Robots
   5 */
   6
   7 #ifndef LIBA_TRAJTRAP_H
   8 #define LIBA_TRAJTRAP_H
   9
  10 #include "a.h"
  11
  12 /*!
  13  @ingroup A
  14  @addtogroup A_TRAJTRAP trapezoidal velocity trajectory
  15  @{
  16 */
  17
  18 typedef struct a_trajtrap a_trajtrap;
  19
  20 #if defined(__cplusplus)
  21 extern "C" {
  22 #endif /* __cplusplus */
  23
  24 /*!
  25  @brief generate for trapezoidal velocity trajectory
  26  @details Assuming that there is no constant velocity phase, but only acceleration and deceleration phases,
  27  the maximum velocity in the motion is \f$v\f$, then we have \f[\frac{v^2-v_0^2}{2a}+\frac{v_1^2-v^2}{2d}=p_1-p_0\f]
  28  Solving for the maximum velocity is \f[|v|=\sqrt{\frac{av_1^2-dv_0^2-2ad(p_1-p_0)}{a-d}}\f]
  29  1. If \f$|v|>|v_m|\f$, then there is a constant velocity phase.
  30  \f[p_a=p_0+v_0T_a+\frac{1}{2}aT_a^2\f] \f[a_d=p_1-v_cT_d-\frac{1}{2}dT_d^2\f]
  31  \f{cases}{T_a=\cfrac{v_m-v_0}{a}\\T_c=\cfrac{p_d-p_a}{v_c}\\T_d=\cfrac{v_1-v_m}{d}\f}
  32  2. Otherwise there is no constant velocity phase.<br>
  33  a. If \f$|v_0|<|v|\le|v_1|\f$, there is only an acceleration phase.
  34  \f[|v_1|=\sqrt{v_0^2+2a(p_1-p_0)}\f] \f{cases}{T_a=\cfrac{v_1-v_0}{a}\\T_c=0\\T_d=0\f}
  35  b. If \f$|v_0|\ge|v|>|v_1|\f$, there is only a deceleration phase.
  36  \f[|v_1|=\sqrt{v_0^2+2d(p_1-p_0)}\f] \f{cases}{T_a=0\\T_c=0\\T_d=\cfrac{v_1-v_0}{d}\f}
  37  c. If \f$|v|>|v_0|\f$, \f$|v|>|v_1|\f$, then there are acceleration and deceleration phases.
  38  \f{cases}{T_a=\cfrac{v-v_0}{a}\\T_c=0\\T_d=\cfrac{v_1-v}{d}\f}
  39  3. Finally, the position and velocity are calculated using the formula.
  40  @param[in,out] ctx points to an instance of trapezoidal velocity trajectory
  41  @param[in] vm defines the maximum velocity during system operation
  42  @param[in] ac defines the acceleration before constant velocity
  43  @param[in] de defines the acceleration after constant velocity
  44  @param[in] p0 defines the initial position
  45  @param[in] p1 defines the final position
  46  @param[in] v0 defines the initial velocity
  47  @param[in] v1 defines the final velocity
  48  @return total duration
  49 */
  50 A_EXTERN a_float a_trajtrap_gen(a_trajtrap *ctx, a_float vm, a_float ac, a_float de,
  51                                 a_float p0, a_float p1, a_float v0, a_float v1);
  52
  53 /*!
  54  @brief calculate position for trapezoidal velocity trajectory
  55  \f[
  56   p(t)=\begin{cases}v_0+\frac{1}{2}at^2,&t\in[0,t_a)\\p_a+v_c(t-t_a),&t\in[t_a,t_d)\\
  57   p_d+v_c(t-t_d)+\frac{1}{2}d(t-t_d)^2,&t\in[t_d,T]\end{cases}
  58  \f]
  59  @param[in] ctx points to an instance of trapezoidal velocity trajectory
  60  @param[in] dt difference between current time and initial time
  61  @return position output
  62 */
  63 A_EXTERN a_float a_trajtrap_pos(a_trajtrap const *ctx, a_float dt);
  64
  65 /*!
  66  @brief calculate velocity for trapezoidal velocity trajectory
  67  \f[
  68   \dot{p}(t)=\begin{cases}v_0+at,&t\in[0,t_a)\\v_c,&t\in[t_a,t_d)\\v_c+d(t-t_d),&t\in[t_d,T]\end{cases}
  69  \f]
  70  @param[in] ctx points to an instance of trapezoidal velocity trajectory
  71  @param[in] dt difference between current time and initial time
  72  @return velocity output
  73 */
  74 A_EXTERN a_float a_trajtrap_vel(a_trajtrap const *ctx, a_float dt);
  75
  76 /*!
  77  @brief calculate acceleration for trapezoidal velocity trajectory
  78  \f[
  79   \ddot{p}(t)=\begin{cases}a,&t\in[0,t_a)\\0,&t\in[t_a,t_d)\\d,&t\in[t_d,T]\end{cases}
  80  \f]
  81  @param[in] ctx points to an instance of trapezoidal velocity trajectory
  82  @param[in] dt difference between current time and initial time
  83  @return acceleration output
  84 */
  85 A_EXTERN a_float a_trajtrap_acc(a_trajtrap const *ctx, a_float dt);
  86
  87 #if defined(__cplusplus)
  88 } /* extern "C" */
  89 namespace a
  90 {
  91 typedef struct a_trajtrap trajtrap;
  92 } /* namespace a */
  93 #endif /* __cplusplus */
  94
  95 /*!
  96  @brief instance structure for trapezoidal velocity trajectory
  97 */
  98 struct a_trajtrap
  99 {
 100     a_float t; //!< total duration
 101     a_float p0; //!< initial position
 102     a_float p1; //!< final position
 103     a_float v0; //!< initial velocity
 104     a_float v1; //!< final velocity
 105     a_float vc; //!< constant velocity
 106     a_float ta; //!< time before constant velocity
 107     a_float td; //!< time after constant velocity
 108     a_float pa; //!< position before constant velocity
 109     a_float pd; //!< position after constant velocity
 110     a_float ac; //!< acceleration before constant velocity
 111     a_float de; //!< acceleration after constant velocity
 112 #if defined(__cplusplus)
 113     A_INLINE a_float gen(a_float vm, a_float _ac, a_float _de, a_float _p0, a_float _p1,
 114                          a_float _v0 = 0, a_float _v1 = 0)
 115     {
 116         return a_trajtrap_gen(this, vm, _ac, _de, _p0, _p1, _v0, _v1);
 117     }
 118     A_INLINE a_float pos(a_float dt)
 119     {
 120         return a_trajtrap_pos(this, dt);
 121     }
 122     A_INLINE a_float vel(a_float dt)
 123     {
 124         return a_trajtrap_vel(this, dt);
 125     }
 126     A_INLINE a_float acc(a_float dt)
 127     {
 128         return a_trajtrap_acc(this, dt);
 129     }
 130 #endif /* __cplusplus */
 131 };
 132
 133 /*! @} A_TRAJTRAP */
 134
 135 #endif /* a/trajtrap.h */