gazebo_yarp_controlboard: Add coupling icub_left_hand_mk4 to model iCub mk4 left hand

CHANGELOG.md

@@ -7,6 +7,7 @@ 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). 
 
 ### 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). 

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

@@ -197,4 +197,30 @@ class HandMk3CouplingHandler : public BaseCouplingHandler
     std::vector<double> index_lut;
 };
 
+class HandMk4CouplingHandler : public BaseCouplingHandler
+{
+
+public:
+    HandMk4CouplingHandler (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);
+
+    const int LUTSIZE;
+
+    std::vector<double> thumb_lut;
+    std::vector<double> index_lut;
+    std::vector<double> pinkie_lut;
+};
+
 #endif //GAZEBOYARP_COUPLING_H

plugins/controlboard/src/ControlBoardDriver.cpp

@@ -175,7 +175,8 @@ bool GazeboYarpControlBoardDriver::gazebo_init()
         m_velocity_watchdog[j] = new Watchdog(0.200); //watchdog set to 200ms
     }
 
-    yCDebug(GAZEBOCONTROLBOARD) << "done";
+    yCDebug(GAZEBOCONTROLBOARD) << "Trajectory successfully generated.";
+
     for (size_t j = 0; j < m_numberOfJoints; ++j)
     {
         m_controlMode[j] = VOCAB_CM_POSITION;
@@ -330,6 +331,12 @@ bool GazeboYarpControlBoardDriver::gazebo_init()
                 m_coupling_handler.push_back(cpl);
                 yCInfo(GAZEBOCONTROLBOARD) << "using icub_hand_mk3";
             }
+            else if (coupling_bottle->get(0).asString()=="icub_left_hand_mk4")
+            {
+                BaseCouplingHandler* cpl = new HandMk4CouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits);
+                m_coupling_handler.push_back(cpl);
+                yCInfo(GAZEBOCONTROLBOARD) << "using icub_left_hand_mk4";
+            }
             else if (coupling_bottle->get(0).asString()=="none")
             {
                 yCDebug(GAZEBOCONTROLBOARD) << "Just for test";