From 78fe6e2152861012d19e6d9fd684d2e70e3fe787 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 21 Oct 2021 11:41:45 +0200 Subject: [PATCH] VelocitySmoothing: fix issue when delta vel is the same on all axes 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. --- src/lib/motion_planning/VelocitySmoothing.cpp | 14 ++++---- .../motion_planning/VelocitySmoothingTest.cpp | 32 +++++++++++++++++++ 2 files changed, 38 insertions(+), 8 deletions(-) diff --git a/src/lib/motion_planning/VelocitySmoothing.cpp b/src/lib/motion_planning/VelocitySmoothing.cpp index 9a29e6a1a7..6471bfaf4c 100644 --- a/src/lib/motion_planning/VelocitySmoothing.cpp +++ b/src/lib/motion_planning/VelocitySmoothing.cpp @@ -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); } } diff --git a/src/lib/motion_planning/VelocitySmoothingTest.cpp b/src/lib/motion_planning/VelocitySmoothingTest.cpp index 2bf2b0e983..d59fd9683d 100644 --- a/src/lib/motion_planning/VelocitySmoothingTest.cpp +++ b/src/lib/motion_planning/VelocitySmoothingTest.cpp @@ -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