From 7cbc2aa809eb80276a77f6fb8eb56ecb1013a35e Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 11 Jan 2023 16:26:53 +0100 Subject: [PATCH 1/7] ControlBoardDriverCoupling: add couplings for hand mk5 --- .../yarp/dev/ControlBoardDriverCoupling.h | 30 +- .../controlboard/src/ControlBoardDriver.cpp | 6 + .../src/ControlBoardDriverCoupling.cpp | 467 ++++++++++++++---- 3 files changed, 416 insertions(+), 87 deletions(-) diff --git a/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h b/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h index e5457da52..66d419baf 100644 --- a/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h +++ b/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& 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& lut); + + const int LUTSIZE; + + std::vector thumb_lut; + std::vector index_lut; + std::vector pinkie_lut; +}; + +class HandMk5CouplingHandler : public BaseCouplingHandler +{ + +public: + HandMk5CouplingHandler (gazebo::physics::Model* model, yarp::sig::VectorOf coupled_joints, std::vector coupled_joint_names, std::vector 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& lut); diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp index fde922b62..30cf2f5ec 100644 --- a/plugins/controlboard/src/ControlBoardDriver.cpp +++ b/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()=="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 hand_mk5"; + } else if (coupling_bottle->get(0).asString()=="none") { yCDebug(GAZEBOCONTROLBOARD) << "Just for test"; diff --git a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp index e56f2764b..650385784 100644 --- a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp +++ b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp @@ -610,14 +610,14 @@ HandMk3CouplingHandler::HandMk3CouplingHandler(gazebo::physics::Model* model, ya { const double RAD2DEG = 180.0/atan2(0.0,-1.0); const double DEG2RAD = 1.0/RAD2DEG; - + m_couplingSize = 11; - + thumb_lut.resize(LUTSIZE); index_lut.resize(LUTSIZE); - + std::vector num(LUTSIZE); - + for (int n = 0; n < LUTSIZE; ++n) { num[n] = 0.0; @@ -625,48 +625,48 @@ HandMk3CouplingHandler::HandMk3CouplingHandler(gazebo::physics::Model* model, ya } // thumb - { + { double L0x = -0.00555, L0y = 0.00195+0.0009; double P1x = 0.020, P1y = 0.0015; double L1x = P1x-0.0055-0.003, L1y = P1y-0.002+0.0019; - + double l2 = (P1x - L1x)*(P1x - L1x) + (P1y - L1y)*(P1y - L1y); double k2 = (L1x - L0x)*(L1x - L0x) + (L1y - L0y)*(L1y - L0y); - + double offset = RAD2DEG*atan2(L1y-P1y, L1x-P1x); - + 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) @@ -675,56 +675,56 @@ HandMk3CouplingHandler::HandMk3CouplingHandler(gazebo::physics::Model* model, ya } } } - + for (int n = 0; n < LUTSIZE; ++n) { num[n] = 0.0; index_lut[n] = 0.0; } - + // finger - { + { 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 = RAD2DEG*atan2(L1y-P1y, L1x-P1x); - + 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))); - + double q2 = alfa + beta - offset; - + while (q2 < 0.0) q2 += 360.0; while (q2 >= 360.0) q2 -= 360.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); - + // Construct LUT index_lut[iindex] += (1.0 - w)*q1; num[iindex] += (1.0 - w); - + index_lut[iindex + 1] += w*q1; num[iindex + 1] += w; } - + // 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) { @@ -759,7 +759,7 @@ bool HandMk3CouplingHandler::decoupleVel (yarp::sig::Vector& current_vel) } bool HandMk3CouplingHandler::decoupleAcc (yarp::sig::Vector& current_acc) -{ +{ if (m_coupledJoints.size()!=m_couplingSize) return false; 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]]; @@ -843,63 +843,63 @@ HandMk4CouplingHandler::HandMk4CouplingHandler(gazebo::physics::Model* model, ya { const double RAD2DEG = 180.0/atan2(0.0,-1.0); const double DEG2RAD = 1.0/RAD2DEG; - + m_couplingSize = 13; - + 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) @@ -909,54 +909,54 @@ HandMk4CouplingHandler::HandMk4CouplingHandler(gazebo::physics::Model* model, ya } } - // index, middle, ring + // 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))); - + double q2 = alfa + beta - offset; - + while (q2 < 0.0) q2 += 360.0; while (q2 >= 360.0) q2 -= 360.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); - + // Construct LUT index_lut[iindex] += (1.0 - w)*q1; num[iindex] += (1.0 - w); - + index_lut[iindex + 1] += w*q1; num[iindex + 1] += w; } - + // 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) { @@ -973,47 +973,47 @@ HandMk4CouplingHandler::HandMk4CouplingHandler(gazebo::physics::Model* model, ya 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; - + double l2 = (P1x - L1x)*(P1x - L1x) + (P1y - L1y)*(P1y - L1y); double k2 = (L1x - L0x)*(L1x - L0x) + (L1y - L0y)*(L1y - L0y); - + 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); - + 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; - + // 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); - + // Construct LUT pinkie_lut[iindex] += (1.0 - w)*q1; num[iindex] += (1.0 - w); - + pinkie_lut[iindex + 1] += w*q1; num[iindex + 1] += w; } - + // 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) { @@ -1048,7 +1048,7 @@ bool HandMk4CouplingHandler::decoupleVel (yarp::sig::Vector& current_vel) } bool HandMk4CouplingHandler::decoupleAcc (yarp::sig::Vector& current_acc) -{ +{ if (m_coupledJoints.size()!=m_couplingSize) return false; 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]]; @@ -1128,3 +1128,300 @@ yarp::sig::Vector HandMk4CouplingHandler::decoupleRefTrq (yarp::sig::Vector& trq out[m_coupledJoints[12]] = trq_ref[m_coupledJoints[6]] - out[m_coupledJoints[11]]; return out; } + +//------------------------------------------------------------------------------------------------------------------ +// HandMk5CouplingHandler +//------------------------------------------------------------------------------------------------------------------ + +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) +{ + 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))); + + double q2 = alfa + beta - offset; + + while (q2 < 0.0) q2 += 360.0; + while (q2 >= 360.0) q2 -= 360.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); + + // Construct LUT + index_lut[iindex] += (1.0 - w)*q1; + num[iindex] += (1.0 - w); + + index_lut[iindex + 1] += w*q1; + num[iindex + 1] += w; + } + + // 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]; + } + } + } + + // 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; + + double l2 = (P1x - L1x)*(P1x - L1x) + (P1y - L1y)*(P1y - L1y); + double k2 = (L1x - L0x)*(L1x - L0x) + (L1y - L0y)*(L1y - L0y); + + 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); + + 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; + + // 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); + + // Construct LUT + pinkie_lut[iindex] += (1.0 - w)*q1; + num[iindex] += (1.0 - w); + + pinkie_lut[iindex + 1] += w*q1; + num[iindex + 1] += w; + } + + // 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]; + } + } + } +} + +bool HandMk5CouplingHandler::decouplePos (yarp::sig::Vector& current_pos) +{ + 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; +} + +bool HandMk5CouplingHandler::decoupleVel (yarp::sig::Vector& current_vel) +{ + 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; +} + +bool HandMk5CouplingHandler::decoupleAcc (yarp::sig::Vector& current_acc) +{ + 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; +} + +bool HandMk5CouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq) +{ + if (m_coupledJoints.size()!=m_couplingSize) return false; + return false; +} + +double HandMk5CouplingHandler::decouple (double q2, std::vector& lut) +{ + + 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; +} + +yarp::sig::Vector HandMk5CouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref) +{ + 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; +} + +yarp::sig::Vector HandMk5CouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref) +{ + 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; +} + +yarp::sig::Vector HandMk5CouplingHandler::decoupleRefTrq (yarp::sig::Vector& trq_ref) +{ + 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; +} + From 84a2b6524068c86ced1b586f1a63b509c472dccc Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 11 Jan 2023 16:30:54 +0100 Subject: [PATCH 2/7] Update Changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0fd1c3424..8de267759 100644 --- a/CHANGELOG.md +++ b/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 `hand_mk5` coupling that models the mk5 hand (https://github.com/robotology/gazebo-yarp-plugins/pull/641). ## [4.5.2] - 2022-11-17 From 86bd1be7e1d2fc82b458b1e7b4cc7c8fd1ea3e57 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 11 Jan 2023 16:31:06 +0100 Subject: [PATCH 3/7] Trailing whitespaces --- CHANGELOG.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8de267759..9c8de3cf0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,7 +5,7 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo ## [Unreleased] ### Added -- In `gazebo_yarp_controlboard` add the `hand_mk5` coupling that models the mk5 hand (https://github.com/robotology/gazebo-yarp-plugins/pull/641). +- 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 @@ -22,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 @@ -35,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). @@ -58,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 From 4c46cfd95339ccdc84b4b99b8634d5b9c1cf1d19 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Fri, 13 Jan 2023 12:04:33 +0100 Subject: [PATCH 4/7] fix name of hand_mk5 coupling --- plugins/controlboard/src/ControlBoardDriver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp index 30cf2f5ec..ddb904265 100644 --- a/plugins/controlboard/src/ControlBoardDriver.cpp +++ b/plugins/controlboard/src/ControlBoardDriver.cpp @@ -337,11 +337,11 @@ bool GazeboYarpControlBoardDriver::gazebo_init() m_coupling_handler.push_back(cpl); yCInfo(GAZEBOCONTROLBOARD) << "using icub_left_hand_mk4"; } - else if (coupling_bottle->get(0).asString()=="hand_mk5") + 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 hand_mk5"; + yCInfo(GAZEBOCONTROLBOARD) << "using icub_hand_mk5"; } else if (coupling_bottle->get(0).asString()=="none") { From 85ecf2c7a9e9e33c80b381a115bab0755cc73ed0 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Fri, 13 Jan 2023 14:17:29 +0100 Subject: [PATCH 5/7] Cleanup CHANGELOG and bumpd to 4.6.0 --- CHANGELOG.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9c8de3cf0..49bba2568 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,8 +4,12 @@ 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] + +## [4.6.0] - 2023-01-13 + ### 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). + +- In `gazebo_yarp_controlboard` add the `icub_hand_mk5` coupling that models the iCub mk5 hand, that is used also in ergoCub 1 robot (https://github.com/robotology/gazebo-yarp-plugins/pull/641). ## [4.5.2] - 2022-11-17 From 7ead6f6426aecb7d421ec7f11c7086f96893f1c8 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Fri, 13 Jan 2023 14:18:17 +0100 Subject: [PATCH 6/7] Bump CMake version to 4.6.0 --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e7d447589..6c39aba26 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,8 +8,8 @@ option(GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS "if enabled removes # Project version set(${PROJECT_NAME}_MAJOR_VERSION 4) -set(${PROJECT_NAME}_MINOR_VERSION 5) -set(${PROJECT_NAME}_PATCH_VERSION 2) +set(${PROJECT_NAME}_MINOR_VERSION 6) +set(${PROJECT_NAME}_PATCH_VERSION 0) set(${PROJECT_NAME}_VERSION ${${PROJECT_NAME}_MAJOR_VERSION}.${${PROJECT_NAME}_MINOR_VERSION}.${${PROJECT_NAME}_PATCH_VERSION}) From 9982ab1d0a317d6b418b773f8d983836c833a8c8 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Fri, 13 Jan 2023 14:19:06 +0100 Subject: [PATCH 7/7] Cleanup conda-ci --- .github/workflows/conda-ci.yml | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/.github/workflows/conda-ci.yml b/.github/workflows/conda-ci.yml index adebef9d9..6d3d4b526 100644 --- a/.github/workflows/conda-ci.yml +++ b/.github/workflows/conda-ci.yml @@ -23,9 +23,8 @@ jobs: - uses: conda-incubator/setup-miniconda@v2 with: - mamba-version: "*" - channels: conda-forge,robotology - channel-priority: true + miniforge-variant: Mambaforge + miniforge-version: latest - uses: rlespinasse/github-slug-action@v3.x @@ -37,7 +36,7 @@ jobs: # Compilation related dependencies mamba install cmake compilers make ninja pkg-config # Actual dependencies - mamba install -c conda-forge -c robotology yarp gazebo + mamba install -c conda-forge yarp gazebo # Additional dependencies useful only on Linux - name: Dependencies [Conda/Linux]