2 changed files with 124 additions and 0 deletions
@ -0,0 +1,122 @@
@@ -0,0 +1,122 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (C) 2019 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* Test code for the Velocity Smoothing library |
||||
* Run this test only using make tests TESTFILTER=VelocitySmoothing |
||||
*/ |
||||
|
||||
#include <gtest/gtest.h> |
||||
#include <matrix/matrix/math.hpp> |
||||
|
||||
#include "VelocitySmoothing.hpp" |
||||
|
||||
using namespace matrix; |
||||
|
||||
class VelocitySmoothingTest : public ::testing::Test |
||||
{ |
||||
public: |
||||
void SetUp() override |
||||
{ |
||||
return; |
||||
} |
||||
|
||||
void setConstraints(float j_max, float a_max, float v_max); |
||||
void setInitialConditions(Vector3f acc, Vector3f vel, Vector3f pos); |
||||
void updateTrajectories(Vector3f velocity_setpoints, float dt); |
||||
|
||||
|
||||
VelocitySmoothing _trajectories[3]; |
||||
}; |
||||
|
||||
void VelocitySmoothingTest::setConstraints(float j_max, float a_max, float v_max) |
||||
{ |
||||
for (int i = 0; i < 3; i++) { |
||||
_trajectories[i].setMaxJerk(j_max); |
||||
_trajectories[i].setMaxAccel(a_max); |
||||
_trajectories[i].setMaxVel(v_max); |
||||
} |
||||
} |
||||
|
||||
void VelocitySmoothingTest::setInitialConditions(Vector3f a0, Vector3f v0, Vector3f x0) |
||||
{ |
||||
for (int i = 0; i < 3; i++) { |
||||
_trajectories[i].setCurrentAcceleration(a0(i)); |
||||
_trajectories[i].setCurrentVelocity(v0(i)); |
||||
_trajectories[i].setCurrentPosition(x0(i)); |
||||
} |
||||
} |
||||
|
||||
void VelocitySmoothingTest::updateTrajectories(Vector3f velocity_setpoints, float dt) |
||||
{ |
||||
for (int i = 0; i < 3; i++) { |
||||
_trajectories[i].updateDurations(dt, velocity_setpoints(i)); |
||||
} |
||||
|
||||
VelocitySmoothing::timeSynchronization(_trajectories, 2); |
||||
|
||||
float dummy; // We don't care about the immediate result
|
||||
for (int i = 0; i < 3; i++) { |
||||
_trajectories[i].integrate(dummy, dummy, dummy); |
||||
} |
||||
} |
||||
|
||||
TEST_F(VelocitySmoothingTest, testConstantSetpoint) |
||||
{ |
||||
// Set the constraints
|
||||
const float j_max = 55.2f; |
||||
const float a_max = 6.f; |
||||
const float v_max = 6.f; |
||||
|
||||
setConstraints(j_max, a_max, v_max); |
||||
|
||||
// Set the initial conditions
|
||||
Vector3f a0(0.22f, 0.f, 0.22f); |
||||
Vector3f v0(2.47f, -5.59e-6f, 2.47f); |
||||
Vector3f x0(0.f, 0.f, 0.f); |
||||
|
||||
setInitialConditions(a0, v0, x0); |
||||
|
||||
// Generate the trajectories with constant setpoints and dt
|
||||
Vector3f velocity_setpoints(0.f, 1.f, 0.f); |
||||
float dt = 0.01f; |
||||
|
||||
for (int i = 0; i < 60; i++) { |
||||
updateTrajectories(velocity_setpoints, dt); |
||||
} |
||||
|
||||
// Check that all the trajectories reached their desired value
|
||||
EXPECT_LE(fabsf(_trajectories[0].getCurrentVelocity() - velocity_setpoints(0)), 0.01f); |
||||
EXPECT_LE(fabsf(_trajectories[1].getCurrentVelocity() - velocity_setpoints(1)), 0.01f); |
||||
EXPECT_LE(fabsf(_trajectories[2].getCurrentVelocity() - velocity_setpoints(2)), 0.01f); |
||||
} |
Loading…
Reference in new issue