PX4 Firmware
PX4 Autopilot Software http://px4.io
VelocitySmoothing.hpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #pragma once
35 
36 struct Trajectory {
37  float j; //< jerk
38  float a; //< acceleration
39  float v; //< velocity
40  float x; //< position
41 };
42 
43 /**
44  * @class VelocitySmoothing
45  *
46  * TODO: document the algorithm
47  * |T1| T2 |T3|
48  * ___
49  * __| |____ __ Jerk
50  * |_|
51  * ___
52  * / \ Acceleration
53  * ___/ \___
54  * ___
55  * ;"
56  * /
57  * / Velocity
58  * ;
59  * ----"
60  */
62 {
63 public:
64  VelocitySmoothing(float initial_accel = 0.f, float initial_vel = 0.f, float initial_pos = 0.f);
65  ~VelocitySmoothing() = default;
66 
67  /**
68  * Reset the state.
69  * @param accel Current acceleration
70  * @param vel Current velocity
71  * @param pos Current position
72  */
73  void reset(float accel, float vel, float pos);
74 
75  /**
76  * Compute T1, T2, T3 depending on the current state and velocity setpoint. This should be called on every cycle
77  * and before updateTraj().
78  * @param vel_setpoint velocity setpoint input
79  */
80  void updateDurations(float vel_setpoint);
81 
82  /**
83  * Generate the trajectory (acceleration, velocity and position) by integrating the current jerk
84  * @param dt integration period
85  * @param time_stretch (optional) used to scale the integration period. This can be used to slow down
86  * or fast-forward the trajectory
87  */
88  void updateTraj(float dt, float time_stretch = 1.f);
89 
90  /**
91  * Getters and setters
92  */
93  float getMaxJerk() const { return _max_jerk; }
94  void setMaxJerk(float max_jerk) { _max_jerk = max_jerk; }
95 
96  float getMaxAccel() const { return _max_accel; }
97  void setMaxAccel(float max_accel) { _max_accel = max_accel; }
98 
99  float getMaxVel() const { return _max_vel; }
100  void setMaxVel(float max_vel) { _max_vel = max_vel; }
101 
102  float getCurrentJerk() const { return _state.j; }
103  void setCurrentAcceleration(const float accel) { _state.a = _state_init.a = accel; }
104  float getCurrentAcceleration() const { return _state.a; }
105  void setCurrentVelocity(const float vel) { _state.v = _state_init.v = vel; }
106  float getCurrentVelocity() const { return _state.v; }
107  void setCurrentPosition(const float pos) { _state.x = _state_init.x = pos; }
108  float getCurrentPosition() const { return _state.x; }
109 
110  float getVelSp() const { return _vel_sp; }
111 
112  float getT1() const { return _T1; }
113  float getT2() const { return _T2; }
114  float getT3() const { return _T3; }
115  float getTotalTime() const { return _T1 + _T2 + _T3; }
116 
117  /**
118  * Synchronize several trajectories to have the same total time. This is required to generate
119  * straight lines.
120  * The resulting total time is the one of the longest trajectory.
121  * @param traj an array of VelocitySmoothing objects
122  * @param n_traj the number of trajectories to be synchronized
123  */
124  static void timeSynchronization(VelocitySmoothing *traj, int n_traj);
125 
126 private:
127 
128  /**
129  * Compute T1, T2, T3 depending on the current state and velocity setpoint.
130  * Minimize the total time of the trajectory
131  */
132  void updateDurationsMinimizeTotalTime();
133 
134  /**
135  * Compute T1, T2, T3 depending on the current state and velocity setpoint.
136  * @param T123 desired total time of the trajectory
137  */
138  void updateDurationsGivenTotalTime(float T123);
139 
140  /**
141  * Compute the direction of the jerk to be applied in order to drive the current state
142  * to the desired one
143  */
144  int computeDirection();
145 
146  /**
147  * Compute the velocity at which the trajectory will be if the maximum jerk is applied
148  * during the time required to cancel the current acceleration
149  */
150  float computeVelAtZeroAcc();
151 
152  /**
153  * Compute increasing acceleration time
154  */
155  inline float computeT1(float a0, float v3, float j_max, float a_max);
156 
157  /**
158  * Compute increasing acceleration time using total time constraint
159  */
160  inline float computeT1(float T123, float a0, float v3, float j_max, float a_max);
161 
162  /**
163  * Saturate T1 in order to respect the maximum acceleration constraint
164  */
165  inline float saturateT1ForAccel(float a0, float j_max, float T1, float a_max);
166 
167  /**
168  * Compute constant acceleration time
169  */
170  inline float computeT2(float T1, float T3, float a0, float v3, float j_max);
171 
172  /**
173  * Compute constant acceleration time using total time constraint
174  */
175  inline float computeT2(float T123, float T1, float T3);
176 
177  /**
178  * Compute decreasing acceleration time
179  */
180  inline float computeT3(float T1, float a0, float j_max);
181 
182  /**
183  * Compute the jerk, acceleration, velocity and position
184  * of a jerk-driven polynomial trajectory at a given time t
185  * @param j jerk
186  * @param a0 initial acceleration at t = 0
187  * @param v0 initial velocity
188  * @param x0 initial postion
189  * @param t current time
190  * @param d direction
191  */
192  inline Trajectory evaluatePoly(float j, float a0, float v0, float x0, float t, int d);
193 
194  /* Input */
195  float _vel_sp{0.0f};
196 
197  /* Constraints */
198  float _max_jerk = 22.f;
199  float _max_accel = 8.f;
200  float _max_vel = 6.f;
201 
202  /* State (previous setpoints) */
203  Trajectory _state{};
204  int _direction{0};
205 
206  /* Initial conditions */
207  Trajectory _state_init{};
208 
209  /* Duration of each phase */
210  float _T1 = 0.f; ///< Increasing acceleration [s]
211  float _T2 = 0.f; ///< Constant acceleration [s]
212  float _T3 = 0.f; ///< Decreasing acceleration [s]
213 
214  float _local_time = 0.f; ///< Current local time
215 };
float getCurrentVelocity() const
int reset(enum LPS22HB_BUS busid)
Reset the driver.
void setMaxJerk(float max_jerk)
float getCurrentJerk() const
float getMaxAccel() const
void setCurrentVelocity(const float vel)
TODO: document the algorithm |T1| T2 |T3| __| |____ __ Jerk |_| / \ Acceleration ___/ ___ ;" / / V...
float getCurrentAcceleration() const
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
float getTotalTime() const
void setCurrentPosition(const float pos)
float getCurrentPosition() const
float getMaxJerk() const
Getters and setters.
float getMaxVel() const
float dt
Definition: px4io.c:73
void setMaxVel(float max_vel)
void setMaxAccel(float max_accel)
void setCurrentAcceleration(const float accel)