Update coupling handlers of ergoCub Hand MK5

CHANGELOG.md

@@ -8,10 +8,13 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 ### Changed
 
 - The `gazebo_imu` plugin used to output orientation measurements as if the sensor was zero-aligned with the world frame, regardless of the initial orientation of the part the sensor is attached to. To match most real-world IMUs, the default behavior was changed to allow measuring orientation with regard to the world frame at all times. The old behavior can be restored by setting the `useInitialSensorOrientationAsReference` config option (https://github.com/robotology/gazebo-yarp-plugins/pull/639).
+- The `gazebo_yarp_controlboard` plugin is compiled using C++ 20 compiler features (https://github.com/robotology/gazebo-yarp-plugins/pull/650).
+- The `BaseCouplingHandler::decoupleVelRef()` methods within `gazebo_yarp_controlboard` provides an input argument containing the full joints position feedback, to be used to implement velocity decoupling laws that depend on the joints position.
 
 ### Fixed
 
 - Fix wrong install include for gazebo_yarp_lib_common library (https://github.com/robotology/gazebo-yarp-plugins/pull/644).
+- Fix wrong implementation of coupling handlers of ergoCub MK5 hands within the `gazebo_yarp_controlboard` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/650).
 
 ## [4.6.0] - 2023-01-13
 

plugins/controlboard/CMakeLists.txt

@@ -47,3 +47,4 @@ add_gazebo_yarp_plugin_target(LIBRARY_NAME controlboard
                               LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES}
                               HEADERS ${controlBoard_headers}
                               SOURCES ${controlBoard_source})
+target_compile_features(gazebo_yarp_controlboard PUBLIC cxx_std_20)

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

@@ -46,7 +46,7 @@ class BaseCouplingHandler
     virtual bool checkJointIsCoupled(int joint);
 
     virtual yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref) = 0;
-    virtual yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref) = 0;
+    virtual yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) = 0;
     virtual yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref) = 0;
 
     virtual void setCoupledJointLimit(int joint, const double& min, const double& max);
@@ -66,7 +66,7 @@ class EyesCouplingHandler : public BaseCouplingHandler
     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 decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
     yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
 };
 
@@ -83,7 +83,7 @@ class ThumbCouplingHandler : public BaseCouplingHandler
     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 decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
     yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
 };
 
@@ -100,7 +100,7 @@ class IndexCouplingHandler : public BaseCouplingHandler
     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 decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
     yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
 };
 
@@ -117,7 +117,7 @@ class MiddleCouplingHandler : public BaseCouplingHandler
     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 decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
     yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
 };
 
@@ -134,7 +134,7 @@ class PinkyCouplingHandler : public BaseCouplingHandler
     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 decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
     yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
 };
 
@@ -151,7 +151,7 @@ class FingersAbductionCouplingHandler : public BaseCouplingHandler
     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 decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
     yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
 };
 
@@ -168,7 +168,7 @@ class CerHandCouplingHandler : public BaseCouplingHandler
     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 decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
     yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
 };
 
@@ -185,7 +185,7 @@ class HandMk3CouplingHandler : public BaseCouplingHandler
     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 decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
     yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
 
 protected:
@@ -210,7 +210,7 @@ class HandMk4CouplingHandler : public BaseCouplingHandler
     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 decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
     yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
 
 protected:
@@ -236,17 +236,129 @@ class HandMk5CouplingHandler : public BaseCouplingHandler
     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 decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
     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;
+private:
+    /**
+     * Parameters from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling
+     */
+    struct FingerParameters
+    {
+        double L0x;
+        double L0y;
+        double L1x;
+        double L1y;
+        double q2bias;
+        double q1off;
+        double k;
+        double d;
+        double l;
+        double b;
+    };
+
+    const FingerParameters mParamsThumb =
+    {
+        .L0x = -0.00555,
+        .L0y = 0.00285,
+        .q2bias = 180.0,
+        .q1off = 4.29,
+        .k = 0.0171,
+        .d = 0.02006,
+        .l = 0.0085,
+        .b = 0.00624
+    };
+
+    const FingerParameters mParamsIndex =
+    {
+        .L0x = -0.0050,
+        .L0y = 0.0040,
+        .q2bias = 173.35,
+        .q1off = 2.86,
+        .k = 0.02918,
+        .d = 0.03004,
+        .l = 0.00604,
+        .b = 0.0064
+    };
+
+    const FingerParameters mParamsPinky =
+    {
+        .L0x = -0.0050,
+        .L0y = 0.0040,
+        .q2bias = 170.54,
+        .q1off = 3.43,
+        .k = 0.02425,
+        .d = 0.02504,
+        .l = 0.00608,
+        .b = 0.0064
+    };
+
+    /*
+     * 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 FingerParameters& parameters);
+
+    /*
+     * This method evalutes the relative angle between the proximal and distal joints of the thumb finger.
+     *
+     * The input q1 and the return value of the function are in degrees.
+     */
+    double evaluateCoupledJointThumb(const double& q1);
+
+    /*
+     * This method evalutes the relative angle between the proximal and distal joints of the index finger.
+     * This is also valid for the middle and ring fingers.
+     *
+     * The input q1 and the return value of the function are in degrees.
+     */
+    double evaluateCoupledJointIndex(const double& q1);
+
+    /*
+     * This method evalutes the relative angle between the proximal and distal joints of the pinky finger.
+     *
+     * The input q1 and the return value of the function are in degrees.
+     */
+    double evaluateCoupledJointPinky(const double& q1);
+
+    /*
+     * 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 FingerParameters& parameters);
+
+    /*
+     * This method evalutes the jacobian of the relative angle between the proximal and distal joints of the thumb finger
+     * with respect to the proximal joint.
+     *
+     * The input q1 is in degrees.
+     */
+    double evaluateCoupledJointJacobianThumb(const double& q1);
+
+    /*
+     * This method evalutes the jacobian of the relative angle between the proximal and distal joints of the index finger
+     * with respect to the proximal joint.
+     *
+     * This is also valid for the middle and ring fingers.
+     *
+     * The input q1 is in degrees.
+     */
+    double evaluateCoupledJointJacobianIndex(const double& q1);
+
+    /*
+     * This method evalutes the jacobian of the relative angle between the proximal and distal joints of the pinky finger
+     * with respect to the proximal joint.
+     *
+     * The input q1 is in degrees.
+     */
+    double evaluateCoupledJointJacobianPinky(const double& q1);
 };
 
 #endif //GAZEBOYARP_COUPLING_H

plugins/controlboard/src/ControlBoardDriver.cpp

@@ -646,7 +646,7 @@ void GazeboYarpControlBoardDriver::onUpdate(const gazebo::common::UpdateInfo& _i
         if (m_coupling_handler[cpl_cnt])
         {
             m_motReferencePositions = m_coupling_handler[cpl_cnt]->decoupleRefPos(m_jntReferencePositions);
-            m_motReferenceVelocities = m_coupling_handler[cpl_cnt]->decoupleRefVel(m_jntReferenceVelocities);
+            m_motReferenceVelocities = m_coupling_handler[cpl_cnt]->decoupleRefVel(m_jntReferenceVelocities, m_motPositions);
             m_motReferenceTorques = m_coupling_handler[cpl_cnt]->decoupleRefTrq(m_jntReferenceTorques);
         }
     }