PX4 Firmware
PX4 Autopilot Software http://px4.io
VelocitySmoothingTest.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 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 /**
35  * Test code for the Velocity Smoothing library
36  * Run this test only using make tests TESTFILTER=VelocitySmoothing
37  */
38 
39 #include <gtest/gtest.h>
40 #include <matrix/matrix/math.hpp>
41 
42 #include "VelocitySmoothing.hpp"
43 
44 using namespace matrix;
45 
46 class VelocitySmoothingTest : public ::testing::Test
47 {
48 public:
49  void setConstraints(float j_max, float a_max, float v_max);
50  void setInitialConditions(Vector3f acc, Vector3f vel, Vector3f pos);
51  void updateTrajectories(float dt, Vector3f velocity_setpoints);
52 
53  VelocitySmoothing _trajectories[3];
54 };
55 
56 void VelocitySmoothingTest::setConstraints(float j_max, float a_max, float v_max)
57 {
58  for (int i = 0; i < 3; i++) {
59  _trajectories[i].setMaxJerk(j_max);
60  _trajectories[i].setMaxAccel(a_max);
61  _trajectories[i].setMaxVel(v_max);
62  }
63 }
64 
66 {
67  for (int i = 0; i < 3; i++) {
68  _trajectories[i].setCurrentAcceleration(a0(i));
69  _trajectories[i].setCurrentVelocity(v0(i));
70  _trajectories[i].setCurrentPosition(x0(i));
71  }
72 }
73 
74 void VelocitySmoothingTest::updateTrajectories(float dt, Vector3f velocity_setpoints)
75 {
76  for (int i = 0; i < 3; i++) {
77  _trajectories[i].updateTraj(dt);
78  EXPECT_LE(fabsf(_trajectories[i].getCurrentJerk()), _trajectories[i].getMaxJerk());
79  EXPECT_LE(fabsf(_trajectories[i].getCurrentAcceleration()), _trajectories[i].getMaxAccel());
80  EXPECT_LE(fabsf(_trajectories[i].getCurrentVelocity()), _trajectories[i].getMaxVel());
81  }
82 
83  for (int i = 0; i < 3; i++) {
84  _trajectories[i].updateDurations(velocity_setpoints(i));
85  }
86 
88 }
89 
90 TEST_F(VelocitySmoothingTest, testTimeSynchronization)
91 {
92  // GIVEN: A set of constraints
93  const float j_max = 55.2f;
94  const float a_max = 6.f;
95  const float v_max = 6.f;
96 
97  setConstraints(j_max, a_max, v_max);
98 
99  // AND: A set of initial conditions
100  Vector3f a0(0.22f, 0.f, 0.22f);
101  Vector3f v0(2.47f, -5.59e-6f, 2.47f);
102  Vector3f x0(0.f, 0.f, 0.f);
103 
104  setInitialConditions(a0, v0, x0);
105 
106  // WHEN: We generate trajectories (time synchronized in XY) with constant setpoints and dt
107  Vector3f velocity_setpoints(-3.f, 1.f, 0.f);
108  updateTrajectories(0.f, velocity_setpoints);
109 
110 
111  // THEN: The X and Y trajectories should have the same total time (= time sunchronized)
112  EXPECT_LE(fabsf(_trajectories[0].getTotalTime() - _trajectories[1].getTotalTime()), 0.0001);
113 }
114 
115 TEST_F(VelocitySmoothingTest, testConstantSetpoint)
116 {
117  // GIVEN: A set of constraints
118  const float j_max = 55.2f;
119  const float a_max = 6.f;
120  const float v_max = 6.f;
121 
122  setConstraints(j_max, a_max, v_max);
123 
124  // AND: A set of initial conditions
125  Vector3f a0(0.f, 0.f, 0.f);
126  Vector3f v0(0.f, 0.f, 0.f);
127  Vector3f x0(0.f, 0.f, 0.f);
128 
129  setInitialConditions(a0, v0, x0);
130 
131  // WHEN: We generate trajectories with constant setpoints and dt
132  Vector3f velocity_setpoints(-3.f, 0.f, -1.f);
133 
134  // Compute the number of steps required to reach desired value
135  // The updateTrajectories is first called once to compute the total time
136  const float dt = 0.01;
137  updateTrajectories(0.f, velocity_setpoints);
138  float t123 = _trajectories[0].getTotalTime();
139  int nb_steps = ceil(t123 / dt);
140 
141  for (int i = 0; i < nb_steps; i++) {
142  updateTrajectories(dt, velocity_setpoints);
143  }
144 
145  // THEN: All the trajectories should have reach their
146  // final state: desired velocity target and zero acceleration
147  for (int i = 0; i < 3; i++) {
148  EXPECT_LE(fabsf(_trajectories[i].getCurrentVelocity() - velocity_setpoints(i)), 0.01f);
149  EXPECT_LE(fabsf(_trajectories[i].getCurrentAcceleration()), 0.0001f);
150  }
151 }
152 
153 TEST_F(VelocitySmoothingTest, testZeroSetpoint)
154 {
155  // GIVEN: A set of null initial conditions
156  Vector3f a0(0.f, 0.f, 0.f);
157  Vector3f v0(0.f, 0.f, 0.f);
158  Vector3f x0(0.f, 0.f, 0.f);
159 
160  setInitialConditions(a0, v0, x0);
161 
162  // AND: Zero setpoints
163  Vector3f velocity_setpoints(0.f, 0.f, 0.f);
164  float t = 0.f;
165  const float dt = 0.01f;
166 
167  // WHEN: We run a few times the algorithm
168  for (int i = 0; i < 60; i++) {
169  updateTrajectories(dt, velocity_setpoints);
170  t += dt;
171  }
172 
173  // THEN: All the trajectories should still be zero
174  for (int i = 0; i < 3; i++) {
175  EXPECT_EQ(_trajectories[i].getCurrentJerk(), 0.f);
176  EXPECT_EQ(_trajectories[i].getCurrentAcceleration(), 0.f);
177  EXPECT_EQ(_trajectories[i].getCurrentVelocity(), 0.f);
178  EXPECT_EQ(_trajectories[i].getCurrentPosition(), 0.f);
179  }
180 }
float t123
Definition: LOSX.c:122
Dual< Scalar, N > ceil(const Dual< Scalar, N > &a)
Definition: Dual.hpp:203
TODO: document the algorithm |T1| T2 |T3| __| |____ __ Jerk |_| / \ Acceleration ___/ ___ ;" / / V...
TEST_F(VelocitySmoothingTest, testTimeSynchronization)
void updateTrajectories(float dt, Vector3f velocity_setpoints)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
float dt
Definition: px4io.c:73
static void timeSynchronization(VelocitySmoothing *traj, int n_traj)
Synchronize several trajectories to have the same total time.
void setInitialConditions(Vector3f acc, Vector3f vel, Vector3f pos)
void setConstraints(float j_max, float a_max, float v_max)