include/a/trajpoly5.h

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