include/a/trajbell.h

   1 /*!
   2  @file trajbell.h
   3  @brief bell-shaped velocity trajectory
   4  @details Trajectory Planning for Automatic Machines and Robots
   5 */
   6
   7 #ifndef LIBA_TRAJBELL_H
   8 #define LIBA_TRAJBELL_H
   9
  10 #include "a.h"
  11
  12 /*!
  13  @ingroup A
  14  @addtogroup A_TRAJBELL bell-shaped velocity trajectory
  15  @{
  16 */
  17
  18 typedef struct a_trajbell a_trajbell;
  19
  20 #if defined(__cplusplus)
  21 extern "C" {
  22 #endif /* __cplusplus */
  23
  24 /*!
  25  @brief generate for bell-shaped velocity trajectory
  26  @details If \f$p_0>p_1\f$, then \f{aligned}{p_0=-p_0\\p_1=-p_1\\v_0=-v_0\\v_1=-v_1\f}
  27  1. If \f$(v_m-v_0)j_m<a_m^2\f$, the maximum acceleration is not reached,
  28  then \f{aligned}{T_{aj}&=\sqrt{\frac{v_m-v_0}{j_m}}\\T_a&=2T_{aj}\\a_m&=+j_mT_{aj}\f}
  29  else \f{aligned}{T_{aj}&=\frac{a_m}{j_m}\\T_a&=T_{aj}+\frac{v_m-v_0}{a_m}\\a_m&=+a_m\f}
  30  2. If \f$(v_m-v_1)j_m<a_m^2\f$, the maximum deceleration is not reached,
  31  then \f{aligned}{T_{dj}&=\sqrt{\frac{v_m-v_1}{j_m}}\\T_d&=2T_{dj}\\d_m&=-j_mT_{dj}\f}
  32  else \f{aligned}{T_{dj}&=\frac{a_m}{j_m}\\T_d&=T_{dj}+\frac{v_m-v_1}{a_m}\\d_m&=-a_m\f}
  33  3. Calculate the constant velocity phase \f[T_v=\frac{p_1-p_0}{v_m}
  34  -\frac{T_a}{2}\left(1+\frac{v_0}{v_m}\right)-\frac{T_d}{2}\left(1+\frac{v_1}{v_m}\right)\f]
  35  If \f$T_v>0\f$, then there is a constant velocity phase and the phase calculation ends.
  36  4. If \f$T_v<=0\f$, then \f$T_v=0\f$, \f{aligned}{T_{aj}&=T_{dj}=T_{j}=\frac{a_m}{j_m}\\
  37  T_a&=\cfrac{\frac{a_m^2}{j_m}+\sqrt\Delta-2v_0}{2a_m}\\T_d&=\cfrac{\frac{a_m^2}{j_m}+\sqrt\Delta-2v_1}{2a_m}\\
  38  \Delta&=\frac{a_m^4}{j_m^2}+2(v_0^2+v_1^2)+a_m\left(4(p_1-p_0)-2\frac{a_m}{j_m}(v_0+v_1)\right)\f}<br>
  39  a. If \f$T_a<0\f$, there is only deceleration phase, then \f$T_a=0\f$,\f$T_{aj}=0\f$,
  40  \f{aligned}{T_d&=2\cfrac{p_1-p_0}{v_1+v_0}\\
  41  T_{dj}&=\frac{j_m(p_1-p_0)-\sqrt{j_m(j_m(p_1-p_0)^2+(v_1-v_0)(v_1+v_0)^2)}}{j_m(v_1+v_0 )}\f}
  42  b. If \f$T_d<0\f$, there is only acceleration phase, then \f$T_d=0\f$,\f$T_{dj}=0\f$,
  43  \f{aligned}{T_a&=2\cfrac{p_1-p_0}{v_1+v_0}\\
  44  T_{aj}&=\frac{j_m(p_1-p_0)-\sqrt{j_m(j_m(p_1-p_0)^2+(v_0-v_1)(v_1+v_0)^2)}}{j_m(v_1+v_0 )}\f}
  45  c. If \f$T_a\ge2T_j\f$, \f$T_d\ge2T_j\f$, then
  46  \f{aligned}{a_m&=+j_mT_{aj}\\d_m&=-j_mT_{dj}\\v_m&=v_0+(T_a-T_{aj})a_m=v_1-(T_d-T_{dj})d_m\f}
  47  d. If none of the above conditions are met, let \f$a_m=\alpha a_m, 0<\alpha<1\f$, and then repeat step 4.
  48  5. Finally, use formulas to calculate position, velocity and acceleration.
  49  @param[in,out] ctx points to an instance of bell-shaped velocity trajectory
  50  @param[in] jm defines the maximum jerk during system operation
  51  @param[in] am defines the maximum acceleration during system operation
  52  @param[in] vm defines the maximum velocity during system operation
  53  @param[in] p0 defines the initial position
  54  @param[in] p1 defines the final position
  55  @param[in] v0 defines the initial velocity
  56  @param[in] v1 defines the final velocity
  57  @return total duration
  58 */
  59 A_EXTERN a_float a_trajbell_gen(a_trajbell *ctx, a_float jm, a_float am, a_float vm,
  60                                 a_float p0, a_float p1, a_float v0, a_float v1);
  61
  62 /*!
  63  @brief calculate position for bell-shaped velocity trajectory
  64  \f[
  65   p(t)=\begin{cases}
  66   p_0+v_0t+j_m\cfrac{t^3}{6},&t\in[0,T_{aj}]\\
  67   p_0+v_0t+\cfrac{a_m}{6}(3t^3-3tT_{aj}+T_{aj}^2),&t\in[T_{aj},T_a-T_{aj}]\\
  68   p_0+(v_m+v_0)\cfrac{T_a}{2}-v_m(T_a-t)+j_m\cfrac{(T_a-t)^3}{6},&t\in[T_a-T_{aj},T_a]\\
  69   p_0+(v_m+v_0)\cfrac{T_a}{2}+v_m(t-T_a),&t\in[T_a,T_a+T_v]\\
  70   p_1-(v_m+v_1)\cfrac{T_d}{2}+v_m(t-T+T_d)-j_m\cfrac{(t-T+T_d)^3}{6},&t\in[T-T_d,T-T_d+T_{dj}]\\
  71   p_1-(v_m+v_1)\cfrac{T_d}{2}+v_m(t-T+T_d)\\
  72   \quad+\cfrac{d_m}{6}\left(3(t-T+T_d)^2-3(t-T+T_d)T_{dj}+T_{dj}^2\right),&t\in[T-T_d+T_{dj},T-T_{dj}]\\
  73   p_1-v_1(T-t)-j_m\cfrac{(T-t)^3}{6},&t\in[T-T_{dj},T]
  74   \end{cases}
  75  \f]
  76  @param[in] ctx points to an instance of bell-shaped velocity trajectory
  77  @param[in] dt difference between current time and initial time
  78  @return position output
  79 */
  80 A_EXTERN a_float a_trajbell_pos(a_trajbell const *ctx, a_float dt);
  81
  82 /*!
  83  @brief calculate velocity for bell-shaped velocity trajectory
  84  \f[
  85   \dot{p}(t)=\begin{cases}
  86   v_0+j_m\frac{t^2}{2},&t\in[0,T_{aj}]\\
  87   v_0+a_m(t-\cfrac{T_{aj}}{2}),&t\in[T_{aj},T_a-T_{aj}]\\
  88   v_m-j_m\cfrac{(T_a-t)^2}{2},&t\in[T_a-T_{aj},T_a]\\
  89   v_m,&t\in[T_a,T_a+T_v]\\
  90   v_m-j_m\cfrac{(t-T+T_d)^2}{2},&t\in[T-T_d,T-T_d+T_{dj}]\\
  91   v_m+d_m(t-T+T_d-\cfrac{T_{dj}}{2}),&t\in[T-T_d+T_{dj},T-T_{dj}]\\
  92   v_1+j_m\cfrac{(T-t)^2}{2},&t\in[T-T_{dj},T]
  93   \end{cases}
  94  \f]
  95  @param[in] ctx points to an instance of bell-shaped velocity trajectory
  96  @param[in] dt difference between current time and initial time
  97  @return velocity output
  98 */
  99 A_EXTERN a_float a_trajbell_vel(a_trajbell const *ctx, a_float dt);
 100
 101 /*!
 102  @brief calculate acceleration for bell-shaped velocity trajectory
 103  \f[
 104   \ddot{p}(t)=\begin{cases}
 105   j_mt,&t\in[0,T_{aj}]\\
 106   j_mT_{aj}=a_m,&t\in[T_{aj},T_a-T_{aj}]\\
 107   j_m(T_a-t),&t\in[T_a-T_{aj},T_a]\\
 108   0,&t\in[T_a,T_a+T_v]\\
 109   -j_m(t-T+T_d),&t\in[T-T_d,T-T_d+T_{dj}]\\
 110   -j_mT_{dj}=d_m,&t\in[T-T_d+T_{dj},T-T_{dj}]\\
 111   -j_m(T-t),&t\in[T-T_{dj},T]
 112   \end{cases}
 113  \f]
 114  @param[in] ctx points to an instance of bell-shaped velocity trajectory
 115  @param[in] dt difference between current time and initial time
 116  @return acceleration output
 117 */
 118 A_EXTERN a_float a_trajbell_acc(a_trajbell const *ctx, a_float dt);
 119
 120 /*!
 121  @brief calculate jerk for bell-shaped velocity trajectory
 122  \f[
 123   p^{(3)}(t)=\begin{cases}
 124   j_m,&t\in[0,T_{aj}]\\
 125   0,&t\in[T_{aj},T_a-T_{aj}]\\
 126   -j_m,&t\in[T_a-T_{aj},T_a]\\
 127   0,&t\in[T_a,T_a+T_v]\\
 128   -j_m,&t\in[T-T_d,T-T_d+T_{dj}]\\
 129   0,&t\in[T-T_d+T_{dj},T-T_{dj}]\\
 130   j_m,&t\in[T-T_{dj},T]
 131   \end{cases}
 132  \f]
 133  @param[in] ctx points to an instance of bell-shaped velocity trajectory
 134  @param[in] dt difference between current time and initial time
 135  @return jerk output
 136 */
 137 A_EXTERN a_float a_trajbell_jer(a_trajbell const *ctx, a_float dt);
 138
 139 #if defined(__cplusplus)
 140 } /* extern "C" */
 141 namespace a
 142 {
 143 typedef struct a_trajbell trajbell;
 144 } /* namespace a */
 145 #endif /* __cplusplus */
 146
 147 /*!
 148  @brief instance structure for bell-shaped velocity trajectory
 149 */
 150 struct a_trajbell
 151 {
 152     a_float t; //!< total duration
 153     a_float tv; //!< constant velocity phase
 154     a_float ta; //!< acceleration phase
 155     a_float td; //!< deceleration phase
 156     a_float taj; //!< time-interval in which the jerk is constant (j max or j min ) during the acceleration phase
 157     a_float tdj; //!< time-interval in which the jerk is constant (j max or j min ) during the deceleration phase
 158     a_float p0; //!< initial position
 159     a_float p1; //!< final position
 160     a_float v0; //!< initial velocity
 161     a_float v1; //!< final velocity
 162     a_float vm; //!< maximum velocity
 163     a_float jm; //!< maximum jerk
 164     a_float am; //!< maximum acceleration
 165     a_float dm; //!< maximum deceleration
 166 #if defined(__cplusplus)
 167     A_INLINE a_float gen(a_float _jm, a_float _am, a_float _vm, a_float _p0, a_float _p1,
 168                          a_float _v0 = 0, a_float _v1 = 0)
 169     {
 170         return a_trajbell_gen(this, _jm, _am, _vm, _p0, _p1, _v0, _v1);
 171     }
 172     A_INLINE a_float pos(a_float dt) const
 173     {
 174         return a_trajbell_pos(this, dt);
 175     }
 176     A_INLINE a_float vel(a_float dt) const
 177     {
 178         return a_trajbell_vel(this, dt);
 179     }
 180     A_INLINE a_float acc(a_float dt) const
 181     {
 182         return a_trajbell_acc(this, dt);
 183     }
 184     A_INLINE a_float jer(a_float dt) const
 185     {
 186         return a_trajbell_jer(this, dt);
 187     }
 188 #endif /* __cplusplus */
 189 };
 190
 191 /*! @} A_TRAJBELL */
 192
 193 #endif /* a/trajbell.h */