ControlBoard: cache the coupled joints

robotology · Dec 16, 2024 · 995128f · 995128f
1 parent 4955965
commit 995128f
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 8 deletions.
diff --git a/plugins/controlboard/include/ControlBoardData.hh b/plugins/controlboard/include/ControlBoardData.hh
@@ -78,11 +78,13 @@ public:
     std::mutex mutex;
     std::vector<PhysicalJointProperties> physicalJoints;
     std::vector<ActuatedAxisProperties>  actuatedAxes;
+    yarp::sig::VectorOf<size_t> coupledActuatedAxes, coupledPhysicalJoints;
     yarp::os::Stamp simTime;
     yarp::dev::IJointCoupling* ijointcoupling{nullptr};
     // TODO (xela95): read this value from configuration file
     std::chrono::milliseconds controlUpdatePeriod = std::chrono::milliseconds(1);
-
+
+    bool initCoupledJoints();
     bool setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode);
     bool setControlMode(int j, int mode);
 };

diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp
@@ -264,8 +264,14 @@ bool ControlBoard::setJointProperties(EntityComponentManager& _ecm)
 
             yInfo() << "Joint " << jointFromConfigName << " added to the control board data.";
         }
+        if (!m_controlBoardData.initCoupledJoints())
+        {
+            yError() << "Error while initializing coupled joints";
+            return false;
+        }
+
         // Let's initialize all the buffers/vectors
-        if(!configureBuffers()) {
+        if (!configureBuffers()) {
             yError() << "Error while configuring buffers";
             return false;
         }

diff --git a/plugins/controlboard/src/ControlBoardData.cpp b/plugins/controlboard/src/ControlBoardData.cpp
@@ -3,16 +3,23 @@
 
 namespace gzyarp
 {
+
+
+bool ControlBoardData::initCoupledJoints() {
+    if (this->ijointcoupling) {
+        bool ok = this->ijointcoupling->getCoupledActuatedAxes(coupledActuatedAxes);
+        ok = ok & this->ijointcoupling->getCoupledPhysicalJoints(coupledPhysicalJoints);
+        return ok;
+    }
+    return true;
+}
 bool ControlBoardData::setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode)
 {
-    yarp::sig::VectorOf<size_t> coupledActuatedAxes, coupledPhysicalJoints;
     try
     {
         // If there is coupling let's check if we have to change the interaction mode for all the coupled joints
         if (this->ijointcoupling)
         {
-            this->ijointcoupling->getCoupledActuatedAxes(coupledActuatedAxes);
-            this->ijointcoupling->getCoupledPhysicalJoints(coupledPhysicalJoints);
             // If the joint is coupled, we have to change the interaction mode for all the coupled joints
             if(std::find(coupledActuatedAxes.begin(), coupledActuatedAxes.end(), axis) != coupledActuatedAxes.end())
             {
@@ -79,12 +86,9 @@ bool ControlBoardData::setControlMode(int j, int mode) {
         desired_mode = VOCAB_CM_IDLE;
     }
 
-    yarp::sig::VectorOf<size_t> coupledActuatedAxes, coupledPhysicalJoints;
     // If there is coupling let's check if we have to change the interaction mode for all the coupled joints
     if (this->ijointcoupling)
     {
-        this->ijointcoupling->getCoupledActuatedAxes(coupledActuatedAxes);
-        this->ijointcoupling->getCoupledPhysicalJoints(coupledPhysicalJoints);
         // If the joint is coupled, we have to change the interaction mode for all the coupled joints
         if(std::find(coupledActuatedAxes.begin(), coupledActuatedAxes.end(), j) != coupledActuatedAxes.end())
         {