ControlBoardDriver: add iJointCoupling interface

plugins/controlboard/CMakeLists.txt

@@ -7,6 +7,7 @@ set(HEADER_FILES
 
 set(SRC_FILES
     src/ControlBoard.cpp
+    src/ControlBoardData.cpp
     src/ControlBoardDriver.cpp
     src/ControlBoardTrajectory.cpp
 )

plugins/controlboard/include/ControlBoard.hh

@@ -60,10 +60,15 @@ private:
     std::string m_deviceId;
     gz::sim::Entity m_modelEntity;
     yarp::dev::PolyDriver m_controlBoardDriver;
+    yarp::dev::PolyDriver m_coupling_driver;
     ControlBoardData m_controlBoardData;
     yarp::os::Network m_yarpNetwork;
     yarp::os::Property m_pluginParameters;
     gz::sim::EntityComponentManager* m_ecm;
+    yarp::sig::Vector m_physicalJointsPositionBuffer, m_physicalJointsVelocityBuffer,
+        m_physicalJointsTorqueBuffer;
+    yarp::sig::Vector m_actuatedAxesPositionBuffer, m_actuatedAxesVelocityBuffer,
+        m_actuatedAxesTorqueBuffer;
 
     enum class AngleUnitEnum
     {
@@ -75,8 +80,7 @@ private:
     void updateSimTime(const gz::sim::UpdateInfo& _info);
     bool readJointsMeasurements(const gz::sim::EntityComponentManager& _ecm);
     void checkForJointsHwFault();
-    bool
-    updateTrajectories(const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm);
+    bool updateTrajectories(const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm);
     bool updateReferences(const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm);
     double getJointTorqueFromTransmittedWrench(const gz::sim::Joint& gzJoint,
                                                const gz::msgs::Wrench& wrench,
@@ -90,17 +94,18 @@ private:
     bool setYarpPIDsParam(const std::vector<double>& pidParams,
                           const std::string& paramName,
                           std::vector<yarp::dev::Pid>& yarpPIDs,
-                          size_t numberOfJoints);
+                          size_t numberOfPhysicalJoints);
     void setJointPositionPIDs(AngleUnitEnum cUnits, const std::vector<yarp::dev::Pid>& yarpPIDs);
-    double convertUserGainToGazeboGain(JointProperties& joint, double value);
-    double convertGazeboGainToUserGain(JointProperties& joint, double value);
-    double convertGazeboToUser(JointProperties& joint, double value);
-    double convertUserToGazebo(JointProperties& joint, double value);
+    double convertUserGainToGazeboGain(PhysicalJointProperties& joint, double value);
+    double convertGazeboGainToUserGain(PhysicalJointProperties& joint, double value);
+    double convertGazeboToUser(PhysicalJointProperties& joint, double value);
+    double convertUserToGazebo(PhysicalJointProperties& joint, double value);
     bool initializeJointPositionLimits(const gz::sim::EntityComponentManager& ecm);
     bool initializeTrajectoryGenerators();
     bool initializeTrajectoryGeneratorReferences(yarp::os::Bottle& trajectoryGeneratorsGroup);
     bool parseInitialConfiguration(std::vector<double>& initialConfigurations);
     void resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponentManager& ecm);
+    void configureBuffers();
 };
 
 } // namespace gzyarp

plugins/controlboard/include/ControlBoardData.hh

@@ -15,6 +15,7 @@
 #include <yarp/conf/numeric.h>
 #include <yarp/dev/IControlMode.h>
 #include <yarp/dev/IInteractionMode.h>
+#include <yarp/dev/IJointCoupling.h>
 #include <yarp/dev/PidEnums.h>
 #include <yarp/os/Stamp.h>
 #include <yarp/os/Vocab.h>
@@ -33,8 +34,7 @@ struct PidControlTypeEnumHashFunction
     }
 };
 
-struct JointProperties
-{
+struct CommonJointProperties {
     std::string name;
     yarp::dev::InteractionModeEnum interactionMode{yarp::dev::InteractionModeEnum::VOCAB_IM_STIFF};
     yarp::conf::vocab32_t controlMode{VOCAB_CM_IDLE};
@@ -52,9 +52,19 @@ struct JointProperties
     double velocity{0.0}; // Joint velocity [deg/s]
     double velocityLimitMin{std::numeric_limits<double>::min()};
     double velocityLimitMax{std::numeric_limits<double>::max()};
+};
+
+struct PhysicalJointProperties
+{
+    CommonJointProperties commonJointProperties;
     std::unordered_map<yarp::dev::PidControlTypeEnum, gz::math::PID, PidControlTypeEnumHashFunction>
         pidControllers;
     std::string positionControlLaw; // TODO: verify usefulness of this field
+};
+
+struct ActuatedAxesProperties
+{
+    CommonJointProperties commonJointProperties;
     std::unique_ptr<yarp::dev::gzyarp::TrajectoryGenerator> trajectoryGenerator;
     double trajectoryGenerationRefPosition{0.0};
     double trajectoryGenerationRefSpeed{0.0};
@@ -66,11 +76,15 @@ class ControlBoardData
 {
 public:
     std::mutex mutex;
-    std::vector<JointProperties> joints;
+    std::vector<PhysicalJointProperties> physicalJoints;
+    std::vector<ActuatedAxesProperties>  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 setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode);
+    bool setControlMode(int j, int mode);
 };
 
 class IControlBoardData