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