include/a/trajpoly7.h

   1 /*!
   2  @file trajpoly7.h
   3  @brief hepta polynomial trajectory
   4  @details Trajectory Planning for Automatic Machines and Robots
   5 */
   6
   7 #ifndef LIBA_TRAJPOLY7_H
   8 #define LIBA_TRAJPOLY7_H
   9
  10 #include "a.h"
  11
  12 /*!
  13  @ingroup liba
  14  @addtogroup a_trajpoly7 hepta polynomial trajectory
  15  @{
  16 */
  17
  18 typedef struct a_trajpoly7 a_trajpoly7;
  19
  20 #if !defined A_TRAJPOLY7
  21 #define A_TRAJPOLY7 4
  22 #endif /* A_TRAJPOLY7 */
  23 #if defined(__cplusplus)
  24 extern "C" {
  25 #endif /* __cplusplus */
  26
  27 /*!
  28  @brief generate for hepta 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{j_{0}}{6}\\
  37   c_{4}=\cfrac{\left(-4\,j_{0}-j_{1}\right)\,t^3+\left(15\,a_{1}-30\,a_{0}\right)\,t^2+\left(-120\,v_{0}-90\,v_{1}\right)\,t+210\,p}{6\,t^4}\\
  38   c_{5}=\cfrac{\left(2\,j_{0}+j_{1}\right)\,t^3+\left(20\,a_{0}-14\,a_{1}\right)\,t^2+\left(90\,v_{0}+78\,v_{1}\right)\,t-168\,p}{2\,t^5}\\
  39   c_{6}=\cfrac{\left(-4\,j_{0}-3\,j_{1}\right)\,t^3+\left(39\,a_{1}-45\,a_{0}\right)\,t^2+\left(-216\,v_{0}-204\,v_{1}\right)\,t+420\,p}{6\,t^6}\\
  40   c_{7}=\cfrac{\left(j_{0}+j_{1}\right)\,t^3+\left(12\,a_{0}-12\,a_{1}\right)\,t^2+\left(60\,v_{0}+60\,v_{1}\right)\,t-120\,p}{6\,t^7}
  41   \end{array}\right.
  42  \f}
  43  @param[in,out] ctx points to an instance of hepta polynomial trajectory
  44  @param[in] ts difference between final time and initial time
  45  @param[in] p0 initial position
  46  @param[in] p1 final position
  47  @param[in] v0 initial velocity
  48  @param[in] v1 final velocity
  49  @param[in] a0 initial acceleration
  50  @param[in] a1 final acceleration
  51  @param[in] j0 initial jerk
  52  @param[in] j1 final jerk
  53 */
  54 A_EXTERN void a_trajpoly7_gen(a_trajpoly7 *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                               a_float j0, a_float j1);
  59 A_EXTERN void a_trajpoly7_gen0(a_trajpoly7 *ctx, a_float ts,
  60                                a_float p0, a_float p1,
  61                                a_float v0, a_float v1,
  62                                a_float a0, a_float a1,
  63                                a_float j0, a_float j1);
  64 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 1)
  65 A_EXTERN void a_trajpoly7_gen1(a_trajpoly7 *ctx);
  66 #endif /* A_TRAJPOLY7 */
  67 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 2)
  68 A_EXTERN void a_trajpoly7_gen2(a_trajpoly7 *ctx);
  69 #endif /* A_TRAJPOLY7 */
  70 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 3)
  71 A_EXTERN void a_trajpoly7_gen3(a_trajpoly7 *ctx);
  72 #endif /* A_TRAJPOLY7 */
  73
  74 /*!
  75  @brief calculate position for hepta polynomial trajectory
  76  \f{aligned}{
  77   \begin{array}{l}
  78   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}+c_{6}\left(t-t_{0}\right)^{6}+c_{7}\left(t-t_{0}\right)^{7}\\
  79   \end{array}
  80  \f}
  81  @param[in] ctx points to an instance of hepta polynomial trajectory
  82  @param[in] x difference between current time and initial time
  83  @return position output
  84 */
  85 A_EXTERN a_float a_trajpoly7_pos(a_trajpoly7 const *ctx, a_float x);
  86
  87 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 1)
  88 /*!
  89  @brief calculate velocity for hepta polynomial trajectory
  90  \f{aligned}{
  91   \begin{array}{l}
  92   \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}+6 c_{6}\left(t-t_{0}\right)^{5}+7 c_{7}\left(t-t_{0}\right)^{6}\\
  93   \end{array}
  94  \f}
  95  @param[in] ctx points to an instance of hepta polynomial trajectory
  96  @param[in] x difference between current time and initial time
  97  @return velocity output
  98 */
  99 A_EXTERN a_float a_trajpoly7_vel(a_trajpoly7 const *ctx, a_float x);
 100 #endif /* A_TRAJPOLY7 */
 101
 102 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 2)
 103 /*!
 104  @brief calculate acceleration for hepta polynomial trajectory
 105  \f{aligned}{
 106   \begin{array}{l}
 107   \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}+30 c_{6}\left(t-t_{0}\right)^{4}+42 c_{7}\left(t-t_{0}\right)^{5}\\
 108   \end{array}
 109  \f}
 110  @param[in] ctx points to an instance of hepta polynomial trajectory
 111  @param[in] x difference between current time and initial time
 112  @return acceleration output
 113 */
 114 A_EXTERN a_float a_trajpoly7_acc(a_trajpoly7 const *ctx, a_float x);
 115 #endif /* A_TRAJPOLY7 */
 116
 117 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 3)
 118 /*!
 119  @brief calculate jerk for hepta polynomial trajectory
 120  \f{aligned}{
 121   \begin{array}{l}
 122   p^{(3)}(t)=6 c_{3}+24 c_{4}\left(t-t_{0}\right)+60 c_{5}\left(t-t_{0}\right)^{2}+120 c_{6}\left(t-t_{0}\right)^{3}+210 c_{7}\left(t-t_{0}\right)^{4}
 123   \end{array}
 124  \f}
 125  @param[in] ctx points to an instance of hepta polynomial trajectory
 126  @param[in] x difference between current time and initial time
 127  @return jerk output
 128 */
 129 A_EXTERN a_float a_trajpoly7_jer(a_trajpoly7 const *ctx, a_float x);
 130 #endif /* A_TRAJPOLY7 */
 131
 132 #if defined(__cplusplus)
 133 } /* extern "C" */
 134 namespace a
 135 {
 136 typedef struct a_trajpoly7 trajpoly7;
 137 } /* namespace a */
 138 #endif /* __cplusplus */
 139
 140 /*!
 141  @brief instance structure for hepta polynomial trajectory
 142  \f{aligned}{
 143   \begin{array}{l}
 144   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}+c_{6}\left(t-t_{0}\right)^{6}+c_{7}\left(t-t_{0}\right)^{7}\\
 145   \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}+6 c_{6}\left(t-t_{0}\right)^{5}+7 c_{7}\left(t-t_{0}\right)^{6}\\
 146   \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}+30 c_{6}\left(t-t_{0}\right)^{4}+42 c_{7}\left(t-t_{0}\right)^{5}\\
 147   p^{(3)}(t)=6 c_{3}+24 c_{4}\left(t-t_{0}\right)+60 c_{5}\left(t-t_{0}\right)^{2}+120 c_{6}\left(t-t_{0}\right)^{3}+210 c_{7}\left(t-t_{0}\right)^{4}
 148   \end{array}
 149  \f}
 150 */
 151 struct a_trajpoly7
 152 {
 153     a_float p[8]; /*!< coefficients of position */
 154 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 1)
 155     a_float v[7]; /*!< coefficients of velocity */
 156 #endif /* A_TRAJPOLY7 */
 157 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 2)
 158     a_float a[6]; /*!< coefficients of acceleration */
 159 #endif /* A_TRAJPOLY7 */
 160 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 3)
 161     a_float j[5]; /*!< coefficients of jerk */
 162 #endif /* A_TRAJPOLY7 */
 163 #if defined(__cplusplus)
 164     A_INLINE void gen(a_float ts, a_float p0, a_float p1,
 165                       a_float v0 = 0, a_float v1 = 0,
 166                       a_float a0 = 0, a_float a1 = 0,
 167                       a_float j0 = 0, a_float j1 = 0)
 168     {
 169         a_trajpoly7_gen(this, ts, p0, p1, v0, v1, a0, a1, j0, j1);
 170     }
 171     A_INLINE void gen0(a_float ts, a_float p0, a_float p1,
 172                        a_float v0 = 0, a_float v1 = 0,
 173                        a_float a0 = 0, a_float a1 = 0,
 174                        a_float j0 = 0, a_float j1 = 0)
 175     {
 176         a_trajpoly7_gen0(this, ts, p0, p1, v0, v1, a0, a1, j0, j1);
 177     }
 178     A_INLINE a_float pos(a_float x) const
 179     {
 180         return a_trajpoly7_pos(this, x);
 181     }
 182 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 1)
 183     A_INLINE void gen1() { a_trajpoly7_gen1(this); }
 184     A_INLINE a_float vel(a_float x) const
 185     {
 186         return a_trajpoly7_vel(this, x);
 187     }
 188 #endif /* A_TRAJPOLY7 */
 189 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 2)
 190     A_INLINE void gen2() { a_trajpoly7_gen2(this); }
 191     A_INLINE a_float acc(a_float x) const
 192     {
 193         return a_trajpoly7_acc(this, x);
 194     }
 195 #endif /* A_TRAJPOLY7 */
 196 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 3)
 197     A_INLINE void gen3() { a_trajpoly7_gen3(this); }
 198     A_INLINE a_float jer(a_float x) const
 199     {
 200         return a_trajpoly7_jer(this, x);
 201     }
 202 #endif /* A_TRAJPOLY7 */
 203 #endif /* __cplusplus */
 204 };
 205
 206 /*! @} a_trajpoly7 */
 207
 208 #endif /* a/trajpoly7.h */