From 6bcccbd8e90e62fdf3597a995aa2cc7a7a62f508 Mon Sep 17 00:00:00 2001 From: Nicola Piga Date: Wed, 17 May 2023 21:00:04 +0000 Subject: [PATCH 1/3] Update coupling handlers of ergoCub Hand MK5 --- .../yarp/dev/ControlBoardDriverCoupling.h | 150 +++++- .../controlboard/src/ControlBoardDriver.cpp | 2 +- .../src/ControlBoardDriverCoupling.cpp | 486 +++++++++--------- 3 files changed, 364 insertions(+), 274 deletions(-) diff --git a/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h b/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h index 66d419baf..f731eabd9 100644 --- a/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h +++ b/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& lut); - - const int LUTSIZE; - - std::vector thumb_lut; - std::vector index_lut; - std::vector 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 diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp index ddb904265..3ef749378 100644 --- a/plugins/controlboard/src/ControlBoardDriver.cpp +++ b/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); } } diff --git a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp index 650385784..72c929b42 100644 --- a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp +++ b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp @@ -152,7 +152,7 @@ yarp::sig::Vector EyesCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_re return out; } -yarp::sig::Vector EyesCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref) +yarp::sig::Vector EyesCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) { yarp::sig::Vector out = vel_ref; if (m_coupledJoints.size() != m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "EyesCouplingHandler: Invalid coupling vector"; return out;} @@ -218,7 +218,7 @@ yarp::sig::Vector ThumbCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_r return out; } -yarp::sig::Vector ThumbCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref) +yarp::sig::Vector ThumbCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) { yarp::sig::Vector out = vel_ref; if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "ThumbCouplingHandler: Invalid coupling vector"; return out;} @@ -287,7 +287,7 @@ yarp::sig::Vector IndexCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_r return out; } -yarp::sig::Vector IndexCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref) +yarp::sig::Vector IndexCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) { yarp::sig::Vector out = vel_ref; if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "IndexCouplingHandler: Invalid coupling vector"; return out;} @@ -354,7 +354,7 @@ yarp::sig::Vector MiddleCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ return out; } -yarp::sig::Vector MiddleCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref) +yarp::sig::Vector MiddleCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) { yarp::sig::Vector out = vel_ref; if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "MiddleCouplingHandler: Invalid coupling vector"; return out;} @@ -424,7 +424,7 @@ yarp::sig::Vector PinkyCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_r return out; } -yarp::sig::Vector PinkyCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref) +yarp::sig::Vector PinkyCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) { yarp::sig::Vector out = vel_ref; if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "PinkyCouplingHandler: Invalid coupling vector"; return out;} @@ -499,7 +499,7 @@ yarp::sig::Vector FingersAbductionCouplingHandler::decoupleRefPos (yarp::sig::Ve return out; } -yarp::sig::Vector FingersAbductionCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref) +yarp::sig::Vector FingersAbductionCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) { yarp::sig::Vector out = vel_ref; if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "FingersAbductionCouplingHandler: Invalid coupling vector"; return out;} @@ -579,7 +579,7 @@ yarp::sig::Vector CerHandCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos return out; } -yarp::sig::Vector CerHandCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref) +yarp::sig::Vector CerHandCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) { yarp::sig::Vector out = vel_ref; if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "CerHandCouplingHandler: Invalid coupling vector"; return out;} @@ -802,7 +802,7 @@ yarp::sig::Vector HandMk3CouplingHandler::decoupleRefPos (yarp::sig::Vector& pos return out; } -yarp::sig::Vector HandMk3CouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref) +yarp::sig::Vector HandMk3CouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) { yarp::sig::Vector out = vel_ref; if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "HandMk3CouplingHandler: Invalid coupling vector"; return out;} @@ -1093,7 +1093,7 @@ yarp::sig::Vector HandMk4CouplingHandler::decoupleRefPos (yarp::sig::Vector& pos return out; } -yarp::sig::Vector HandMk4CouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref) +yarp::sig::Vector HandMk4CouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) { yarp::sig::Vector out = vel_ref; if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "HandMk4CouplingHandler: Invalid coupling vector"; return out;} @@ -1134,294 +1134,272 @@ yarp::sig::Vector HandMk4CouplingHandler::decoupleRefTrq (yarp::sig::Vector& trq //------------------------------------------------------------------------------------------------------------------ HandMk5CouplingHandler::HandMk5CouplingHandler(gazebo::physics::Model* model, yarp::sig::VectorOf coupled_joints, std::vector coupled_joint_names, std::vector coupled_joint_limits) -: BaseCouplingHandler(model, coupled_joints,coupled_joint_names, coupled_joint_limits), LUTSIZE(4096) +: BaseCouplingHandler(model, coupled_joints,coupled_joint_names, coupled_joint_limits) { - const double RAD2DEG = 180.0/atan2(0.0,-1.0); - const double DEG2RAD = 1.0/RAD2DEG; - m_couplingSize = 12; +} - thumb_lut.resize(LUTSIZE); - index_lut.resize(LUTSIZE); - pinkie_lut.resize(LUTSIZE); - - std::vector num(LUTSIZE); - - // thumb - for (int n = 0; n < LUTSIZE; ++n) - { - num[n] = 0.0; - thumb_lut[n] = 0.0; - } - { - double L0x = -0.00555, L0y = 0.00285; - double P1x = 0.02, P1y = 0.0015; - double L1x = 0.0115, L1y = 0.0015; - - double l2 = (P1x - L1x)*(P1x - L1x) + (P1y - L1y)*(P1y - L1y); - double k2 = (L1x - L0x)*(L1x - L0x) + (L1y - L0y)*(L1y - L0y); - - double offset = 180; - - for (double q1 = 0.0; q1 <= 85.5; q1 += 0.01) - { - double cq1 = cos(DEG2RAD*q1); - double sq1 = sin(DEG2RAD*q1); - - double P1xr = cq1*P1x-sq1*P1y; - double P1yr = sq1*P1x+cq1*P1y; - - double h2 = (P1xr - L0x)*(P1xr - L0x) + (P1yr - L0y)*(P1yr - L0y); - - double alfa = RAD2DEG*atan2(L0y - P1yr, L0x - P1xr); - - double beta = RAD2DEG*acos((h2 + l2 - k2)/(2.0*sqrt(h2*l2))); - - double q2 = alfa + beta - offset; - - while (q2 < 0.0) q2 += 360.0; - while (q2 >= 360.0) q2 -= 360.0; - - double dindex = q2*10.0; - - int iindex = int(dindex); - - double w = dindex - double(iindex); - - thumb_lut[iindex] += (1.0 - w)*q1; - num[iindex] += (1.0 - w); - - thumb_lut[iindex + 1] += w*q1; - num[iindex + 1] += w; - } - - for (int n = 0; n < LUTSIZE; ++n) - { - if (num[n] > 0.0) - { - thumb_lut[n] /= num[n]; - } - } - } - - - // index, middle, ring - for (int n = 0; n < LUTSIZE; ++n) - { - num[n] = 0.0; - index_lut[n] = 0.0; - } - { - double P1x = 0.0300, P1y = 0.0015; - double L0x = -0.0050, L0y = 0.0040; - double L1x = 0.0240, L1y = 0.0008; - - double l2 = (P1x - L1x)*(P1x - L1x) + (P1y - L1y)*(P1y - L1y); - double k2 = (L1x - L0x)*(L1x - L0x) + (L1y - L0y)*(L1y - L0y); - - double offset = 173.35; - - for (double q1 = 0.0; q1 <= 95.5; q1 += 0.01) - { - double cq1 = cos(DEG2RAD*q1); - double sq1 = sin(DEG2RAD*q1); - - double P1xr = cq1*P1x-sq1*P1y; - double P1yr = sq1*P1x+cq1*P1y; - - double h2 = (P1xr - L0x)*(P1xr - L0x) + (P1yr - L0y)*(P1yr - L0y); - - double alfa = RAD2DEG*atan2(L0y - P1yr, L0x - P1xr); - - double beta = RAD2DEG*acos((h2 + l2 - k2)/(2.0*sqrt(h2*l2))); +bool HandMk5CouplingHandler::decouplePos (yarp::sig::Vector& current_pos) +{ + if (m_coupledJoints.size()!=m_couplingSize) return false; - double q2 = alfa + beta - offset; + const yarp::sig::Vector tmp = current_pos; + + /* thumb_add <-- thumb_add */ + current_pos[m_coupledJoints[0]] = tmp[0]; + /* thumb_oc <-- thumb_prox */ + current_pos[m_coupledJoints[1]] = tmp[1]; + /* index_add <-- index_add */ + current_pos[m_coupledJoints[2]] = tmp[3]; + /* index_oc <-- index_prox */ + current_pos[m_coupledJoints[3]] = tmp[4]; + /* middle_oc <-- middle_prox */ + current_pos[m_coupledJoints[4]] = tmp[6]; + /** + * ring_pinky_oc <-- pinkie_prox + * as, on the real robot, the coupled group composed of ring_prox, ring_dist, pinkie_prox and pinkie_dist + * is controlled using the encoder on the pinkie_prox as feedback + */ + current_pos[m_coupledJoints[5]] = tmp[10]; - while (q2 < 0.0) q2 += 360.0; - while (q2 >= 360.0) q2 -= 360.0; + return true; +} - // get decimal part of q2 to find out how to weigh index and index+1 - double dindex = q2*10.0; - int iindex = int(dindex); - double w = dindex - double(iindex); +bool HandMk5CouplingHandler::decoupleVel (yarp::sig::Vector& current_vel) +{ + if (m_coupledJoints.size()!=m_couplingSize) return false; - // Construct LUT - index_lut[iindex] += (1.0 - w)*q1; - num[iindex] += (1.0 - w); + const yarp::sig::Vector tmp = current_vel; + + /* thumb_add <-- thumb_add */ + current_vel[m_coupledJoints[0]] = tmp[0]; + /* thumb_oc <-- thumb_prox */ + current_vel[m_coupledJoints[1]] = tmp[1]; + /* index_add <-- index_add */ + current_vel[m_coupledJoints[2]] = tmp[3]; + /* index_oc <-- index_prox */ + current_vel[m_coupledJoints[3]] = tmp[4]; + /* middle_oc <-- middle_prox */ + current_vel[m_coupledJoints[4]] = tmp[6]; + /** + * ring_pinky_oc <-- pinkie_prox + * as, on the real robot, the coupled group composed of ring_prox, ring_dist, pinkie_prox and pinkie_dist + * is controlled using the encoder on the pinkie_prox as feedback + */ + current_vel[m_coupledJoints[5]] = tmp[10]; - index_lut[iindex + 1] += w*q1; - num[iindex + 1] += w; - } + return true; +} - // divide each value in the LUT by the weight if it is greater than 0 to extract q1 - for (int n = 0; n < LUTSIZE; ++n) - { - if (num[n] > 0.0) - { - index_lut[n] /= num[n]; - } - } - } +bool HandMk5CouplingHandler::decoupleAcc (yarp::sig::Vector& current_acc) +{ + /** + * Acceleration control not available for fingers on the real robot. + * Note: this method is never called within the controlboard plugin code. + */ + return false; +} - // pinkie - for (int n = 0; n < LUTSIZE; ++n) - { - num[n] = 0.0; - pinkie_lut[n] = 0.0; - } - { - double P1x = 0.0250, P1y = 0.0015; - double L0x = -0.0050, L0y = 0.0040; - double L1x = 0.0190, L1y = 0.0005; +bool HandMk5CouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq) +{ + /** + * Torque control not available for fingers on the real robot. + */ + return false; +} - double l2 = (P1x - L1x)*(P1x - L1x) + (P1y - L1y)*(P1y - L1y); - double k2 = (L1x - L0x)*(L1x - L0x) + (L1y - L0y)*(L1y - L0y); +yarp::sig::Vector HandMk5CouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref) +{ + if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "HandMk5CouplingHandler: Invalid coupling vector"; return pos_ref;} + + yarp::sig::Vector out(pos_ref.size()); + + /* thumb_add <-- thumb_add */ + out[0] = pos_ref[m_coupledJoints[0]]; + /* thumb_prox <-- thumb_oc */ + out[1] = pos_ref[m_coupledJoints[1]]; + /* thumb_dist <-- coupling_law(thumb_prox) */ + out[2] = evaluateCoupledJointThumb(out[1]); + /* index_add <-- index_add */ + out[3] = pos_ref[m_coupledJoints[2]]; + /* index_prox <-- index_oc */ + out[4] = pos_ref[m_coupledJoints[3]]; + /* index_dist <-- coupling_law(index_prox) */ + out[5] = evaluateCoupledJointIndex(out[4]); + /* middle_prox <-- middle_oc */ + out[6] = pos_ref[m_coupledJoints[4]]; + /* middle_dist <-- coupling_law(middle_prox) */ + out[7] = evaluateCoupledJointIndex(out[6]); + /* ring_prox <-- ring_pinky_oc */ + out[8] = pos_ref[m_coupledJoints[5]]; + /* ring_dist <-- coupling_law(ring_prox) */ + out[9] = evaluateCoupledJointIndex(out[8]); + /* pinky_prox <-- ring_pinky_oc */ + out[10] = pos_ref[m_coupledJoints[5]]; + /* pinky_dist <-- coupling_law(pinky_prox) */ + out[11] = evaluateCoupledJointPinky(out[10]); - double offset = 170.54; - for (double q1 = 0.0; q1 <= 95.5; q1 += 0.01) - { - double cq1 = cos(DEG2RAD*q1); - double sq1 = sin(DEG2RAD*q1); + return out; +} - double P1xr = cq1*P1x-sq1*P1y; - double P1yr = sq1*P1x+cq1*P1y; - double h2 = (P1xr - L0x)*(P1xr - L0x) + (P1yr - L0y)*(P1yr - L0y); +yarp::sig::Vector HandMk5CouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) +{ + if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "HandMk5CouplingHandler: Invalid coupling vector"; return vel_ref;} + + /** + * Extract the current position of proximal joints from pos_feedback. + */ + double lastThumbProx = pos_feedback[1]; + double lastIndexProx = pos_feedback[4]; + double lastMiddleProx = pos_feedback[6]; + double lastRingProx = pos_feedback[8]; + double lastPinkyProx = pos_feedback[10]; + + /** + * In the following, we use the fact that: + * /dot{distal_joint} = \partial{distal_joint}{proximal_joint} \dot{proximal_joint}. + */ + + yarp::sig::Vector out(vel_ref.size()); + + /* thumb_add <-- thumb_add */ + out[0] = vel_ref[m_coupledJoints[0]]; + /* thumb_prox <-- thumb_oc */ + out[1] = vel_ref[m_coupledJoints[1]]; + /* thumb_dist <-- coupling_law_jacobian(thumb_prox_position) * thumb_prox */ + out[2] = evaluateCoupledJointJacobianThumb(lastThumbProx) * out[1]; + /* index_add <-- index_add */ + out[3] = vel_ref[m_coupledJoints[2]]; + /* index_prox <-- index_oc */ + out[4] = vel_ref[m_coupledJoints[3]]; + /* index_dist <-- coupling_law_jacobian(index_prox_position) * index_prox */ + out[5] = evaluateCoupledJointJacobianIndex(lastIndexProx) * out[4]; + /* middle_prox <-- middle_oc */ + out[6] = vel_ref[m_coupledJoints[4]]; + /* middle_dist <-- coupling_law_jacobian(middle_prox_position) * middle_prox */ + out[7] = evaluateCoupledJointJacobianIndex(lastMiddleProx) * out[6]; + /* ring_prox <-- ring_pinky_oc */ + out[8] = vel_ref[m_coupledJoints[5]]; + /* ring_dist <-- coupling_law_jacobian(ring_prox_position) * ring_prox */ + out[9] = evaluateCoupledJointJacobianIndex(lastRingProx) * out[8]; + /* pinky_prox <-- ring_pinky_oc */ + out[10] = vel_ref[m_coupledJoints[5]]; + /* pinky_dist <-- coupling_law(pinky_prox) */ + out[11] = evaluateCoupledJointJacobianPinky(lastPinkyProx) * out[10]; - double alfa = RAD2DEG*atan2(L0y - P1yr, L0x - P1xr); + return out; +} - double beta = RAD2DEG*acos((h2 + l2 - k2)/(2.0*sqrt(h2*l2))); +yarp::sig::Vector HandMk5CouplingHandler::decoupleRefTrq (yarp::sig::Vector& trq_ref) +{ + /** + * Torque control not available for fingers on the real robot. + */ + return trq_ref; +} - double q2 = alfa + beta - offset; +double HandMk5CouplingHandler::evaluateCoupledJoint(const double& q1, const FingerParameters& params) +{ + /** + * Coupling law taken from from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling + */ - while (q2 < 0.0) q2 += 360.0; - while (q2 >= 360.0) q2 -= 360.0; + double q1_rad = q1 * M_PI / 180.0; + double q1off_rad = params.q1off * M_PI / 180.0; + double q2bias_rad = params.q2bias * M_PI / 180.0; - // get decimal part of q2 to find out how to weigh index and index+1 - double dindex = q2*10.0; - int iindex = int(dindex); - double w = dindex - double(iindex); + double P1x_q1 = params.d * cos(q1_rad + q1off_rad); + double P1y_q1 = params.d * sin(q1_rad + q1off_rad); - // Construct LUT - pinkie_lut[iindex] += (1.0 - w)*q1; - num[iindex] += (1.0 - w); + double h_sq = std::pow(P1x_q1 - params.L0x, 2) + std::pow(P1y_q1 - params.L0y, 2); + double h = std::sqrt(h_sq); + double l_sq = std::pow(params.l, 2); + double k_sq = std::pow(params.k, 2); - pinkie_lut[iindex + 1] += w*q1; - num[iindex + 1] += w; - } + double q2 = atan2(P1y_q1 - params.L0y, P1x_q1 - params.L0x) + \ + acos((h_sq + l_sq - k_sq) / (2.0 * params.l * h)) + \ + q2bias_rad - M_PI; - // divide each value in the LUT by the weight if it is greater than 0 to extract q1 - for (int n = 0; n < LUTSIZE; ++n) - { - if (num[n] > 0.0) - { - pinkie_lut[n] /= num[n]; - } - } - } + return q2 * 180.0 / M_PI; } -bool HandMk5CouplingHandler::decouplePos (yarp::sig::Vector& current_pos) +double HandMk5CouplingHandler::evaluateCoupledJointThumb(const double& q1) { - if (m_coupledJoints.size()!=m_couplingSize) return false; - current_pos[m_coupledJoints[1]] = current_pos[m_coupledJoints[1]] + current_pos[m_coupledJoints[2]]; - current_pos[m_coupledJoints[2]] = current_pos[m_coupledJoints[3]]; - current_pos[m_coupledJoints[3]] = current_pos[m_coupledJoints[4]] + current_pos[m_coupledJoints[5]]; - current_pos[m_coupledJoints[4]] = current_pos[m_coupledJoints[6]] + current_pos[m_coupledJoints[7]]; - current_pos[m_coupledJoints[5]] = current_pos[m_coupledJoints[8]] + current_pos[m_coupledJoints[9]]; - return true; + /** + * The value of q1 is subtracted from the result as evaluateCoupledJoint + * provides the absolute angle of the coupled distal joint with respect to the palm. + */ + return evaluateCoupledJoint(q1, mParamsThumb) - q1; } -bool HandMk5CouplingHandler::decoupleVel (yarp::sig::Vector& current_vel) +double HandMk5CouplingHandler::evaluateCoupledJointIndex(const double& q1) { - if (m_coupledJoints.size()!=m_couplingSize) return false; - current_vel[m_coupledJoints[1]] = current_vel[m_coupledJoints[1]] + current_vel[m_coupledJoints[2]]; - current_vel[m_coupledJoints[2]] = current_vel[m_coupledJoints[3]]; - current_vel[m_coupledJoints[3]] = current_vel[m_coupledJoints[4]] + current_vel[m_coupledJoints[5]]; - current_vel[m_coupledJoints[4]] = current_vel[m_coupledJoints[6]] + current_vel[m_coupledJoints[7]]; - current_vel[m_coupledJoints[5]] = current_vel[m_coupledJoints[8]] + current_vel[m_coupledJoints[9]]; - return true; + /** + * The value of q1 is subtracted from the result as evaluateCoupledJoint + * provides the absolute angle of the coupled distal joint with respect to the palm. + */ + return evaluateCoupledJoint(q1, mParamsIndex) - q1; } -bool HandMk5CouplingHandler::decoupleAcc (yarp::sig::Vector& current_acc) +double HandMk5CouplingHandler::evaluateCoupledJointPinky(const double& q1) { - if (m_coupledJoints.size()!=m_couplingSize) return false; - current_acc[m_coupledJoints[1]] = current_acc[m_coupledJoints[1]] + current_acc[m_coupledJoints[2]]; - current_acc[m_coupledJoints[2]] = current_acc[m_coupledJoints[3]]; - current_acc[m_coupledJoints[3]] = current_acc[m_coupledJoints[4]] + current_acc[m_coupledJoints[5]]; - current_acc[m_coupledJoints[4]] = current_acc[m_coupledJoints[6]] + current_acc[m_coupledJoints[7]]; - current_acc[m_coupledJoints[5]] = current_acc[m_coupledJoints[8]] + current_acc[m_coupledJoints[9]]; - return true; + /** + * The value of q1 is subtracted from the result as evaluateCoupledJoint + * provides the absolute angle of the coupled distal joint with respect to the palm. + */ + return evaluateCoupledJoint(q1, mParamsPinky) - q1; } -bool HandMk5CouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq) +double HandMk5CouplingHandler::evaluateCoupledJointJacobian(const double& q1, const FingerParameters& params) { - if (m_coupledJoints.size()!=m_couplingSize) return false; - return false; -} + /** + * Coupling law jacobian taken from from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling + */ -double HandMk5CouplingHandler::decouple (double q2, std::vector& lut) -{ + double q1_rad = q1 * M_PI / 180.0; + double q1off_rad = params.q1off * M_PI / 180.0; - double dindex = q2*10.0; - int iindex = int(dindex); - // get decimal part of q2 to find out how to weigh index and index+1 - double w = dindex - double(iindex); - // interpolate between index and the next with a convex combination weighting - return lut[iindex]*(1.0 - w) + lut[iindex + 1]*w; + double P1x_q1 = params.d * cos(q1_rad + q1off_rad); + double P1y_q1 = params.d * sin(q1_rad + q1off_rad); + + double h_sq = std::pow(P1x_q1 - params.L0x, 2) + std::pow(P1y_q1 - params.L0y, 2); + double h = std::sqrt(h_sq); + double l_sq = std::pow(params.l, 2); + double k_sq = std::pow(params.k, 2); + + double dq2_dq1_11 = 1; + double dq2_dq1_21 = 2 - (std::pow(params.d, 2) - std::pow(params.b, 2)) / (std::pow(params.d, 2) - (params.L0x * P1x_q1 + params.L0y * P1y_q1)); + double dq2_dq1_12 = (params.L0x * P1y_q1 - params.L0y * P1x_q1) * (l_sq - k_sq - h_sq); + double dq2_dq1_22 = 2 * params.l * h * h_sq * std::sqrt(1 - std::pow((l_sq - k_sq + h_sq) / (2 * params.l * h), 2)); + double dq2_dq1 = dq2_dq1_11 / dq2_dq1_21 + dq2_dq1_12 / dq2_dq1_22; + + return dq2_dq1; } -yarp::sig::Vector HandMk5CouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref) +double HandMk5CouplingHandler::evaluateCoupledJointJacobianThumb(const double& q1) { - yarp::sig::Vector out = pos_ref; - if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "HandMk5CouplingHandler: Invalid coupling vector"; return out;} - out[m_coupledJoints[1]] = decouple(pos_ref[m_coupledJoints[1]], thumb_lut); - out[m_coupledJoints[2]] = pos_ref[m_coupledJoints[1]] - out[m_coupledJoints[1]]; - out[m_coupledJoints[3]] = pos_ref[m_coupledJoints[2]]; - out[m_coupledJoints[4]] = decouple(pos_ref[m_coupledJoints[3]], index_lut); - out[m_coupledJoints[5]] = pos_ref[m_coupledJoints[3]] - out[m_coupledJoints[4]]; - out[m_coupledJoints[6]] = decouple(pos_ref[m_coupledJoints[4]], index_lut); - out[m_coupledJoints[7]] = pos_ref[m_coupledJoints[4]] - out[m_coupledJoints[6]]; - out[m_coupledJoints[8]] = decouple(pos_ref[m_coupledJoints[5]], index_lut); - out[m_coupledJoints[9]] = pos_ref[m_coupledJoints[5]] - out[m_coupledJoints[8]]; - out[m_coupledJoints[10]] = decouple(pos_ref[m_coupledJoints[5]], pinkie_lut);; - out[m_coupledJoints[11]] = pos_ref[m_coupledJoints[5]] - out[m_coupledJoints[10]]; - return out; + /** + * The value of 1 is subtracted from the result as evaluateCoupledJointJacobian + * provides the jacobian of the absolute angle of the coupled distal joint. + */ + return evaluateCoupledJointJacobian(q1, mParamsThumb) - 1; } -yarp::sig::Vector HandMk5CouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref) +double HandMk5CouplingHandler::evaluateCoupledJointJacobianIndex(const double& q1) { - yarp::sig::Vector out = vel_ref; - if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "HandMk5CouplingHandler: Invalid coupling vector"; return out;} - out[m_coupledJoints[1]] = decouple(vel_ref[m_coupledJoints[1]], thumb_lut); - out[m_coupledJoints[2]] = vel_ref[m_coupledJoints[1]] - out[m_coupledJoints[1]]; - out[m_coupledJoints[3]] = vel_ref[m_coupledJoints[2]]; - out[m_coupledJoints[4]] = decouple(vel_ref[m_coupledJoints[3]], index_lut); - out[m_coupledJoints[5]] = vel_ref[m_coupledJoints[3]] - out[m_coupledJoints[4]]; - out[m_coupledJoints[6]] = decouple(vel_ref[m_coupledJoints[4]], index_lut); - out[m_coupledJoints[7]] = vel_ref[m_coupledJoints[4]] - out[m_coupledJoints[6]]; - out[m_coupledJoints[8]] = decouple(vel_ref[m_coupledJoints[5]], index_lut); - out[m_coupledJoints[9]] = vel_ref[m_coupledJoints[5]] - out[m_coupledJoints[8]]; - out[m_coupledJoints[10]] = decouple(vel_ref[m_coupledJoints[5]], pinkie_lut);; - out[m_coupledJoints[11]] = vel_ref[m_coupledJoints[5]] - out[m_coupledJoints[10]]; - return out; + /** + * The value of 1 is subtracted from the result as evaluateCoupledJointJacobian + * provides the jacobian of the absolute angle of the coupled distal joint. + */ + return evaluateCoupledJointJacobian(q1, mParamsIndex) - 1; } -yarp::sig::Vector HandMk5CouplingHandler::decoupleRefTrq (yarp::sig::Vector& trq_ref) +double HandMk5CouplingHandler::evaluateCoupledJointJacobianPinky(const double& q1) { - yarp::sig::Vector out =trq_ref; - if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "HandMk5CouplingHandler: Invalid coupling vector"; return out;} - out[m_coupledJoints[1]] = decouple(trq_ref[m_coupledJoints[1]], thumb_lut); - out[m_coupledJoints[2]] = trq_ref[m_coupledJoints[1]] - out[m_coupledJoints[1]]; - out[m_coupledJoints[3]] = trq_ref[m_coupledJoints[2]]; - out[m_coupledJoints[4]] = decouple(trq_ref[m_coupledJoints[3]], index_lut); - out[m_coupledJoints[5]] = trq_ref[m_coupledJoints[3]] - out[m_coupledJoints[4]]; - out[m_coupledJoints[6]] = decouple(trq_ref[m_coupledJoints[4]], index_lut); - out[m_coupledJoints[7]] = trq_ref[m_coupledJoints[4]] - out[m_coupledJoints[6]]; - out[m_coupledJoints[8]] = decouple(trq_ref[m_coupledJoints[5]], index_lut); - out[m_coupledJoints[9]] = trq_ref[m_coupledJoints[5]] - out[m_coupledJoints[8]]; - out[m_coupledJoints[10]] = decouple(trq_ref[m_coupledJoints[5]], pinkie_lut);; - out[m_coupledJoints[11]] = trq_ref[m_coupledJoints[5]] - out[m_coupledJoints[10]]; - return out; + /** + * The value of 1 is subtracted from the result as evaluateCoupledJointJacobian + * provides the jacobian of the absolute angle of the coupled distal joint. + */ + return evaluateCoupledJointJacobian(q1, mParamsPinky) - 1; } - From cc8bea0f084c1661705e08d93c802c4dde5fb27a Mon Sep 17 00:00:00 2001 From: Nicola Piga Date: Wed, 17 May 2023 22:08:17 +0000 Subject: [PATCH 2/3] Use C++20 compiler features in gazebo_yarp_controlboard --- plugins/controlboard/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/plugins/controlboard/CMakeLists.txt b/plugins/controlboard/CMakeLists.txt index 0be197476..21cd8c0e5 100644 --- a/plugins/controlboard/CMakeLists.txt +++ b/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) From 27de5f489774ebf4ef24f9fbb333eb5c99dd717b Mon Sep 17 00:00:00 2001 From: Nicola Piga Date: Thu, 18 May 2023 07:50:53 +0000 Subject: [PATCH 3/3] Update CHANGELOG.md --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6be955b86..fca2fa658 100644 --- a/CHANGELOG.md +++ b/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