include/a/polytraj5.h

   1 /*!
   2  @file polytraj5.h
   3  @brief quintic polynomial trajectory
   4  @details Trajectory Planning for Automatic Machines and Robots
   5 */
   6
   7 #ifndef LIBA_POLYTRAJ5_H
   8 #define LIBA_POLYTRAJ5_H
   9
  10 #include "a.h"
  11
  12 /*!
  13  @ingroup A
  14  @addtogroup A_POLYTRAJ5 quintic polynomial trajectory
  15  @{
  16 */
  17
  18 #if !defined A_POLYTRAJ5
  19 #define A_POLYTRAJ5 3
  20 #endif /* A_POLYTRAJ5 */
  21 typedef struct a_polytraj5 a_polytraj5;
  22
  23 #if defined(__cplusplus)
  24 extern "C" {
  25 #endif /* __cplusplus */
  26
  27 /*!
  28  @brief generate for quintic polynomial trajectory
  29  \f{aligned}{
  30   \left\{\begin{array}{l}
  31   t=t_{1}-t_{0}\\
  32   q=q_{1}-q_{0}\\
  33   k_{0}=q_{0}\\
  34   k_{1}=v_{0}\\
  35   k_{2}=\cfrac{a_{0}}{2}\\
  36   k_{3}=\cfrac{\left(a_{1}-3\,a_{0}\right)\,t^2+\left(-12\,v_{0}-8\,v_{1}\right)\,t+20\,q}{2\,t^3}\\
  37   k_{4}=\cfrac{\left(3\,a_{0}-2\,a_{1}\right)\,t^2+\left(16\,v_{0}+14\,v_{1}\right)\,t-30\,q}{2\,t^4}\\
  38   k_{5}=\cfrac{\left(a_{1}-a_{0}\right)\,t^2+\left(-6\,v_{0}-6\,v_{1}\right)\,t+12\,q}{2\,t^5}
  39   \end{array}\right.
  40  \f}
  41  @param[in,out] ctx points to an instance of quintic polynomial trajectory
  42  @param[in] t0 time for source
  43  @param[in] t1 time for target
  44  @param[in] q0 position for source
  45  @param[in] q1 position for target
  46  @param[in] v0 velocity for source
  47  @param[in] v1 velocity for target
  48  @param[in] a0 acceleration for source
  49  @param[in] a1 acceleration for target
  50 */
  51 A_EXTERN void a_polytraj5_gen(a_polytraj5 *ctx,
  52                               a_float t0, a_float t1,
  53                               a_float q0, a_float q1,
  54                               a_float v0, a_float v1,
  55                               a_float a0, a_float a1);
  56 A_EXTERN void a_polytraj5_gen0(a_polytraj5 *ctx,
  57                                a_float t0, a_float t1,
  58                                a_float q0, a_float q1,
  59                                a_float v0, a_float v1,
  60                                a_float a0, a_float a1);
  61 #if defined(A_POLYTRAJ5) && (A_POLYTRAJ5 + 0 > 1)
  62 A_EXTERN void a_polytraj5_gen1(a_polytraj5 *ctx);
  63 #endif /* A_POLYTRAJ5 */
  64 #if defined(A_POLYTRAJ5) && (A_POLYTRAJ5 + 0 > 2)
  65 A_EXTERN void a_polytraj5_gen2(a_polytraj5 *ctx);
  66 #endif /* A_POLYTRAJ5 */
  67
  68 /*!
  69  @brief calculate for quintic polynomial trajectory position
  70  \f{aligned}{
  71   \begin{array}{l}
  72   q(t)=k_{0}+k_{1}\left(t-t_{0}\right)+k_{2}\left(t-t_{0}\right)^{2}+k_{3}\left(t-t_{0}\right)^{3}+k_{4}\left(t-t_{0}\right)^{4}+k_{5}\left(t-t_{0}\right)^{5}\\
  73   \end{array}
  74  \f}
  75  @param[in] ctx points to an instance of quintic polynomial trajectory
  76  @param[in] dt difference between current time and initial time
  77  @return position output
  78 */
  79 A_EXTERN a_float a_polytraj5_pos(a_polytraj5 const *ctx, a_float dt);
  80
  81 #if defined(A_POLYTRAJ5) && (A_POLYTRAJ5 + 0 > 1)
  82 /*!
  83  @brief calculate for quintic polynomial trajectory velocity
  84  \f{aligned}{
  85   \begin{array}{l}
  86   \dot{q}(t)=k_{1}+2 k_{2}\left(t-t_{0}\right)+3 k_{3}\left(t-t_{0}\right)^{2}+4 k_{4}\left(t-t_{0}\right)^{3}+5 k_{5}\left(t-t_{0}\right)^{4}\\
  87   \end{array}
  88  \f}
  89  @param[in] ctx points to an instance of quintic polynomial trajectory
  90  @param[in] dt difference between current time and initial time
  91  @return velocity output
  92 */
  93 A_EXTERN a_float a_polytraj5_vel(a_polytraj5 const *ctx, a_float dt);
  94 #endif /* A_POLYTRAJ5 */
  95
  96 #if defined(A_POLYTRAJ5) && (A_POLYTRAJ5 + 0 > 2)
  97 /*!
  98  @brief calculate for quintic polynomial trajectory acceleration
  99  \f{aligned}{
 100   \begin{array}{l}
 101   \ddot{q}(t)=2 k_{2}+6 k_{3}\left(t-t_{0}\right)+12 k_{4}\left(t-t_{0}\right)^{2}+20 k_{5}\left(t-t_{0}\right)^{3}
 102   \end{array}
 103  \f}
 104  @param[in] ctx points to an instance of quintic polynomial trajectory
 105  @param[in] dt difference between current time and initial time
 106  @return acceleration output
 107 */
 108 A_EXTERN a_float a_polytraj5_acc(a_polytraj5 const *ctx, a_float dt);
 109 #endif /* A_POLYTRAJ5 */
 110
 111 #if defined(__cplusplus)
 112 } /* extern "C" */
 113 namespace a
 114 {
 115 typedef struct a_polytraj5 polytraj5;
 116 } /* namespace a */
 117 #endif /* __cplusplus */
 118
 119 /*!
 120  @brief instance structure for quintic polynomial trajectory
 121  \f{aligned}{
 122   \begin{array}{l}
 123   q(t)=k_{0}+k_{1}\left(t-t_{0}\right)+k_{2}\left(t-t_{0}\right)^{2}+k_{3}\left(t-t_{0}\right)^{3}+k_{4}\left(t-t_{0}\right)^{4}+k_{5}\left(t-t_{0}\right)^{5}\\
 124   \dot{q}(t)=k_{1}+2 k_{2}\left(t-t_{0}\right)+3 k_{3}\left(t-t_{0}\right)^{2}+4 k_{4}\left(t-t_{0}\right)^{3}+5 k_{5}\left(t-t_{0}\right)^{4}\\
 125   \ddot{q}(t)=2 k_{2}+6 k_{3}\left(t-t_{0}\right)+12 k_{4}\left(t-t_{0}\right)^{2}+20 k_{5}\left(t-t_{0}\right)^{3}
 126   \end{array}
 127  \f}
 128 */
 129 struct a_polytraj5
 130 {
 131     a_float q[6]; //!< coefficients of position
 132 #if defined(A_POLYTRAJ5) && (A_POLYTRAJ5 + 0 > 1)
 133     a_float v[5]; //!< coefficients of velocity
 134 #endif /* A_POLYTRAJ5 */
 135 #if defined(A_POLYTRAJ5) && (A_POLYTRAJ5 + 0 > 2)
 136     a_float a[4]; //!< coefficients of acceleration
 137 #endif /* A_POLYTRAJ5 */
 138 #if defined(__cplusplus)
 139     A_INLINE void gen(a_float t0, a_float t1,
 140                       a_float q0, a_float q1,
 141                       a_float v0 = 0, a_float v1 = 0,
 142                       a_float a0 = 0, a_float a1 = 0)
 143     {
 144         a_polytraj5_gen(this, t0, t1, q0, q1, v0, v1, a0, a1);
 145     }
 146     A_INLINE void gen0(a_float t0, a_float t1,
 147                        a_float q0, a_float q1,
 148                        a_float v0 = 0, a_float v1 = 0,
 149                        a_float a0 = 0, a_float a1 = 0)
 150     {
 151         a_polytraj5_gen0(this, t0, t1, q0, q1, v0, v1, a0, a1);
 152     }
 153     A_INLINE a_float pos(a_float dt)
 154     {
 155         return a_polytraj5_pos(this, dt);
 156     }
 157 #if defined(A_POLYTRAJ5) && (A_POLYTRAJ5 + 0 > 1)
 158     A_INLINE void gen1() { a_polytraj5_gen1(this); }
 159     A_INLINE a_float vel(a_float dt)
 160     {
 161         return a_polytraj5_vel(this, dt);
 162     }
 163 #endif /* A_POLYTRAJ5 */
 164 #if defined(A_POLYTRAJ5) && (A_POLYTRAJ5 + 0 > 2)
 165     A_INLINE void gen2() { a_polytraj5_gen2(this); }
 166     A_INLINE a_float acc(a_float dt)
 167     {
 168         return a_polytraj5_acc(this, dt);
 169     }
 170 #endif /* A_POLYTRAJ5 */
 171 #endif /* __cplusplus */
 172 };
 173
 174 /*! @} A_POLYTRAJ5 */
 175
 176 #endif /* a/polytraj5.h */