From 539ef8b0d1c18ce3cb5b5f6f3816994fad494496 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 25 Jul 2025 09:04:56 +0200 Subject: [PATCH] PositionControlTest: Additional test case velocity setpoint with invalid states (#25286) --- .../PositionControl/PositionControlTest.cpp | 43 ++++++++++++++++++- 1 file changed, 41 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 253f80960f..dde441c652 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -316,23 +316,62 @@ TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations) } } -TEST_F(PositionControlBasicTest, InvalidState) +TEST_F(PositionControlBasicTest, PositionSetpointInvalidState) { - Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position); + Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position); // Position setpoint + // All states valid - ok PositionControlStates states{}; + _position_control.setState(states); + EXPECT_TRUE(runController()); + + // Position invalid states.position(0) = NAN; _position_control.setState(states); EXPECT_FALSE(runController()); + // Position and velocity invalid states.velocity(0) = NAN; _position_control.setState(states); EXPECT_FALSE(runController()); + // Velocity invalid states.position(0) = 0.f; _position_control.setState(states); EXPECT_FALSE(runController()); + // Velocity derivative invalid + states.velocity(0) = 0.f; + states.acceleration(1) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); +} + +TEST_F(PositionControlBasicTest, VelocitySetpointInvalidState) +{ + Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.velocity); // Velocity setpoint + + // All states valid - ok + PositionControlStates states{}; + _position_control.setState(states); + EXPECT_TRUE(runController()); + + // Position invalid - ok + states.position(0) = NAN; + _position_control.setState(states); + EXPECT_TRUE(runController()); + + // Position and velocity invalid + states.velocity(0) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + // Velocity invalid + states.position(0) = 0.f; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + // Velocity derivative invalid states.velocity(0) = 0.f; states.acceleration(1) = NAN; _position_control.setState(states);