Address review requests

robotology · Dec 16, 2024 · 8caedce · 8caedce
1 parent 9381d0e
commit 8caedce
Show file tree

Hide file tree

Showing 4 changed files with 40 additions and 22 deletions.
diff --git a/plugins/controlboard/include/ControlBoard.hh b/plugins/controlboard/include/ControlBoard.hh
@@ -105,7 +105,7 @@ private:
     bool initializeTrajectoryGeneratorReferences(yarp::os::Bottle& trajectoryGeneratorsGroup);
     bool parseInitialConfiguration(std::vector<double>& initialConfigurations);
     void resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponentManager& ecm);
-    void configureBuffers();
+    bool configureBuffers();
 };
 
 } // namespace gzyarp
diff --git a/plugins/controlboard/include/ControlBoardData.hh b/plugins/controlboard/include/ControlBoardData.hh
@@ -62,7 +62,7 @@ struct PhysicalJointProperties
     std::string positionControlLaw; // TODO: verify usefulness of this field
 };
 
-struct ActuatedAxesProperties
+struct ActuatedAxisProperties
 {
     CommonJointProperties commonJointProperties;
     std::unique_ptr<yarp::dev::gzyarp::TrajectoryGenerator> trajectoryGenerator;
@@ -77,14 +77,17 @@ class ControlBoardData
 public:
     std::mutex mutex;
     std::vector<PhysicalJointProperties> physicalJoints;
-    std::vector<ActuatedAxesProperties>  actuatedAxes;
+    std::vector<ActuatedAxisProperties>  actuatedAxes;
     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);
+private:
+    yarp::sig::VectorOf<size_t> coupledActuatedAxes, coupledPhysicalJoints;
 };
 
 class IControlBoardData

diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp
@@ -134,18 +134,21 @@ void ControlBoard::Configure(const Entity& _entity,
 
     auto& coupling_group_bottle = m_pluginParameters.findGroup("COUPLING");
     if (!coupling_group_bottle.isNull()) {
-        m_pluginParameters.unput("device");
+        yarp::os::Property couplingParameters;
+        couplingParameters.fromString(m_pluginParameters.toString());
+        couplingParameters.unput("device");
+        // We need to remove the device name from the coupling parameters and set it to the coupling driver
         auto coupling_device_str = coupling_group_bottle.find("device").asString();
-        m_pluginParameters.put("device", coupling_device_str);
-        if(!m_coupling_driver.open(m_pluginParameters))
+        couplingParameters.put("device", coupling_device_str);
+        if(!m_coupling_driver.open(couplingParameters))
         {
             yError() << "gz-sim-yarp-controlboard-system Plugin failed: error in opening yarp "
-                        "coupling driver";
+                        "coupling driver with device name " << coupling_device_str;
             return;
         }
         if(!m_coupling_driver.view(m_controlBoardData.ijointcoupling)) {
             yError() << "gz-sim-yarp-controlboard-system Plugin failed: error in getting "
-                        "IJointCoupling interface";
+                        "IJointCoupling interface from device with name " << coupling_device_str;
             return;
         }
 
@@ -264,8 +267,17 @@ 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
-        configureBuffers();
+        if (!configureBuffers()) {
+            yError() << "Error while configuring buffers";
+            return false;
+        }
 
 
         for (size_t i=0; i<m_controlBoardData.actuatedAxes.size(); i++)
@@ -428,7 +440,6 @@ bool ControlBoard::updateTrajectories(const UpdateInfo& _info, EntityComponentMa
 
         switch (joint.commonJointProperties.controlMode)
         {
-        // PUT THE COUPLING HERE!!
         case VOCAB_CM_POSITION:
             joint.commonJointProperties.refPosition = joint.trajectoryGenerator->computeTrajectory();
             joint.isMotionDone = joint.trajectoryGenerator->isMotionDone();
@@ -1049,7 +1060,7 @@ void ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponen
     }
 }
 
-void ControlBoard::configureBuffers() {
+bool ControlBoard::configureBuffers() {
     size_t nrOfActuatedAxes{0};
     size_t nrOfPhysicalJoints{0};
     if(m_controlBoardData.ijointcoupling)
@@ -1058,13 +1069,13 @@ void ControlBoard::configureBuffers() {
         if(!ok)
         {
             yError() << "Failed to get number of actuated axes";
-            return;
+            return false;
         }
         ok = m_controlBoardData.ijointcoupling->getNrOfPhysicalJoints(nrOfPhysicalJoints);
         if(!ok)
         {
             yError() << "Failed to get number of physical joints";
-            return;
+            return false;
         }
 
     }
@@ -1080,7 +1091,7 @@ void ControlBoard::configureBuffers() {
     m_physicalJointsPositionBuffer.resize(nrOfPhysicalJoints);
     m_physicalJointsVelocityBuffer.resize(nrOfPhysicalJoints);
     m_physicalJointsTorqueBuffer.resize(nrOfPhysicalJoints);
-    return;
+    return true;
 
 }
 

diff --git a/plugins/controlboard/src/ControlBoardData.cpp b/plugins/controlboard/src/ControlBoardData.cpp
@@ -1,18 +1,25 @@
 
-#include "ControlBoardData.hh"
+#include <ControlBoardData.hh>
 
 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())
         {