From 9381d0e84330242d10a2561d4540513fa3991d8e Mon Sep 17 00:00:00 2001
From: Nicogene <nicolo.genesio@iit.it>
Date: Fri, 13 Dec 2024 16:53:32 +0100
Subject: [PATCH] ControlBoard: fix limits and joint names for the actuated
 axes

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

diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp
index 294aa42..e1f6dfb 100644
--- a/plugins/controlboard/src/ControlBoard.cpp
+++ b/plugins/controlboard/src/ControlBoard.cpp
@@ -267,6 +267,21 @@ bool ControlBoard::setJointProperties(EntityComponentManager& _ecm)
         // Let's initialize all the buffers/vectors
         configureBuffers();
 
+
+        for (size_t i=0; i<m_controlBoardData.actuatedAxes.size(); i++)
+        {
+            if (m_controlBoardData.ijointcoupling) {
+                bool ok = m_controlBoardData.ijointcoupling->getActuatedAxisName(i, m_controlBoardData.actuatedAxes[i].commonJointProperties.name);
+                if (!ok) {
+                    yError() << "Error while getting actuated axis name";
+                    return false;
+                }
+            }
+            else {
+                m_controlBoardData.actuatedAxes[i].commonJointProperties.name = m_controlBoardData.physicalJoints[i].commonJointProperties.name;
+            }
+        }
+
         if (!initializeJointPositionLimits(_ecm))
         {
             yError() << "Error while setting joint position limits";
@@ -769,6 +784,15 @@ bool ControlBoard::initializeJointPositionLimits(const gz::sim::EntityComponentM
         }
        
     }
+    else {
+        // If no coupling is present, the actuated axes are the same as the physical joints
+        for (size_t i = 0; i < m_controlBoardData.actuatedAxes.size(); ++i)
+        {
+            auto& actuatedAxis = m_controlBoardData.actuatedAxes[i];
+            actuatedAxis.commonJointProperties.positionLimitMin = m_controlBoardData.physicalJoints[i].commonJointProperties.positionLimitMin;
+            actuatedAxis.commonJointProperties.positionLimitMax = m_controlBoardData.physicalJoints[i].commonJointProperties.positionLimitMax;
+        }
+    }
 
     return true;
 }