include/a/trajpoly3.h

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