ControlBoardDriverCoupling: add couplings for hand mk5

CHANGELOG.md

@@ -4,6 +4,8 @@ All notable changes to this project will be documented in this file.
 The format of this document is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
+### Added
+- In `gazebo_yarp_controlboard` add the `icub_hand_mk5` coupling that models the mk5 hand (https://github.com/robotology/gazebo-yarp-plugins/pull/641).
 
 ## [4.5.2] - 2022-11-17
 
@@ -20,7 +22,7 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 ### Added
 - The `gazebo_yarp_forcetorque` plugin now exposes also the `yarp::dev::ISixAxisForceTorqueSensors` interface, so it can be used with ` multipleanalogsensorsserver`, `multipleanalogsensorsremapper` and `multipleanalogsensorsclient` devices (https://github.com/robotology/gazebo-yarp-plugins/issues/384, https://github.com/robotology/gazebo-yarp-plugins/pull/628).
 
-### Fixed 
+### Fixed
 - Fix YARP 3.8 compatibility (https://github.com/robotology/gazebo-yarp-plugins/pull/627).
 
 ## [4.4.0] - 2022-05-31
@@ -33,12 +35,12 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ### Added
 - In `gazebo_yarp_camera` parse the `yarpDeviceName` option to enable its use with `gazebo_yarp_robotinterface` (https://github.com/robotology/gazebo-yarp-plugins/pull/614).
-- In `gazebo_yarp_controlboard` add the `icub_left_hand_mk4` coupling that models the iCub mk4 left hand (https://github.com/robotology/gazebo-yarp-plugins/pull/620). 
+- In `gazebo_yarp_controlboard` add the `icub_left_hand_mk4` coupling that models the iCub mk4 left hand (https://github.com/robotology/gazebo-yarp-plugins/pull/620).
 
 ### Changed
-- Migrate the example models under the tutorial directory to avoid the use of implicit network wrapper servers, and use the `gazebo_yarp_robotinterface` plugin to spawn their network wrapper servers (https://github.com/robotology/gazebo-yarp-plugins/pull/615 and https://github.com/robotology/gazebo-yarp-plugins/pull/616). 
+- Migrate the example models under the tutorial directory to avoid the use of implicit network wrapper servers, and use the `gazebo_yarp_robotinterface` plugin to spawn their network wrapper servers (https://github.com/robotology/gazebo-yarp-plugins/pull/615 and https://github.com/robotology/gazebo-yarp-plugins/pull/616).
 
-### Fixed 
+### Fixed
 - It is now possible to remove and add again to the simulation models that use `gazebo_yarp_robotinterface` without any crash (https://github.com/robotology/gazebo-yarp-plugins/pull/618, https://github.com/robotology/gazebo-yarp-plugins/pull/619).
 - Fixed value returned by getDeviceStatus method in `gazebo_yarp_laser` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/617).
 
@@ -56,12 +58,12 @@ disable any network wrapper server that the plugin created, by just creating the
 
 ## [4.1.2] - 2022-01-19
 
-### Fixed 
+### Fixed
 - Fix compilation against YARP 3.7 (https://github.com/robotology/gazebo-yarp-plugins/pull/607, https://github.com/robotology/gazebo-yarp-plugins/issues/608).
 
 ## [4.1.1] - 2022-01-13
 
-### Fixed 
+### Fixed
 - Fix compilation against Gazebo 11.10.0 (https://github.com/robotology/gazebo-yarp-plugins/pull/605, https://github.com/robotology/gazebo-yarp-plugins/issues/606).
 
 ## [4.1.0] - 2021-12-23

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

@@ -187,7 +187,7 @@ class HandMk3CouplingHandler : public BaseCouplingHandler
     yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
     yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
     yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
-    
+
 protected:
     double decouple (double q2, std::vector<double>& lut);
 
@@ -212,7 +212,33 @@ class HandMk4CouplingHandler : public BaseCouplingHandler
     yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
     yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
     yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
-
+
+protected:
+    double decouple (double q2, std::vector<double>& lut);
+
+    const int LUTSIZE;
+
+    std::vector<double> thumb_lut;
+    std::vector<double> index_lut;
+    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 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);
+    yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
+
 protected:
     double decouple (double q2, std::vector<double>& lut);
 

plugins/controlboard/src/ControlBoardDriver.cpp

@@ -337,6 +337,12 @@ bool GazeboYarpControlBoardDriver::gazebo_init()
                 m_coupling_handler.push_back(cpl);
                 yCInfo(GAZEBOCONTROLBOARD) << "using icub_left_hand_mk4";
             }
+            else if (coupling_bottle->get(0).asString()=="icub_hand_mk5")
+            {
+                BaseCouplingHandler* cpl = new HandMk5CouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits);
+                m_coupling_handler.push_back(cpl);
+                yCInfo(GAZEBOCONTROLBOARD) << "using icub_hand_mk5";
+            }
             else if (coupling_bottle->get(0).asString()=="none")
             {
                 yCDebug(GAZEBOCONTROLBOARD) << "Just for test";