1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
/*!
@file trajtrap.h
@brief trapezoidal velocity trajectory
@details Trajectory Planning for Automatic Machines and Robots
*/
#ifndef LIBA_TRAJTRAP_H
#define LIBA_TRAJTRAP_H
#include "a.h"
/*!
@ingroup liba
@addtogroup a_trajtrap trapezoidal velocity trajectory
@{
*/
typedef struct a_trajtrap a_trajtrap;
#if defined(__cplusplus)
extern "C" {
#endif /* __cplusplus */
/*!
@brief generate for trapezoidal velocity trajectory
@details Assuming that there is no constant velocity phase, but only acceleration and deceleration phases,
the maximum velocity in the motion is \f$v\f$, then we have \f[\frac{v^2-v_0^2}{2a}+\frac{v_1^2-v^2}{2d}=p_1-p_0\f]
Solving for the maximum velocity is \f[|v|=\sqrt{\frac{av_1^2-dv_0^2-2ad(p_1-p_0)}{a-d}}\f]
1. If \f$|v|>|v_m|\f$, then there is a constant velocity phase.
\f[p_a=p_0+v_0T_a+\frac{1}{2}aT_a^2\f] \f[a_d=p_1-v_cT_d-\frac{1}{2}dT_d^2\f]
\f{cases}{T_a=\cfrac{v_m-v_0}{a}\\T_c=\cfrac{p_d-p_a}{v_c}\\T_d=\cfrac{v_1-v_m}{d}\f}
2. Otherwise there is no constant velocity phase.<br>
a. If \f$|v_0|<|v|\le|v_1|\f$, there is only an acceleration phase.
\f[|v_1|=\sqrt{v_0^2+2a(p_1-p_0)}\f] \f{cases}{T_a=\cfrac{v_1-v_0}{a}\\T_c=0\\T_d=0\f}
b. If \f$|v_0|\ge|v|>|v_1|\f$, there is only a deceleration phase.
\f[|v_1|=\sqrt{v_0^2+2d(p_1-p_0)}\f] \f{cases}{T_a=0\\T_c=0\\T_d=\cfrac{v_1-v_0}{d}\f}
c. If \f$|v|>|v_0|\f$, \f$|v|>|v_1|\f$, then there are acceleration and deceleration phases.
\f{cases}{T_a=\cfrac{v-v_0}{a}\\T_c=0\\T_d=\cfrac{v_1-v}{d}\f}
3. Finally, the position and velocity are calculated using the formula.
@param[in,out] ctx points to an instance of trapezoidal velocity trajectory
@param[in] vm defines the maximum velocity during system operation
@param[in] ac defines the acceleration before constant velocity
@param[in] de defines the acceleration after constant velocity
@param[in] p0 defines the initial position
@param[in] p1 defines the final position
@param[in] v0 defines the initial velocity
@param[in] v1 defines the final velocity
@return total duration
*/
A_EXTERN a_float a_trajtrap_gen(a_trajtrap *ctx, a_float vm, a_float ac, a_float de,
a_float p0, a_float p1, a_float v0, a_float v1);
/*!
@brief calculate position for trapezoidal velocity trajectory
\f[
p(t)=\begin{cases}v_0+\frac{1}{2}at^2,&t\in[0,t_a)\\p_a+v_c(t-t_a),&t\in[t_a,t_d)\\
p_d+v_c(t-t_d)+\frac{1}{2}d(t-t_d)^2,&t\in[t_d,T]\end{cases}
\f]
@param[in] ctx points to an instance of trapezoidal velocity trajectory
@param[in] x difference between current time and initial time
@return position output
*/
A_EXTERN a_float a_trajtrap_pos(a_trajtrap const *ctx, a_float x);
/*!
@brief calculate velocity for trapezoidal velocity trajectory
\f[
\dot{p}(t)=\begin{cases}v_0+at,&t\in[0,t_a)\\v_c,&t\in[t_a,t_d)\\v_c+d(t-t_d),&t\in[t_d,T]\end{cases}
\f]
@param[in] ctx points to an instance of trapezoidal velocity trajectory
@param[in] x difference between current time and initial time
@return velocity output
*/
A_EXTERN a_float a_trajtrap_vel(a_trajtrap const *ctx, a_float x);
/*!
@brief calculate acceleration for trapezoidal velocity trajectory
\f[
\ddot{p}(t)=\begin{cases}a,&t\in[0,t_a)\\0,&t\in[t_a,t_d)\\d,&t\in[t_d,T]\end{cases}
\f]
@param[in] ctx points to an instance of trapezoidal velocity trajectory
@param[in] x difference between current time and initial time
@return acceleration output
*/
A_EXTERN a_float a_trajtrap_acc(a_trajtrap const *ctx, a_float x);
#if defined(__cplusplus)
} /* extern "C" */
namespace a
{
typedef struct a_trajtrap trajtrap;
} /* namespace a */
#endif /* __cplusplus */
/*!
@brief instance structure for trapezoidal velocity trajectory
*/
struct a_trajtrap
{
a_float t; //!< total duration
a_float p0; //!< initial position
a_float p1; //!< final position
a_float v0; //!< initial velocity
a_float v1; //!< final velocity
a_float vc; //!< constant velocity
a_float ta; //!< time before constant velocity
a_float td; //!< time after constant velocity
a_float pa; //!< position before constant velocity
a_float pd; //!< position after constant velocity
a_float ac; //!< acceleration before constant velocity
a_float de; //!< acceleration after constant velocity
#if defined(__cplusplus)
A_INLINE a_float gen(a_float vm, a_float ac_, a_float de_, a_float p0_, a_float p1_,
a_float v0_ = 0, a_float v1_ = 0)
{
return a_trajtrap_gen(this, vm, ac_, de_, p0_, p1_, v0_, v1_);
}
A_INLINE a_float pos(a_float x) const
{
return a_trajtrap_pos(this, x);
}
A_INLINE a_float vel(a_float x) const
{
return a_trajtrap_vel(this, x);
}
A_INLINE a_float acc(a_float x) const
{
return a_trajtrap_acc(this, x);
}
#endif /* __cplusplus */
};
/*! @} a_trajtrap */
#endif /* a/trajtrap.h */