Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 14:47:44 +08:00
Code Issues Actions 16 Packages Projects Releases Wiki Activity

VelocitySmoothing: fix issue when delta vel is the same on all axes

Browse Source 
When the change in velocity is exactly the same on several axes, the T1
of the recumputed trajectories after time sync was set to 0 because it
was skipping both if and else if.
This commit is contained in:
bresch
2021-10-21 11:41:45 +02:00
committed by Mathieu Bresciani
parent 9301288d1f
commit 78fe6e2152
2 changed files with 38 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/motion_planning/VelocitySmoothing.cpp
+6 -8
View File
@@ -120,16 +120,13 @@ float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max,
float T3_plus = a0 / j_max + T1_plus;
float T3_minus = a0 / j_max + T1_minus;
float T13_plus = T1_plus + T3_plus;
float T13_minus = T1_minus + T3_minus;
float T1 = 0.f;
if (T13_plus > T123) {
T1 = T1_minus;
} else if (T13_minus > T123) {
if ((T1_plus >= 0.f && T3_plus >= 0.f) && ((T1_plus + T3_plus) <= T123)) {
T1 = T1_plus;
} else if ((T1_minus >= 0.f && T3_minus >= 0.f) && ((T1_minus + T3_minus) <= T123)) {
T1 = T1_minus;
}
T1 = saturateT1ForAccel(a0, j_max, T1, a_max);
@@ -282,7 +279,8 @@ void VelocitySmoothing::timeSynchronization(VelocitySmoothing *traj, int n_traj)
if (desired_time > FLT_EPSILON) {
for (int i = 0; i < n_traj; i++) {
if (i != longest_traj_index) {
if ((i != longest_traj_index)
&& (traj[i].getTotalTime() < desired_time)) {
traj[i].updateDurationsGivenTotalTime(desired_time);
}
}
src/lib/motion_planning/VelocitySmoothingTest.cpp
+32
View File
@@ -112,6 +112,38 @@ TEST_F(VelocitySmoothingTest, testTimeSynchronization)
EXPECT_LE(fabsf(_trajectories[0].getTotalTime() - _trajectories[1].getTotalTime()), 0.0001);
}
TEST_F(VelocitySmoothingTest, testTimeSynchronizationSameDelta)
{
// GIVEN: a set of initial conditions
Vector3f a0(0.f, 0.f, 0.f);
Vector3f v0(0.5f, -0.2f, 0.f);
Vector3f x0(0.f, 0.f, 0.f);
setInitialConditions(a0, v0, x0);
// WHEN: the same delta velocity is set to the XY-axes
const float delta_v = 0.3f;
Vector3f velocity_setpoints{v0(0) + delta_v, v0(1) + delta_v, 0.f};
for (int i = 0; i < 3; i++) {
_trajectories[i].updateDurations(velocity_setpoints(i));
}
VelocitySmoothing::timeSynchronization(_trajectories, 3);
// THEN: they should have the same T1, T2 and T3 dirations
EXPECT_FLOAT_EQ(_trajectories[0].getTotalTime(), _trajectories[1].getTotalTime());
EXPECT_FLOAT_EQ(_trajectories[0].getT1(), _trajectories[1].getT1());
EXPECT_FLOAT_EQ(_trajectories[0].getT2(), _trajectories[1].getT2());
EXPECT_FLOAT_EQ(_trajectories[0].getT3(), _trajectories[1].getT3());
// AND: the Z axis should have the same duration but spend all its time in T2 (constant phase)
EXPECT_FLOAT_EQ(_trajectories[2].getTotalTime(), _trajectories[0].getTotalTime());
EXPECT_FLOAT_EQ(_trajectories[2].getT1(), 0.f);
EXPECT_FLOAT_EQ(_trajectories[2].getT2(), _trajectories[0].getTotalTime());
EXPECT_FLOAT_EQ(_trajectories[2].getT3(), 0.f);
}
TEST_F(VelocitySmoothingTest, testConstantSetpoint)
{
// GIVEN: A set of constraints