From 2fdcd3c832540be9f8c5b561db2063910625cda4 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio@traversaro.it>
Date: Wed, 21 Aug 2024 15:09:23 +0200
Subject: [PATCH] ControlBoardDriver: Add check that min position limit is
 lower than max position limit

---
 plugins/controlboard/src/ControlBoardDriver.cpp | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp
index 50b12bbe1..8fd536ae3 100644
--- a/plugins/controlboard/src/ControlBoardDriver.cpp
+++ b/plugins/controlboard/src/ControlBoardDriver.cpp
@@ -882,6 +882,17 @@ bool GazeboYarpControlBoardDriver::setMinMaxPos()
         yCWarning(GAZEBOCONTROLBOARD) << "Missing LIMITS section";
     }
 
+    // Check if the limits have sense
+    for (size_t i = 0; i < m_numberOfJoints; ++i)
+    {   
+        if(m_jointPosLimits[i].min > m_jointPosLimits[i].max)
+        {
+            yCError(GAZEBOCONTROLBOARD) << "Joint " << m_jointNames[i] << " has inconsistent min " << m_jointPosLimits[i].min 
+                                        << " and max " << m_jointPosLimits[i].max << " position limits (min should be lower than max)." ;
+            return false;
+        }
+    }
+
     return true;
 }