From 196eef92bc9859ad92ef2c3a9b2e39fe1738d946 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 7 Sep 2023 15:57:00 +0200 Subject: [PATCH 1/4] HandMk5CouplingHandler: refactor it in order to make it configurable It fixes #661 --- .../yarp/dev/ControlBoardDriverCoupling.h | 91 +------------- plugins/controlboard/src/ControlBoard.cc | 26 ++-- .../controlboard/src/ControlBoardDriver.cpp | 13 ++ .../src/ControlBoardDriverCoupling.cpp | 117 +++++++----------- 4 files changed, 79 insertions(+), 168 deletions(-) diff --git a/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h b/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h index f731eabd9..9e3382bb7 100644 --- a/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h +++ b/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h @@ -230,6 +230,8 @@ class HandMk5CouplingHandler : public BaseCouplingHandler HandMk5CouplingHandler (gazebo::physics::Model* model, yarp::sig::VectorOf coupled_joints, std::vector coupled_joint_names, std::vector coupled_joint_limits); public: + + bool parseFingerParameters(yarp::os::Bottle& hand_params); bool decouplePos (yarp::sig::Vector& current_pos); bool decoupleVel (yarp::sig::Vector& current_vel); bool decoupleAcc (yarp::sig::Vector& current_acc); @@ -247,8 +249,6 @@ class HandMk5CouplingHandler : public BaseCouplingHandler { double L0x; double L0y; - double L1x; - double L1y; double q2bias; double q1off; double k; @@ -257,41 +257,7 @@ class HandMk5CouplingHandler : public BaseCouplingHandler 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 - }; + std::map mFingerParameters; /* * This method implements the law q2 = q2(q1) from @@ -300,29 +266,7 @@ class HandMk5CouplingHandler : public BaseCouplingHandler * * 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); + double evaluateCoupledJoint(const double& q1, const std::string& finger_name); /* * This method implements the law \frac{\partial{q2}}{\partial{q1}} from @@ -332,33 +276,8 @@ class HandMk5CouplingHandler : public BaseCouplingHandler * * 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); + double evaluateCoupledJointJacobian(const double& q1, const std::string& finger_name); - /* - * 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/ControlBoard.cc b/plugins/controlboard/src/ControlBoard.cc index 97e0eb51b..4819d9248 100644 --- a/plugins/controlboard/src/ControlBoard.cc +++ b/plugins/controlboard/src/ControlBoard.cc @@ -43,12 +43,12 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) m_wrapper.close(); } - if (m_iVirtAnalogSensorWrap) + if (m_iVirtAnalogSensorWrap) { m_iVirtAnalogSensorWrap->detachAll(); m_iVirtAnalogSensorWrap = nullptr; } - + if (m_virtAnalogSensorWrapper.isValid()) { m_virtAnalogSensorWrapper.close(); @@ -110,15 +110,15 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) } #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS bool disable_wrapper = m_parameters.check("disableImplicitNetworkWrapper"); - + if (disable_wrapper && !m_parameters.check("yarpDeviceName")) { yError() << "GazeboYarpControlBoard : missing yarpDeviceName parameter for one device in robot " << m_robotName; return; } - - - if (!disable_wrapper) + + + if (!disable_wrapper) { yarp::os::Bottle wrapper_group = m_parameters.findGroup("WRAPPER"); if(wrapper_group.isNull()) @@ -166,8 +166,8 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) virt_group.append(yarp::os::Bottle(networks)); } - - + + yarp::os::Bottle *netList = wrapper_group.find("networks").asList(); if (netList->isNull()) { @@ -175,9 +175,9 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) m_wrapper.close(); return; } - - + + for (int n = 0; n < netList->size(); n++) { yarp::dev::PolyDriverDescriptor newPoly; @@ -201,9 +201,9 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) scopedDeviceName = m_robotName + "::" + m_parameters.find("yarpDeviceName").asString(); m_scopedDeviceName = scopedDeviceName; } - + newPoly.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(scopedDeviceName); - + if( newPoly.poly != NULL) { // device already exists, use it, setting it againg to increment the usage counter. @@ -276,7 +276,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) return; } } - + if (!m_iWrap || !m_iWrap->attachAll(m_controlBoards)) { yCError(GAZEBOCONTROLBOARD) << "error while attaching wrapper to device."; diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp index 3ef749378..5c0fba41e 100644 --- a/plugins/controlboard/src/ControlBoardDriver.cpp +++ b/plugins/controlboard/src/ControlBoardDriver.cpp @@ -339,7 +339,20 @@ bool GazeboYarpControlBoardDriver::gazebo_init() } else if (coupling_bottle->get(0).asString()=="icub_hand_mk5") { + yarp::os::Bottle& coupling_group_bottle = m_pluginParameters.findGroup("COUPLING_PARAMS"); + + if(coupling_group_bottle.isNull()) + { + yCError(GAZEBOCONTROLBOARD) << "Missing param in COUPLING_PARAMS section"; + return false; + } + BaseCouplingHandler* cpl = new HandMk5CouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); + if (! static_cast(cpl)->parseFingerParameters(coupling_group_bottle)) + { + yCError(GAZEBOCONTROLBOARD) << "Error parsing coupling parameter, wrong size of the finger parameters list"; + return false; + } m_coupling_handler.push_back(cpl); yCInfo(GAZEBOCONTROLBOARD) << "using icub_hand_mk5"; } diff --git a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp index 72c929b42..1cd534fb0 100644 --- a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp +++ b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp @@ -1139,6 +1139,39 @@ HandMk5CouplingHandler::HandMk5CouplingHandler(gazebo::physics::Model* model, ya m_couplingSize = 12; } +bool HandMk5CouplingHandler::parseFingerParameters(yarp::os::Bottle& hand_params) +{ + auto L0x = hand_params.findGroup("L0x"); + auto L0y = hand_params.findGroup("L0y"); + auto q2bias = hand_params.findGroup("q2bias"); + auto q1off = hand_params.findGroup("q1off"); + auto k = hand_params.findGroup("k"); + auto d = hand_params.findGroup("d"); + auto l = hand_params.findGroup("l"); + auto b = hand_params.findGroup("b"); + + constexpr int nFingers = 5; + // All the +1 is because the first element of the bottle is the name of the group + if(L0x.size()!=nFingers+1 || L0y.size()!=nFingers+1 || q2bias.size()!=nFingers+1 || + q1off.size()!=nFingers+1 || k.size()!=nFingers+1 || d.size()!=nFingers+1 || + l.size()!=nFingers+1 || b.size()!=nFingers+1 ) + { + yError()<<"HandMk5CouplingHandler: invalid hand parameters, check your configuration file"; + return false; + } + + + const std::array names = {"thumb", "index", "middle", "ring", "pinky"}; + for (std::size_t i = 0; i < names.size(); i++) + { + mFingerParameters.insert({names.at(i), {L0x.get(i+1).asFloat32(), L0y.get(i+1).asFloat32(), q2bias.get(i+1).asFloat32(), + q1off.get(i+1).asFloat32(), k.get(i+1).asFloat32(), d.get(i+1).asFloat32(), + l.get(i+1).asFloat32(), b.get(i+1).asFloat32()}}); + } + + return true; +} + bool HandMk5CouplingHandler::decouplePos (yarp::sig::Vector& current_pos) { if (m_coupledJoints.size()!=m_couplingSize) return false; @@ -1219,25 +1252,25 @@ yarp::sig::Vector HandMk5CouplingHandler::decoupleRefPos (yarp::sig::Vector& pos /* thumb_prox <-- thumb_oc */ out[1] = pos_ref[m_coupledJoints[1]]; /* thumb_dist <-- coupling_law(thumb_prox) */ - out[2] = evaluateCoupledJointThumb(out[1]); + out[2] = evaluateCoupledJoint(out[1], "thumb"); /* 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]); + out[5] = evaluateCoupledJoint(out[4], "index"); /* middle_prox <-- middle_oc */ out[6] = pos_ref[m_coupledJoints[4]]; /* middle_dist <-- coupling_law(middle_prox) */ - out[7] = evaluateCoupledJointIndex(out[6]); + out[7] = evaluateCoupledJoint(out[6], "middle"); /* ring_prox <-- ring_pinky_oc */ out[8] = pos_ref[m_coupledJoints[5]]; /* ring_dist <-- coupling_law(ring_prox) */ - out[9] = evaluateCoupledJointIndex(out[8]); + out[9] = evaluateCoupledJoint(out[8], "ring"); /* pinky_prox <-- ring_pinky_oc */ out[10] = pos_ref[m_coupledJoints[5]]; /* pinky_dist <-- coupling_law(pinky_prox) */ - out[11] = evaluateCoupledJointPinky(out[10]); + out[11] = evaluateCoupledJoint(out[10], "pinky"); return out; } @@ -1268,25 +1301,25 @@ yarp::sig::Vector HandMk5CouplingHandler::decoupleRefVel (yarp::sig::Vector& vel /* 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]; + out[2] = evaluateCoupledJointJacobian(lastThumbProx, "thumb") * 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]; + out[5] = evaluateCoupledJointJacobian(lastIndexProx, "index") * 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]; + out[7] = evaluateCoupledJointJacobian(lastMiddleProx, "middle") * 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]; + out[9] = evaluateCoupledJointJacobian(lastRingProx, "ring") * 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]; + out[11] = evaluateCoupledJointJacobian(lastPinkyProx, "pinky") * out[10]; return out; } @@ -1299,12 +1332,12 @@ yarp::sig::Vector HandMk5CouplingHandler::decoupleRefTrq (yarp::sig::Vector& trq return trq_ref; } -double HandMk5CouplingHandler::evaluateCoupledJoint(const double& q1, const FingerParameters& params) +double HandMk5CouplingHandler::evaluateCoupledJoint(const double& q1, const std::string& finger_name) { /** * Coupling law taken from from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling */ - + auto params = mFingerParameters.at(finger_name); 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; @@ -1319,44 +1352,17 @@ double HandMk5CouplingHandler::evaluateCoupledJoint(const double& q1, const Fing 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; + -q2bias_rad - M_PI; return q2 * 180.0 / M_PI; } -double HandMk5CouplingHandler::evaluateCoupledJointThumb(const double& q1) -{ - /** - * 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; -} - -double HandMk5CouplingHandler::evaluateCoupledJointIndex(const double& q1) -{ - /** - * 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; -} - -double HandMk5CouplingHandler::evaluateCoupledJointPinky(const double& q1) -{ - /** - * 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; -} - -double HandMk5CouplingHandler::evaluateCoupledJointJacobian(const double& q1, const FingerParameters& params) +double HandMk5CouplingHandler::evaluateCoupledJointJacobian(const double& q1, const std::string& finger_name) { /** * Coupling law jacobian taken from from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling */ - + auto params = mFingerParameters.at(finger_name); double q1_rad = q1 * M_PI / 180.0; double q1off_rad = params.q1off * M_PI / 180.0; @@ -1376,30 +1382,3 @@ double HandMk5CouplingHandler::evaluateCoupledJointJacobian(const double& q1, co return dq2_dq1; } - -double HandMk5CouplingHandler::evaluateCoupledJointJacobianThumb(const double& q1) -{ - /** - * 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; -} - -double HandMk5CouplingHandler::evaluateCoupledJointJacobianIndex(const double& q1) -{ - /** - * 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; -} - -double HandMk5CouplingHandler::evaluateCoupledJointJacobianPinky(const double& q1) -{ - /** - * 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 9de8b0e19733c44304d3508714f07757f987411b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nicol=C3=B2=20Genesio=20=28=E4=BA=8C=E3=82=B3=E3=82=B2?= =?UTF-8?q?=E3=83=8D=29?= Date: Wed, 13 Sep 2023 10:32:58 +0200 Subject: [PATCH 2/4] Update CHANGELOG.md --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5c3566bbd..899d6a296 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,9 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo ## [Unreleased] +### Changed +- The `icub_hand_mk5` coupling handler has been refactored, it now requires the group `COUPLING_PARAMS` that allows to differentiate between the coupling mk5.0 and mk5.1(https://github.com/robotology/gazebo-yarp-plugins/pull/662). + ## [4.8.0] - 2023-07-31 ### Added From 5ec5be3454e8cab345823fcef80f8c255b8568bf Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Sun, 17 Sep 2023 19:05:01 +0200 Subject: [PATCH 3/4] ControlBoardDriverCoupling.cpp: include array header as array is used in the code (#665) --- plugins/controlboard/src/ControlBoardDriverCoupling.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp index 1cd534fb0..4d49830d2 100644 --- a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp +++ b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp @@ -9,6 +9,7 @@ #include #include +#include #include #include #include From ba42e4d4fbaedc81d0b9fa52838ae16c7e5ee3d4 Mon Sep 17 00:00:00 2001 From: Nicola Piga Date: Mon, 18 Sep 2023 08:33:55 +0000 Subject: [PATCH 4/4] Fix HandMk5CouplingHandler::{evaluateCoupledJointJacobian, :evaluateCoupledJointJacobian} to take into account that coupling formulae provides absolute angles w.r.t. the palm --- plugins/controlboard/src/ControlBoardDriverCoupling.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp index 4d49830d2..0ae704914 100644 --- a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp +++ b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp @@ -1355,7 +1355,9 @@ double HandMk5CouplingHandler::evaluateCoupledJoint(const double& q1, const std: acos((h_sq + l_sq - k_sq) / (2.0 * params.l * h)) + \ -q2bias_rad - M_PI; - return q2 * 180.0 / M_PI; + // The value of q1 is subtracted from the result as the formula provides + // the absolute angle of the coupled distal joint with respect to the palm. + return q2 * 180.0 / M_PI - q1; } double HandMk5CouplingHandler::evaluateCoupledJointJacobian(const double& q1, const std::string& finger_name) @@ -1381,5 +1383,7 @@ double HandMk5CouplingHandler::evaluateCoupledJointJacobian(const double& q1, co 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; + // The value of 1 is subtracted from the result as evaluateCoupledJointJacobian provides + // the jacobian of the absolute angle of the coupled distal joint. + return dq2_dq1 - 1; }