TEMP

src/lib/matrix/matrix/helper_functions.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cmath>
+#include <px4_defines.h>
 
 namespace matrix
 {
@@ -41,7 +42,7 @@ Floating wrap_floating(Floating x, Floating low, Floating high)
 	return x - range * num_wraps;
 }
 
-}  // namespace detail
+} // namespace detail
 
 /**
  * Wrap single precision floating point value to stay in range [low, high)

src/lib/motion_planning/HeadingSmoother.hpp

@@ -31,16 +31,11 @@
  *
  ****************************************************************************/
 
-/**
- * @file HeadingSmoother.hpp
- *
- */
-
 #pragma once
 
-#include <motion_planning/VelocitySmoothing.hpp>
-#include <matrix/matrix/helper_functions.hpp>
 #include <float.h>
+#include <matrix/matrix/helper_functions.hpp>
+#include "VelocitySmoothing.hpp"
 
 /**
  * @brief Wrapper class for smoothing heading via maximum angular acceleration limited trajectories.
@@ -68,8 +63,8 @@ class HeadingSmoother
 	HeadingSmoother()
 	{
 		reset(0.f, 0.f);
-		setMaxHeadingRate(VelocitySmoothing::kMinAccel);
-		setMaxHeadingAccel(VelocitySmoothing::kMinJerk);
+		setMaxHeadingRate(0.f);
+		setMaxHeadingAccel(0.f);
 		_heading_smoother.setMaxVel(6.f * M_PI_F); // arbitrary large angle the wrapped value should never reach
 	}
 
@@ -144,10 +139,10 @@ class HeadingSmoother
 	}
 
 	// [rad/s] minimum value of the smoother's maximum heading rate
-	static constexpr float kMinHeadingRate = VelocitySmoothing::kMinAccel;
+	static constexpr float kMinHeadingRate = 0.f;
 
 	// [rad/s^2] minimum value of the smoother's maximum heading acceleration
-	static constexpr float kMinHeadingAccel = VelocitySmoothing::kMinJerk;
+	static constexpr float kMinHeadingAccel = 0.f;
 
 private:
 

src/lib/motion_planning/VelocitySmoothing.cpp

@@ -68,6 +68,8 @@ float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, flo
 		T1_new = (-a_max - a0) / j_max;
 	}
 
+	// printf("T1_new: %.3f\n", (double)T1_new);
+
 	return T1_new;
 }
 
@@ -97,6 +99,12 @@ float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max)
 	}
 
 	T1 = saturateT1ForAccel(a0, j_max, T1, a_max);
+	// printf("j_max: %.3f\n", (double)j_max);
+	// printf("T1_plus: %.3f\n", (double)T1_plus);
+	// printf("T1_minus: %.3f\n", (double)T1_minus);
+	// printf("T3_plus: %.3f\n", (double)T3_plus);
+	// printf("T3_minus: %.3f\n", (double)T3_minus);
+	// printf("T1: %.3f\n", (double)T1);
 
 	return math::max(T1, 0.f);
 }
@@ -167,13 +175,9 @@ void VelocitySmoothing::updateDurations(float vel_setpoint)
 	_state_init = _state;
 
 	_direction = computeDirection();
+	printf("_direction %.3f\n", (double) _direction);
 
-	if (_direction != 0) {
-		updateDurationsMinimizeTotalTime();
-
-	} else {
-		_T1 = _T2 = _T3 = 0.f;
-	}
+	updateDurationsMinimizeTotalTime();
 }
 
 int VelocitySmoothing::computeDirection() const
@@ -187,7 +191,7 @@ int VelocitySmoothing::computeDirection() const
 
 	if (direction == 0) {
 		// If by braking immediately the velocity is exactly
-		// the require one with zero acceleration, then brake
+		// the required one with zero acceleration, then brake
 		direction = sign(_state.a);
 	}
 
@@ -212,14 +216,19 @@ void VelocitySmoothing::updateDurationsMinimizeTotalTime()
 	float jerk_max_T1 = _direction * _max_jerk;
 	float delta_v = _vel_sp - _state.v;
 
-	// compute increasing acceleration time
-	_T1 = computeT1(_state.a, delta_v, jerk_max_T1, _max_accel);
+	if (jerk_max_T1 > FLT_EPSILON) { // zero direction or jerk would lead to division by zero
+		// compute increasing acceleration time
+		_T1 = computeT1(_state.a, delta_v, jerk_max_T1, _max_accel);
+
+		// compute decreasing acceleration time
+		_T3 = computeT3(_T1, _state.a, jerk_max_T1);
 
-	// compute decreasing acceleration time
-	_T3 = computeT3(_T1, _state.a, jerk_max_T1);
+		// compute constant acceleration time
+		_T2 = computeT2(_T1, _T3, _state.a, delta_v, jerk_max_T1);
 
-	// compute constant acceleration time
-	_T2 = computeT2(_T1, _T3, _state.a, delta_v, jerk_max_T1);
+	} else {
+		_T1 = _T2 = _T3 = 0.f;
+	}
 }
 
 Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0, float t, int d) const
@@ -292,12 +301,17 @@ void VelocitySmoothing::updateDurationsGivenTotalTime(float T123)
 	float jerk_max_T1 = _direction * _max_jerk;
 	float delta_v = _vel_sp - _state.v;
 
-	// compute increasing acceleration time
-	_T1 = computeT1(T123, _state.a, delta_v, jerk_max_T1, _max_accel);
+	if (jerk_max_T1 > FLT_EPSILON) { // zero direction or jerk would lead to division by zero
+		// compute increasing acceleration time
+		_T1 = computeT1(T123, _state.a, delta_v, jerk_max_T1, _max_accel);
+
+		// compute decreasing acceleration time
+		_T3 = computeT3(_T1, _state.a, jerk_max_T1);
 
-	// compute decreasing acceleration time
-	_T3 = computeT3(_T1, _state.a, jerk_max_T1);
+		// compute constant acceleration time
+		_T2 = computeT2(T123, _T1, _T3);
 
-	// compute constant acceleration time
-	_T2 = computeT2(T123, _T1, _T3);
+	} else {
+		_T1 = _T2 = _T3 = 0.f;
+	}
 }

src/lib/motion_planning/VelocitySmoothing.hpp

@@ -107,8 +107,6 @@ class VelocitySmoothing
 	void setCurrentPosition(const float pos) { _state.x = _state_init.x = pos; }
 	float getCurrentPosition() const { return _state.x; }
 
-	float getVelSp() const { return _vel_sp; }
-
 	float getT1() const { return _T1; }
 	float getT2() const { return _T2; }
 	float getT3() const { return _T3; }
@@ -192,12 +190,12 @@ class VelocitySmoothing
 	inline Trajectory evaluatePoly(float j, float a0, float v0, float x0, float t, int d) const;
 
 	/* Input */
-	float _vel_sp{0.0f};
+	float _vel_sp{0.f};
 
 	/* Constraints */
-	float _max_jerk = 22.f;
-	float _max_accel = 8.f;
-	float _max_vel = 6.f;
+	float _max_jerk{0.f};
+	float _max_accel{0.f};
+	float _max_vel{0.f};
 
 	/* State (previous setpoints) */
 	Trajectory _state{};
@@ -207,9 +205,9 @@ class VelocitySmoothing
 	Trajectory _state_init{};
 
 	/* Duration of each phase */
-	float _T1 = 0.f; ///< Increasing acceleration [s]
-	float _T2 = 0.f; ///< Constant acceleration [s]
-	float _T3 = 0.f; ///< Decreasing acceleration [s]
+	float _T1{0.f}; ///< Increasing acceleration [s]
+	float _T2{0.f}; ///< Constant acceleration [s]
+	float _T3{0.f}; ///< Decreasing acceleration [s]
 
-	float _local_time = 0.f; ///< Current local time
+	float _local_time{0.f}; ///< Current local time
 };

src/lib/motion_planning/VelocitySmoothingTest.cpp

@@ -43,6 +43,65 @@
 
 using namespace matrix;
 
+TEST(VelocitySmoothingBasicTest, AllZeroCase)
+{
+	// GIVEN: An unconfigured VelocitySmoothing instance
+	VelocitySmoothing trajectory;
+
+	// WHEN: We update it
+	trajectory.updateDurations(0.f);
+	trajectory.updateTraj(0.f);
+
+	// THEN: All the trajectories should still be zero
+	EXPECT_FLOAT_EQ(trajectory.getTotalTime(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentJerk(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentAcceleration(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentVelocity(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentPosition(), 0.f);
+}
+
+TEST(VelocitySmoothingBasicTest, DivisionByZeroComputeT1)
+{
+	// GIVEN: A VelocitySmoothing instance with zero maximum jerk and acceleration
+	VelocitySmoothing trajectory;
+	trajectory.setMaxJerk(0.f);
+	trajectory.setMaxAccel(0.f);
+	trajectory.setMaxVel(1.f);
+
+	// WHEN: We update it
+	trajectory.updateDurations(1.f);
+	trajectory.updateTraj(1.f);
+
+	// THEN: All the trajectories should still be zero
+	EXPECT_FLOAT_EQ(trajectory.getTotalTime(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentJerk(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentAcceleration(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentVelocity(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentPosition(), 0.f);
+}
+
+TEST(VelocitySmoothingBasicTest, DivisionByZeroSaturateT1ForAccel)
+{
+	// GIVEN: A VelocitySmoothing instance with zero maximum jerk and acceleration
+	VelocitySmoothing trajectory(1.f, 0.f, 0.f);
+	trajectory.setMaxJerk(0.f);
+	trajectory.setMaxAccel(0.f);
+	trajectory.setMaxVel(1.f);
+
+	// WHEN: We update it
+	trajectory.updateDurations(1.f);
+	trajectory.updateTraj(1.f);
+
+	// THEN: All the trajectories should still be zero
+	EXPECT_FLOAT_EQ(trajectory.getTotalTime(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentJerk(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentAcceleration(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentVelocity(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentPosition(), 0.f);
+}
+
+//TEST(Test, Blocker) { EXPECT_TRUE(false); }
+
 class VelocitySmoothingTest : public ::testing::Test
 {
 public:
@@ -174,7 +233,7 @@ TEST_F(VelocitySmoothingTest, testConstantSetpoint)
 		updateTrajectories(dt, velocity_setpoints);
 	}
 
-	// THEN: All the trajectories should have reach their
+	// THEN: All the trajectories should have reached their
 	// final state: desired velocity target and zero acceleration
 	for (int i = 0; i < 3; i++) {
 		EXPECT_LE(fabsf(_trajectories[i].getCurrentVelocity() - velocity_setpoints(i)), 0.01f);