ControlBoardDriver: support use of IJointCoupling devices in gazebo_y…

…arp_controlboard and drop support for hardcoded HandMK5 coupling (#671)

Co-authored-by: Silvio Traversaro <[email protected]>

CHANGELOG.md

@@ -5,6 +5,13 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ## [Unreleased]
 
+### Added
+- The `gazebo_yarp_controlboard` plugin gained support to load at runtime arbitrary couplings specified by a yarp device that exposes the [`yarp::dev::IJointCoupling`](https://github.com/robotology/yarp/blob/v3.9.0/src/libYARP_dev/src/yarp/dev/IJointCoupling.h#L16) interface. The name of the yarp device used to load the coupling is passed via the `device` parameter in the `COUPLING` group. For an example of PR that uses this new feature, check https://github.com/icub-tech-iit/ergocub-software/pull/178 .
+
+### Removed
+
+- The support for passing the `icub_hand_mk5` parameter in the `COUPLING` the `gazebo_yarp_controlboard` plugin was removed. This breaks compatibility with models contained in ergocub-software <= 0.6.0, to use those models use a newer version of ergocub-software. 
+
 ## [4.9.0] - 2023-10-31
 
 ### Changed

CMakeLists.txt

@@ -46,7 +46,7 @@ if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE)
     list(APPEND YARP_ADDITIONAL_COMPONENTS_REQUIRED "robotinterface")
 endif()
 
-find_package(YARP 3.6 REQUIRED COMPONENTS os sig dev math idl_tools ${YARP_ADDITIONAL_COMPONENTS_REQUIRED})
+find_package(YARP 3.9 REQUIRED COMPONENTS os sig dev math idl_tools ${YARP_ADDITIONAL_COMPONENTS_REQUIRED})
 find_package(Gazebo REQUIRED)
 if (Gazebo_VERSION_MAJOR LESS 11.0)
     message(status "Gazebo version : " ${Gazebo_VERSION_MAJOR}.${Gazebo_VERSION_MINOR}.${Gazebo_VERSION_PATCH})
@@ -60,7 +60,7 @@ if(MSVC)
     # It is tipically better to use target_include_directories in place of
     # global include_directories, but in this case it is more compact to use include_directories
     include_directories(${OGRE_Paging_INCLUDE_DIRS})
-    
+
     # Workaround for https://github.com/robotology/gazebo-yarp-plugins/issues/482
     add_definitions(-D__TBB_NO_IMPLICIT_LINKAGE=1)
 endif()

plugins/controlboard/include/yarp/dev/ControlBoardDriver.h

@@ -18,6 +18,7 @@
 #include <yarp/dev/ControlBoardInterfaces.h>
 #include <yarp/dev/IControlMode.h>
 #include <yarp/dev/IInteractionMode.h>
+#include <yarp/dev/IJointCoupling.h>
 #include <yarp/dev/IRemoteVariables.h>
 #include <yarp/dev/IVirtualAnalogSensor.h>
 #include <yarp/sig/Vector.h>
@@ -372,6 +373,9 @@ class yarp::dev::GazeboYarpControlBoardDriver:
     std::vector<Range> m_jointPosLimits;
     std::vector<Range> m_jointVelLimits;
 
+    std::vector<Range> m_actuatedAxesPosLimits;
+    std::vector<Range> m_actuatedAxesVelLimits;
+
     /**
      * The zero position is the position of the GAZEBO joint that will be read as the starting one
      * i.e. getEncoder(j)=m_zeroPosition+gazebo.getEncoder(j);
@@ -410,11 +414,14 @@ class yarp::dev::GazeboYarpControlBoardDriver:
 
     //trajectory generator
     std::vector<TrajectoryGenerator*> m_trajectory_generator;
-    std::vector<BaseCouplingHandler*>  m_coupling_handler;
+    BaseCouplingHandler*     m_coupling_handler{nullptr};
     std::vector<RampFilter*> m_speed_ramp_handler;
     std::vector<Watchdog*> m_velocity_watchdog;
     std::vector<Watchdog*> m_velocityControl;
 
+    yarp::dev::IJointCoupling* m_ijointcoupling{nullptr};
+    yarp::dev::PolyDriver m_coupling_driver;
+
 
     yarp::sig::Vector m_trajectoryGenerationReferencePosition; /**< reference position for trajectory generation in position mode [Degrees] */
     yarp::sig::Vector m_trajectoryGenerationReferenceSpeed; /**< reference speed for trajectory generation in position mode [Degrees/Seconds]*/

plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h

@@ -223,61 +223,4 @@ class HandMk4CouplingHandler : public BaseCouplingHandler
     std::vector<double> pinkie_lut;
 };
 
-class HandMk5CouplingHandler : public BaseCouplingHandler
-{
-
-public:
-    HandMk5CouplingHandler (gazebo::physics::Model* model, yarp::sig::VectorOf<int> coupled_joints, std::vector<std::string> coupled_joint_names, std::vector<Range> coupled_joint_limits);
-
-public:
-
-    bool parseFingerParameters(yarp::os::Bottle& hand_params);
-    bool decouplePos (yarp::sig::Vector& current_pos);
-    bool decoupleVel (yarp::sig::Vector& current_vel);
-    bool decoupleAcc (yarp::sig::Vector& current_acc);
-    bool decoupleTrq (yarp::sig::Vector& current_trq);
-
-    yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
-    yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
-    yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
-
-private:
-    /**
-     * Parameters from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling
-     */
-    struct FingerParameters
-    {
-        double L0x;
-        double L0y;
-        double q2bias;
-        double q1off;
-        double k;
-        double d;
-        double l;
-        double b;
-    };
-
-    std::map<std::string, FingerParameters> mFingerParameters;
-
-    /*
-     * This method implements the law q2 = q2(q1) from
-     * https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling,
-     * i.e., the absolute angle of the distal joint q2 with respect to the palm.
-     *
-     * The inputs q1 and the return value of the function are in degrees.
-     */
-    double evaluateCoupledJoint(const double& q1, const std::string& finger_name);
-
-    /*
-     * This method implements the law \frac{\partial{q2}}{\partial{q1}} from
-     * https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling,
-     * i.e., the jacobian of the absolute angle of the distal joint q2 measured from the palm,
-     * with respect to the proximal joint q1.
-     *
-     * The input q1 is in degrees.
-     */
-    double evaluateCoupledJointJacobian(const double& q1, const std::string& finger_name);
-
-};
-
 #endif //GAZEBOYARP_COUPLING_H