From 918929e7d523ab1d68e79e77cb4bb7c04b3d1f15 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 21 Sep 2023 16:00:31 +0200 Subject: [PATCH 01/15] HandMk5 refactor as device It is a draft, just compiles --- CMakeLists.txt | 1 + src/modules/CMakeLists.txt | 5 +- .../handMk5CouplingHandler/CMakeLists.txt | 43 +++ .../HandMk5CouplingHandler.cpp | 281 ++++++++++++++++++ .../HandMk5CouplingHandler.h | 106 +++++++ 5 files changed, 435 insertions(+), 1 deletion(-) create mode 100644 src/modules/handMk5CouplingHandler/CMakeLists.txt create mode 100644 src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp create mode 100644 src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 9049ac3f..69ad745c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,3 +49,4 @@ if(COMPILE_ergoCubEmotions) endif() + diff --git a/src/modules/CMakeLists.txt b/src/modules/CMakeLists.txt index 79ebdfb1..80370de7 100644 --- a/src/modules/CMakeLists.txt +++ b/src/modules/CMakeLists.txt @@ -4,4 +4,7 @@ # This software may be modified and distributed under the terms of the # BSD-3-Clause license. See the accompanying LICENSE file for details. -add_subdirectory(ergoCubEmotions) +if(COMPILE_ergoCubEmotions) + add_subdirectory(ergoCubEmotions) +endif() +add_subdirectory(handMk5CouplingHandler) diff --git a/src/modules/handMk5CouplingHandler/CMakeLists.txt b/src/modules/handMk5CouplingHandler/CMakeLists.txt new file mode 100644 index 00000000..cb689502 --- /dev/null +++ b/src/modules/handMk5CouplingHandler/CMakeLists.txt @@ -0,0 +1,43 @@ +# Copyright (C) 2006-2023 Istituto Italiano di Tecnologia (IIT) +# All rights reserved. +# +# This software may be modified and distributed under the terms of the +# BSD-3-Clause license. See the accompanying LICENSE file for details. + +find_package(YARP 3.7.2 REQUIRED) + +yarp_configure_plugins_installation(ergocub-software) + +yarp_prepare_plugin(handMk5CouplingHandler + CATEGORY device + TYPE HandMk5CouplingHandler + INCLUDE HandMk5CouplingHandler.h + DEFAULT OFF) + +if(ENABLE_handMk5CouplingHandler) + + yarp_add_plugin(yarp_handMk5CouplingHandler) + + if(MSVC) + add_definitions(-D_USE_MATH_DEFINES) + endif() + + target_sources(yarp_handMk5CouplingHandler PRIVATE HandMk5CouplingHandler.cpp + HandMk5CouplingHandler.h) + + target_link_libraries(yarp_handMk5CouplingHandler PRIVATE YARP::YARP_os + YARP::YARP_dev) + list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS YARP_os + YARP_dev) + + yarp_install(TARGETS yarp_handMk5CouplingHandler + EXPORT YARP_${YARP_PLUGIN_MASTER} + COMPONENT ${YARP_PLUGIN_MASTER} + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) + + set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE) + + set_property(TARGET yarp_handMk5CouplingHandler PROPERTY FOLDER "Plugins/Device") +endif() \ No newline at end of file diff --git a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp b/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp new file mode 100644 index 00000000..31c8f7db --- /dev/null +++ b/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp @@ -0,0 +1,281 @@ +/* + * Copyright (C) 2006-2023 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#include "HandMk5CouplingHandler.h" +#include +#include + + +YARP_DECLARE_LOG_COMPONENT(HANDMK5COUPLINGHANDLER) + + +HandMk5CouplingHandler::HandMk5CouplingHandler() +{ + 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::open(yarp::os::Searchable& config) { + yarp::os::Bottle& coupling_group_bottle = config.findGroup("COUPLING_PARAMS"); + if(coupling_group_bottle.isNull()) + { + yError()<<"HandMk5CouplingHandler: missing coupling parameters, check your configuration file"; + return false; + } + + return parseFingerParameters(coupling_group_bottle); +} + +bool HandMk5CouplingHandler::decouplePos (yarp::sig::Vector& current_pos) +{ + if (m_coupledJoints.size()!=m_couplingSize) return false; + + 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]; + + return true; +} + +bool HandMk5CouplingHandler::decoupleVel (yarp::sig::Vector& current_vel) +{ + if (m_coupledJoints.size()!=m_couplingSize) return false; + + 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]; + + return true; +} + +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; +} + +bool HandMk5CouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq) +{ + /** + * Torque control not available for fingers on the real robot. + */ + return false; +} + +yarp::sig::Vector HandMk5CouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref) +{ + if (m_coupledJoints.size()!=m_couplingSize) {yCError( HANDMK5COUPLINGHANDLER) << "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] = 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] = evaluateCoupledJoint(out[4], "index"); + /* middle_prox <-- middle_oc */ + out[6] = pos_ref[m_coupledJoints[4]]; + /* middle_dist <-- coupling_law(middle_prox) */ + 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] = evaluateCoupledJoint(out[8], "ring"); + /* pinky_prox <-- ring_pinky_oc */ + out[10] = pos_ref[m_coupledJoints[5]]; + /* pinky_dist <-- coupling_law(pinky_prox) */ + out[11] = evaluateCoupledJoint(out[10], "pinky"); + + return out; +} + + +yarp::sig::Vector HandMk5CouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) +{ + if (m_coupledJoints.size()!=m_couplingSize) {yCError( HANDMK5COUPLINGHANDLER) << "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] = 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] = 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] = 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] = 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] = evaluateCoupledJointJacobian(lastPinkyProx, "pinky") * out[10]; + + return out; +} + +yarp::sig::Vector HandMk5CouplingHandler::decoupleRefTrq (yarp::sig::Vector& trq_ref) +{ + /** + * Torque control not available for fingers on the real robot. + */ + return trq_ref; +} + +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; + + 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 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; + + // 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) +{ + /** + * 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; + + 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; + + // 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; +} \ No newline at end of file diff --git a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h b/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h new file mode 100644 index 00000000..1575c71d --- /dev/null +++ b/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h @@ -0,0 +1,106 @@ +/* + * Copyright (C) 2006-2023 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#ifndef HANDMK5COUPLINGHANDLER_H +#define HANDMK5COUPLINGHANDLER_H + +#include +#include + +#include +#include +#include + +/** TBD + */ +class HandMk5CouplingHandler : public yarp::dev::DeviceDriver, + public yarp::dev::IJointCoupling { +private: + + /** + * Parameters from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling + */ + struct FingerParameters + { + double L0x; + double L0y; + double q2bias; + double q1off; + double k; + double d; + double l; + double b; + }; + + std::unordered_map mFingerParameters; + + struct Range { + Range() : min(0), max(0){} + double min; + double max; + }; + // These could go in a Basic class + yarp::sig::VectorOf m_coupledJoints; + std::vector m_coupledJointNames; + std::unordered_map m_coupledJointLimits; + unsigned int m_controllerPeriod; + unsigned int m_couplingSize; + + /* + * 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 std::string& finger_name); + + /* + * 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 std::string& finger_name); + + bool parseFingerParameters(yarp::os::Bottle& hand_params); +public: + HandMk5CouplingHandler(); + ~HandMk5CouplingHandler() override; + + + virtual bool decouplePos(yarp::sig::Vector& current_pos); + virtual bool decoupleVel(yarp::sig::Vector& current_vel); + virtual bool decoupleAcc(yarp::sig::Vector& current_acc); + virtual bool decoupleTrq(yarp::sig::Vector& current_trq) override; + virtual yarp::sig::Vector decoupleRefPos(yarp::sig::Vector& pos_ref) override; + virtual yarp::sig::Vector decoupleRefVel(yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) override; + virtual yarp::sig::Vector decoupleRefTrq(yarp::sig::Vector& trq_ref) override; + + // These could go in a Basic class + virtual yarp::sig::VectorOf getCoupledJoints() override { return m_coupledJoints; }; + virtual std::string getCoupledJointName(int joint) override { return m_coupledJointNames[joint]; }; + virtual bool checkJointIsCoupled(int joint) override { return m_coupledJoints[joint] != -1; }; + + + virtual void setCoupledJointLimit(int joint, const double& min, const double& max) override { m_coupledJointLimits[joint].min = min; m_coupledJointLimits[joint].max = max; }; + virtual void getCoupledJointLimit(int joint, double& min, double& max) override { min = m_coupledJointLimits[joint].min; max = m_coupledJointLimits[joint].max; }; + + //DeviceDriver + bool close() override; + /** + * Configure with a set of options. + * @param config The options to use + * @return true iff the object could be configured. + */ + bool open(yarp::os::Searchable& config) override; +}; + +#endif // HANDMK5COUPLINGHANDLER_H \ No newline at end of file From 58542dcdba8a25f2916bc5eafa94ae30935c4603 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Fri, 22 Sep 2023 16:05:42 +0200 Subject: [PATCH 02/15] Second draft of refactor --- .../handMk5CouplingHandler/CMakeLists.txt | 2 +- .../HandMk5CouplingHandler.cpp | 224 +----------------- .../HandMk5CouplingHandler.h | 65 ++--- 3 files changed, 33 insertions(+), 258 deletions(-) diff --git a/src/modules/handMk5CouplingHandler/CMakeLists.txt b/src/modules/handMk5CouplingHandler/CMakeLists.txt index cb689502..196dfeef 100644 --- a/src/modules/handMk5CouplingHandler/CMakeLists.txt +++ b/src/modules/handMk5CouplingHandler/CMakeLists.txt @@ -4,7 +4,7 @@ # This software may be modified and distributed under the terms of the # BSD-3-Clause license. See the accompanying LICENSE file for details. -find_package(YARP 3.7.2 REQUIRED) +find_package(YARP 3.9.0 REQUIRED) yarp_configure_plugins_installation(ergocub-software) diff --git a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp b/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp index 31c8f7db..0f4508b5 100644 --- a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp +++ b/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp @@ -56,6 +56,8 @@ bool HandMk5CouplingHandler::parseFingerParameters(yarp::os::Bottle& hand_params bool HandMk5CouplingHandler::open(yarp::os::Searchable& config) { yarp::os::Bottle& coupling_group_bottle = config.findGroup("COUPLING_PARAMS"); + + // TODO INVOKE ImplementCoupling::initialise() if(coupling_group_bottle.isNull()) { yError()<<"HandMk5CouplingHandler: missing coupling parameters, check your configuration file"; @@ -65,217 +67,11 @@ bool HandMk5CouplingHandler::open(yarp::os::Searchable& config) { return parseFingerParameters(coupling_group_bottle); } -bool HandMk5CouplingHandler::decouplePos (yarp::sig::Vector& current_pos) -{ - if (m_coupledJoints.size()!=m_couplingSize) return false; - - 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]; - - return true; -} - -bool HandMk5CouplingHandler::decoupleVel (yarp::sig::Vector& current_vel) -{ - if (m_coupledJoints.size()!=m_couplingSize) return false; - - 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]; - - return true; -} - -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; -} - -bool HandMk5CouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq) -{ - /** - * Torque control not available for fingers on the real robot. - */ - return false; -} - -yarp::sig::Vector HandMk5CouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref) -{ - if (m_coupledJoints.size()!=m_couplingSize) {yCError( HANDMK5COUPLINGHANDLER) << "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] = 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] = evaluateCoupledJoint(out[4], "index"); - /* middle_prox <-- middle_oc */ - out[6] = pos_ref[m_coupledJoints[4]]; - /* middle_dist <-- coupling_law(middle_prox) */ - 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] = evaluateCoupledJoint(out[8], "ring"); - /* pinky_prox <-- ring_pinky_oc */ - out[10] = pos_ref[m_coupledJoints[5]]; - /* pinky_dist <-- coupling_law(pinky_prox) */ - out[11] = evaluateCoupledJoint(out[10], "pinky"); - - return out; -} - - -yarp::sig::Vector HandMk5CouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) -{ - if (m_coupledJoints.size()!=m_couplingSize) {yCError( HANDMK5COUPLINGHANDLER) << "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] = 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] = 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] = 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] = 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] = evaluateCoupledJointJacobian(lastPinkyProx, "pinky") * out[10]; - - return out; -} - -yarp::sig::Vector HandMk5CouplingHandler::decoupleRefTrq (yarp::sig::Vector& trq_ref) -{ - /** - * Torque control not available for fingers on the real robot. - */ - return trq_ref; -} - -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; - - 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 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; - - // 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) -{ - /** - * 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; - - 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; - - // 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; -} \ No newline at end of file + bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) { return false; } + bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) { return false; } + bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) { return false; } + bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) { return false; } + bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) { return false; } + bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) { return false; } + bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) { return false; } + bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) { return false; } diff --git a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h b/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h index 1575c71d..bf27c2b5 100644 --- a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h +++ b/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h @@ -10,7 +10,7 @@ #define HANDMK5COUPLINGHANDLER_H #include -#include +#include #include #include @@ -19,7 +19,27 @@ /** TBD */ class HandMk5CouplingHandler : public yarp::dev::DeviceDriver, - public yarp::dev::IJointCoupling { + public yarp::dev::ImplementJointCoupling { +public: + HandMk5CouplingHandler(); + ~HandMk5CouplingHandler() override; + bool convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) override; + bool convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) override; + bool convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) override; + bool convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) override; + bool convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) override; + bool convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) override; + bool convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) override; + bool convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) override; + + //DeviceDriver + bool close() override; + /** + * Configure with a set of options. + * @param config The options to use + * @return true iff the object could be configured. + */ + bool open(yarp::os::Searchable& config) override; private: /** @@ -39,17 +59,6 @@ class HandMk5CouplingHandler : public yarp::dev::DeviceDriver, std::unordered_map mFingerParameters; - struct Range { - Range() : min(0), max(0){} - double min; - double max; - }; - // These could go in a Basic class - yarp::sig::VectorOf m_coupledJoints; - std::vector m_coupledJointNames; - std::unordered_map m_coupledJointLimits; - unsigned int m_controllerPeriod; - unsigned int m_couplingSize; /* * This method implements the law q2 = q2(q1) from @@ -71,36 +80,6 @@ class HandMk5CouplingHandler : public yarp::dev::DeviceDriver, double evaluateCoupledJointJacobian(const double& q1, const std::string& finger_name); bool parseFingerParameters(yarp::os::Bottle& hand_params); -public: - HandMk5CouplingHandler(); - ~HandMk5CouplingHandler() override; - - - virtual bool decouplePos(yarp::sig::Vector& current_pos); - virtual bool decoupleVel(yarp::sig::Vector& current_vel); - virtual bool decoupleAcc(yarp::sig::Vector& current_acc); - virtual bool decoupleTrq(yarp::sig::Vector& current_trq) override; - virtual yarp::sig::Vector decoupleRefPos(yarp::sig::Vector& pos_ref) override; - virtual yarp::sig::Vector decoupleRefVel(yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) override; - virtual yarp::sig::Vector decoupleRefTrq(yarp::sig::Vector& trq_ref) override; - - // These could go in a Basic class - virtual yarp::sig::VectorOf getCoupledJoints() override { return m_coupledJoints; }; - virtual std::string getCoupledJointName(int joint) override { return m_coupledJointNames[joint]; }; - virtual bool checkJointIsCoupled(int joint) override { return m_coupledJoints[joint] != -1; }; - - - virtual void setCoupledJointLimit(int joint, const double& min, const double& max) override { m_coupledJointLimits[joint].min = min; m_coupledJointLimits[joint].max = max; }; - virtual void getCoupledJointLimit(int joint, double& min, double& max) override { min = m_coupledJointLimits[joint].min; max = m_coupledJointLimits[joint].max; }; - - //DeviceDriver - bool close() override; - /** - * Configure with a set of options. - * @param config The options to use - * @return true iff the object could be configured. - */ - bool open(yarp::os::Searchable& config) override; }; #endif // HANDMK5COUPLINGHANDLER_H \ No newline at end of file From 3f8d9bfbbd5253d563322d3abffe4d38d613fad8 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 13 Dec 2023 11:04:53 +0100 Subject: [PATCH 03/15] HandMK5CouplingHandler: make it compile with the new parsing --- .../HandMk5CouplingHandler.cpp | 121 +++++++++++++++--- .../HandMk5CouplingHandler.h | 3 +- 2 files changed, 108 insertions(+), 16 deletions(-) diff --git a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp b/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp index 0f4508b5..982c7db7 100644 --- a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp +++ b/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp @@ -8,6 +8,7 @@ #include "HandMk5CouplingHandler.h" #include +#include #include @@ -21,8 +22,15 @@ HandMk5CouplingHandler::HandMk5CouplingHandler() -bool HandMk5CouplingHandler::parseFingerParameters(yarp::os::Bottle& hand_params) +bool HandMk5CouplingHandler::parseFingerParameters(yarp::os::Searchable& config) { + + yarp::os::Bottle& hand_params = config.findGroup("COUPLING_PARAMS"); + if(hand_params.isNull()) + { + yCError(HANDMK5COUPLINGHANDLER)<<"Missing COUPLING_PARAMS section in the configuration file"; + return false; + } auto L0x = hand_params.findGroup("L0x"); auto L0y = hand_params.findGroup("L0y"); auto q2bias = hand_params.findGroup("q2bias"); @@ -38,7 +46,7 @@ bool HandMk5CouplingHandler::parseFingerParameters(yarp::os::Bottle& hand_params 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"; + yCError(HANDMK5COUPLINGHANDLER)<<"Invalid hand parameters, check your configuration file"; return false; } @@ -54,24 +62,107 @@ bool HandMk5CouplingHandler::parseFingerParameters(yarp::os::Bottle& hand_params return true; } +bool HandMk5CouplingHandler::parseCouplingParameters(yarp::os::Searchable& config) { + yarp::sig::VectorOf coupled_physical_joints{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}; + yarp::sig::VectorOf coupled_actuated_axes{0, 1, 2, 3, 4, 5}; + std::vector physical_joint_names; + std::vector actuated_axes_names; + std::vector> physical_joint_limits; + + yarp::os::Bottle physical_joint_names_bottle = config.findGroup("jointNames"); + + if (physical_joint_names_bottle.isNull()) { + yCError(HANDMK5COUPLINGHANDLER) << "Error cannot find jointNames." ; + return false; + } + + for (size_t i = 1; i < physical_joint_names.size(); i++) { + physical_joint_names.push_back(physical_joint_names_bottle.get(i).asString().c_str()); + } + + yarp::os::Bottle& coupling_params = config.findGroup("COUPLING"); + if (coupling_params.size() ==0) + { + yCError(HANDMK5COUPLINGHANDLER) << "Missing param in COUPLING section"; + return false; + } + yCDebug(HANDMK5COUPLINGHANDLER) << "Requested couplings:" << coupling_params.toString(); + yCDebug(HANDMK5COUPLINGHANDLER) << "Size: " << coupling_params.size(); + + yarp::os::Bottle actuated_axes_names_bottle = coupling_params.findGroup("actuatedAxesNames"); + + if (actuated_axes_names_bottle.isNull()) { + yCError(HANDMK5COUPLINGHANDLER) << "Error cannot find actuatedAxesNames." ; + return false; + } + + for (size_t i = 1; i < actuated_axes_names_bottle.size(); i++) { + actuated_axes_names.push_back(actuated_axes_names_bottle.get(i).asString().c_str()); + } + physical_joint_limits.resize(physical_joint_names.size()); + yarp::os::Bottle& limits_bottle = config.findGroup("LIMITS"); + if (!limits_bottle.isNull()) + { + yarp::os::Bottle& pos_limit_min = limits_bottle.findGroup("jntPosMin"); + if (!pos_limit_min.isNull() && static_cast(pos_limit_min.size()) == physical_joint_limits.size()+1) + { + for(size_t i = 0; i < physical_joint_limits.size(); ++i) + { + physical_joint_limits[i].first = pos_limit_min.get(i+1).asFloat64(); + } + } + else + { + yCError(HANDMK5COUPLINGHANDLER) << "Failed to parse jntPosMin parameter"; + return false; + } + yarp::os::Bottle& pos_limit_max = limits_bottle.findGroup("jntPosMax"); + if (!pos_limit_max.isNull() && static_cast(pos_limit_max.size()) == physical_joint_limits.size()+1) + { + for(size_t i = 0; i < physical_joint_limits.size(); ++i) + { + physical_joint_limits[i].second = pos_limit_max.get(i+1).asFloat64(); + + } + } + else + { + yCError(HANDMK5COUPLINGHANDLER) << "Failed to parse jntPosMax parameter"; + return false; + } + } + else + { + yCError(HANDMK5COUPLINGHANDLER) << "Failed to parse LIMITS parameter"; + return false; + } + initialise(coupled_physical_joints, coupled_actuated_axes, physical_joint_names, actuated_axes_names, physical_joint_limits); + return true; +} + bool HandMk5CouplingHandler::open(yarp::os::Searchable& config) { - yarp::os::Bottle& coupling_group_bottle = config.findGroup("COUPLING_PARAMS"); // TODO INVOKE ImplementCoupling::initialise() - if(coupling_group_bottle.isNull()) - { - yError()<<"HandMk5CouplingHandler: missing coupling parameters, check your configuration file"; + bool ok = parseFingerParameters(config); + if (!ok) { + yCError(HANDMK5COUPLINGHANDLER) << "Error parsing finger parameters"; + return false; + } + + ok &= parseCouplingParameters(config); + if (!ok) { + yCError(HANDMK5COUPLINGHANDLER) << "Error parsing coupling parameters"; return false; } - return parseFingerParameters(coupling_group_bottle); + return ok; } - bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) { return false; } - bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) { return false; } - bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) { return false; } - bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) { return false; } - bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) { return false; } - bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) { return false; } - bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) { return false; } - bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) { return false; } +bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) { return false; } +bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) { return false; } +bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) { return false; } +bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) { return false; } +bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) { return false; } +bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) { return false; } +bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) { return false; } +bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) { return false; } diff --git a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h b/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h index bf27c2b5..ecdb20e8 100644 --- a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h +++ b/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h @@ -79,7 +79,8 @@ class HandMk5CouplingHandler : public yarp::dev::DeviceDriver, */ double evaluateCoupledJointJacobian(const double& q1, const std::string& finger_name); - bool parseFingerParameters(yarp::os::Bottle& hand_params); + bool parseFingerParameters(yarp::os::Searchable& config); + bool parseCouplingParameters(yarp::os::Searchable& config); }; #endif // HANDMK5COUPLINGHANDLER_H \ No newline at end of file From e3f18c36accb1af1d362814049c9b6f91bd6f1cf Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 13 Dec 2023 11:49:23 +0100 Subject: [PATCH 04/15] HandMk5CouplingHandler: implement coupling/decoupling --- .../HandMk5CouplingHandler.cpp | 228 +++++++++++++++++- 1 file changed, 220 insertions(+), 8 deletions(-) diff --git a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp b/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp index 982c7db7..0a4f8e72 100644 --- a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp +++ b/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp @@ -15,6 +15,63 @@ YARP_DECLARE_LOG_COMPONENT(HANDMK5COUPLINGHANDLER) +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; + + 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 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; + + // 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) +{ + /** + * 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; + + 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; + + // 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; +} + + HandMk5CouplingHandler::HandMk5CouplingHandler() { m_couplingSize = 12; @@ -158,11 +215,166 @@ bool HandMk5CouplingHandler::open(yarp::os::Searchable& config) { return ok; } -bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) { return false; } -bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) { return false; } -bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) { return false; } -bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) { return false; } -bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) { return false; } -bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) { return false; } -bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) { return false; } -bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) { return false; } +bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) { + size_t nrOfPhysicalJoints; + size_t nrOfActuatedAxes; + auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); + ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); + if (!ok || physJointsPos.size() != nrOfPhysicalJoints || actAxesPos.size() != nrOfActuatedAxes) { + yCError(HANDMK5COUPLINGHANDLER) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size"; + return false; + } + + /* thumb_add <-- thumb_add */ + actAxesPos[0] = physJointsPos[0]; + /* thumb_oc <-- thumb_prox */ + actAxesPos[1] = physJointsPos[1]; + /* index_add <-- index_add */ + actAxesPos[2] = physJointsPos[3]; + /* index_oc <-- index_prox */ + actAxesPos[3] = physJointsPos[4]; + /* middle_oc <-- middle_prox */ + actAxesPos[4] = physJointsPos[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 + */ + actAxesPos[5] = physJointsPos[10]; + return true; +} + +bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) { + size_t nrOfPhysicalJoints; + size_t nrOfActuatedAxes; + auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); + ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); + if (!ok || physJointsVel.size() != nrOfPhysicalJoints || actAxesVel.size() != nrOfActuatedAxes) { + yCError(HANDMK5COUPLINGHANDLER) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size"; + return false; + } + /* thumb_add <-- thumb_add */ + actAxesVel[0] = physJointsVel[0]; + /* thumb_oc <-- thumb_prox */ + actAxesVel[1] = physJointsVel[1]; + /* index_add <-- index_add */ + actAxesVel[2] = physJointsVel[3]; + /* index_oc <-- index_prox */ + actAxesVel[3] = physJointsVel[4]; + /* middle_oc <-- middle_prox */ + actAxesVel[4] = physJointsVel[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 + */ + actAxesVel[5] = physJointsVel[10]; + return true; +} + + +bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) { + return false; +} + +bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) { + return false; +} + + +bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) { + size_t nrOfPhysicalJoints; + size_t nrOfActuatedAxes; + auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); + ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); + if (!ok || physJointsPos.size() != nrOfPhysicalJoints || actAxesPos.size() != nrOfActuatedAxes) { + yCError(HANDMK5COUPLINGHANDLER) << "convertFromActuatedAxesToPhysicalJointsPos: input or output vectors have wrong size"; + return false; + } + physJointsPos[0] = actAxesPos[0]; + /* thumb_prox <-- thumb_oc */ + physJointsPos[1] = actAxesPos[1]; + /* thumb_dist <-- coupling_law(thumb_prox) */ + physJointsPos[2] = evaluateCoupledJoint(physJointsPos[1], "thumb"); + /* index_add <-- index_add */ + physJointsPos[3] = actAxesPos[2]; + /* index_prox <-- index_oc */ + physJointsPos[4] = actAxesPos[3]; + /* index_dist <-- coupling_law(index_prox) */ + physJointsPos[5] = evaluateCoupledJoint(physJointsPos[4], "index"); + /* middle_prox <-- middle_oc */ + physJointsPos[6] = actAxesPos[4]; + /* middle_dist <-- coupling_law(middle_prox) */ + physJointsPos[7] = evaluateCoupledJoint(physJointsPos[6], "middle"); + /* ring_prox <-- ring_pinky_oc */ + physJointsPos[8] = actAxesPos[5]; + /* ring_dist <-- coupling_law(ring_prox) */ + physJointsPos[9] = evaluateCoupledJoint(physJointsPos[8], "ring"); + /* pinky_prox <-- ring_pinky_oc */ + physJointsPos[10] = actAxesPos[5]; + /* pinky_dist <-- coupling_law(pinky_prox) */ + physJointsPos[11] = evaluateCoupledJoint(physJointsPos[10], "pinky"); + + return true; +} + + +bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) { + size_t nrOfPhysicalJoints; + size_t nrOfActuatedAxes; + auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); + ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); + if (!ok || actAxesPos.size() != nrOfActuatedAxes || physJointsVel.size() != nrOfPhysicalJoints || actAxesVel.size() != nrOfActuatedAxes) { + yCError(HANDMK5COUPLINGHANDLER) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size"; + return false; + } + + /** + * Extract the current position of proximal joints from pos_feedback. + */ + double lastThumbProx = actAxesPos[1]; + double lastIndexProx = actAxesPos[3]; + double lastMiddleProx = actAxesPos[4]; + double lastRingProx = actAxesPos[5]; + double lastPinkyProx = actAxesPos[5]; + + /** + * In the following, we use the fact that: + * /dot{distal_joint} = \partial{distal_joint}{proximal_joint} \dot{proximal_joint}. + */ + + + /* thumb_add <-- thumb_add */ + physJointsVel[0] = actAxesVel[0]; + /* thumb_prox <-- thumb_oc */ + physJointsVel[1] = actAxesVel[1]; + /* thumb_dist <-- coupling_law_jacobian(thumb_prox_position) * thumb_prox */ + physJointsVel[2] = evaluateCoupledJointJacobian(lastThumbProx, "thumb") * physJointsVel[1]; + /* index_add <-- index_add */ + physJointsVel[3] = actAxesVel[2]; + /* index_prox <-- index_oc */ + physJointsVel[4] = actAxesVel[3]; + /* index_dist <-- coupling_law_jacobian(index_prox_position) * index_prox */ + physJointsVel[5] = evaluateCoupledJointJacobian(lastIndexProx, "index") * physJointsVel[4]; + /* middle_prox <-- middle_oc */ + physJointsVel[6] = actAxesVel[4]; + /* middle_dist <-- coupling_law_jacobian(middle_prox_position) * middle_prox */ + physJointsVel[7] = evaluateCoupledJointJacobian(lastMiddleProx, "middle") * physJointsVel[6]; + /* ring_prox <-- ring_pinky_oc */ + physJointsVel[8] = actAxesVel[5]; + /* ring_dist <-- coupling_law_jacobian(ring_prox_position) * ring_prox */ + physJointsVel[9] = evaluateCoupledJointJacobian(lastRingProx, "ring") * physJointsVel[8]; + /* pinky_prox <-- ring_pinky_oc */ + physJointsVel[10] = actAxesVel[5]; + /* pinky_dist <-- coupling_law(pinky_prox) */ + physJointsVel[11] = evaluateCoupledJointJacobian(lastPinkyProx, "pinky") * physJointsVel[10]; + + return true; +} + +bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) { + return false; +} +bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) { + return false; +} From a94b6ee5f789f34dd8f1a7df72fafe4f7bf2996b Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 13 Dec 2023 11:53:59 +0100 Subject: [PATCH 05/15] Rename plugin name --- .../handMk5CouplingHandler/CMakeLists.txt | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/handMk5CouplingHandler/CMakeLists.txt b/src/modules/handMk5CouplingHandler/CMakeLists.txt index 196dfeef..d6741b47 100644 --- a/src/modules/handMk5CouplingHandler/CMakeLists.txt +++ b/src/modules/handMk5CouplingHandler/CMakeLists.txt @@ -8,29 +8,29 @@ find_package(YARP 3.9.0 REQUIRED) yarp_configure_plugins_installation(ergocub-software) -yarp_prepare_plugin(handMk5CouplingHandler +yarp_prepare_plugin(couplingHandMk5 CATEGORY device TYPE HandMk5CouplingHandler INCLUDE HandMk5CouplingHandler.h DEFAULT OFF) -if(ENABLE_handMk5CouplingHandler) +if(ENABLE_couplingHandMk5) - yarp_add_plugin(yarp_handMk5CouplingHandler) + yarp_add_plugin(yarp_couplingHandMk5) if(MSVC) add_definitions(-D_USE_MATH_DEFINES) endif() - target_sources(yarp_handMk5CouplingHandler PRIVATE HandMk5CouplingHandler.cpp + target_sources(yarp_couplingHandMk5 PRIVATE HandMk5CouplingHandler.cpp HandMk5CouplingHandler.h) - target_link_libraries(yarp_handMk5CouplingHandler PRIVATE YARP::YARP_os + target_link_libraries(yarp_couplingHandMk5 PRIVATE YARP::YARP_os YARP::YARP_dev) list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS YARP_os YARP_dev) - yarp_install(TARGETS yarp_handMk5CouplingHandler + yarp_install(TARGETS yarp_couplingHandMk5 EXPORT YARP_${YARP_PLUGIN_MASTER} COMPONENT ${YARP_PLUGIN_MASTER} LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} @@ -39,5 +39,5 @@ if(ENABLE_handMk5CouplingHandler) set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE) - set_property(TARGET yarp_handMk5CouplingHandler PROPERTY FOLDER "Plugins/Device") -endif() \ No newline at end of file + set_property(TARGET yarp_couplingHandMk5 PROPERTY FOLDER "Plugins/Device") +endif() From 588ed9e9e902dec5b73e625b568c3e59e702f31b Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 13 Dec 2023 12:14:11 +0100 Subject: [PATCH 06/15] Rename into couplingXCubHandMk5 --- src/modules/CMakeLists.txt | 2 +- .../CMakeLists.txt | 22 +++++++------- .../CouplingXCubHandMk5.cpp} | 30 +++++++++---------- .../CouplingXCubHandMk5.h} | 14 ++++----- 4 files changed, 34 insertions(+), 34 deletions(-) rename src/modules/{handMk5CouplingHandler => couplingXCubHandMk5}/CMakeLists.txt (61%) rename src/modules/{handMk5CouplingHandler/HandMk5CouplingHandler.cpp => couplingXCubHandMk5/CouplingXCubHandMk5.cpp} (87%) rename src/modules/{handMk5CouplingHandler/HandMk5CouplingHandler.h => couplingXCubHandMk5/CouplingXCubHandMk5.h} (91%) diff --git a/src/modules/CMakeLists.txt b/src/modules/CMakeLists.txt index 80370de7..845ff50e 100644 --- a/src/modules/CMakeLists.txt +++ b/src/modules/CMakeLists.txt @@ -7,4 +7,4 @@ if(COMPILE_ergoCubEmotions) add_subdirectory(ergoCubEmotions) endif() -add_subdirectory(handMk5CouplingHandler) +add_subdirectory(couplingXCubHandMk5) diff --git a/src/modules/handMk5CouplingHandler/CMakeLists.txt b/src/modules/couplingXCubHandMk5/CMakeLists.txt similarity index 61% rename from src/modules/handMk5CouplingHandler/CMakeLists.txt rename to src/modules/couplingXCubHandMk5/CMakeLists.txt index d6741b47..60f9f0f1 100644 --- a/src/modules/handMk5CouplingHandler/CMakeLists.txt +++ b/src/modules/couplingXCubHandMk5/CMakeLists.txt @@ -8,29 +8,29 @@ find_package(YARP 3.9.0 REQUIRED) yarp_configure_plugins_installation(ergocub-software) -yarp_prepare_plugin(couplingHandMk5 +yarp_prepare_plugin(couplingXCubHandMk5 CATEGORY device - TYPE HandMk5CouplingHandler - INCLUDE HandMk5CouplingHandler.h + TYPE CouplingXCubHandMk5 + INCLUDE CouplingXCubHandMk5.h DEFAULT OFF) -if(ENABLE_couplingHandMk5) +if(ENABLE_couplingXCubHandMk5) - yarp_add_plugin(yarp_couplingHandMk5) + yarp_add_plugin(yarp_couplingXCubHandMk5) if(MSVC) add_definitions(-D_USE_MATH_DEFINES) endif() - target_sources(yarp_couplingHandMk5 PRIVATE HandMk5CouplingHandler.cpp - HandMk5CouplingHandler.h) + target_sources(yarp_couplingXCubHandMk5 PRIVATE CouplingXCubHandMk5.cpp + CouplingXCubHandMk5.h) - target_link_libraries(yarp_couplingHandMk5 PRIVATE YARP::YARP_os - YARP::YARP_dev) + target_link_libraries(yarp_couplingXCubHandMk5 PRIVATE YARP::YARP_os + YARP::YARP_dev) list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS YARP_os YARP_dev) - yarp_install(TARGETS yarp_couplingHandMk5 + yarp_install(TARGETS yarp_couplingXCubHandMk5 EXPORT YARP_${YARP_PLUGIN_MASTER} COMPONENT ${YARP_PLUGIN_MASTER} LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} @@ -39,5 +39,5 @@ if(ENABLE_couplingHandMk5) set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE) - set_property(TARGET yarp_couplingHandMk5 PROPERTY FOLDER "Plugins/Device") + set_property(TARGET yarp_couplingXCubHandMk5 PROPERTY FOLDER "Plugins/Device") endif() diff --git a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp similarity index 87% rename from src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp rename to src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp index 0a4f8e72..e14b07fe 100644 --- a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp +++ b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp @@ -6,7 +6,7 @@ * BSD-3-Clause license. See the accompanying LICENSE file for details. */ -#include "HandMk5CouplingHandler.h" +#include "CouplingXCubHandMk5.h" #include #include #include @@ -15,7 +15,7 @@ YARP_DECLARE_LOG_COMPONENT(HANDMK5COUPLINGHANDLER) -double HandMk5CouplingHandler::evaluateCoupledJoint(const double& q1, const std::string& finger_name) +double CouplingXCubHandMk5::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 @@ -43,7 +43,7 @@ double HandMk5CouplingHandler::evaluateCoupledJoint(const double& q1, const std: } -double HandMk5CouplingHandler::evaluateCoupledJointJacobian(const double& q1, const std::string& finger_name) +double CouplingXCubHandMk5::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 @@ -72,14 +72,14 @@ double HandMk5CouplingHandler::evaluateCoupledJointJacobian(const double& q1, co } -HandMk5CouplingHandler::HandMk5CouplingHandler() +CouplingXCubHandMk5::CouplingXCubHandMk5() { m_couplingSize = 12; } -bool HandMk5CouplingHandler::parseFingerParameters(yarp::os::Searchable& config) +bool CouplingXCubHandMk5::parseFingerParameters(yarp::os::Searchable& config) { yarp::os::Bottle& hand_params = config.findGroup("COUPLING_PARAMS"); @@ -119,7 +119,7 @@ bool HandMk5CouplingHandler::parseFingerParameters(yarp::os::Searchable& config) return true; } -bool HandMk5CouplingHandler::parseCouplingParameters(yarp::os::Searchable& config) { +bool CouplingXCubHandMk5::parseCouplingParameters(yarp::os::Searchable& config) { yarp::sig::VectorOf coupled_physical_joints{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}; yarp::sig::VectorOf coupled_actuated_axes{0, 1, 2, 3, 4, 5}; std::vector physical_joint_names; @@ -197,7 +197,7 @@ bool HandMk5CouplingHandler::parseCouplingParameters(yarp::os::Searchable& confi return true; } -bool HandMk5CouplingHandler::open(yarp::os::Searchable& config) { +bool CouplingXCubHandMk5::open(yarp::os::Searchable& config) { // TODO INVOKE ImplementCoupling::initialise() bool ok = parseFingerParameters(config); @@ -215,7 +215,7 @@ bool HandMk5CouplingHandler::open(yarp::os::Searchable& config) { return ok; } -bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) { +bool CouplingXCubHandMk5::convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) { size_t nrOfPhysicalJoints; size_t nrOfActuatedAxes; auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); @@ -244,7 +244,7 @@ bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesPos(const ya return true; } -bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) { +bool CouplingXCubHandMk5::convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) { size_t nrOfPhysicalJoints; size_t nrOfActuatedAxes; auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); @@ -273,16 +273,16 @@ bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesVel(const ya } -bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) { +bool CouplingXCubHandMk5::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) { return false; } -bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) { +bool CouplingXCubHandMk5::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) { return false; } -bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) { +bool CouplingXCubHandMk5::convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) { size_t nrOfPhysicalJoints; size_t nrOfActuatedAxes; auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); @@ -319,7 +319,7 @@ bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsPos(const ya } -bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) { +bool CouplingXCubHandMk5::convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) { size_t nrOfPhysicalJoints; size_t nrOfActuatedAxes; auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); @@ -372,9 +372,9 @@ bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsVel(const ya return true; } -bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) { +bool CouplingXCubHandMk5::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) { return false; } -bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) { +bool CouplingXCubHandMk5::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) { return false; } diff --git a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.h similarity index 91% rename from src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h rename to src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.h index ecdb20e8..421bf6e5 100644 --- a/src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h +++ b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.h @@ -6,8 +6,8 @@ * BSD-3-Clause license. See the accompanying LICENSE file for details. */ -#ifndef HANDMK5COUPLINGHANDLER_H -#define HANDMK5COUPLINGHANDLER_H +#ifndef COUPLINGXCUBHANDMK5_H +#define COUPLINGXCUBHANDMK5_H #include #include @@ -18,11 +18,11 @@ /** TBD */ -class HandMk5CouplingHandler : public yarp::dev::DeviceDriver, - public yarp::dev::ImplementJointCoupling { +class CouplingXCubHandMk5 : public yarp::dev::DeviceDriver, + public yarp::dev::ImplementJointCoupling { public: - HandMk5CouplingHandler(); - ~HandMk5CouplingHandler() override; + CouplingXCubHandMk5(); + ~CouplingXCubHandMk5() override; bool convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) override; bool convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) override; bool convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) override; @@ -83,4 +83,4 @@ class HandMk5CouplingHandler : public yarp::dev::DeviceDriver, bool parseCouplingParameters(yarp::os::Searchable& config); }; -#endif // HANDMK5COUPLINGHANDLER_H \ No newline at end of file +#endif // COUPLINGXCUBHANDMK5_H \ No newline at end of file From e4b480e86880cdb4c31b6989e4f925a98091269c Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 13 Dec 2023 14:35:01 +0100 Subject: [PATCH 07/15] Add YCM ad dependency --- CMakeLists.txt | 33 +++++++++++++++++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 69ad745c..ca477c54 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,8 +6,8 @@ cmake_minimum_required(VERSION 3.12) -project(ergocub-software - VERSION 0.6.0) +project(ergocub-software LANGUAGES C CXX + VERSION 0.6.0) find_package(YARP 3.7.2 REQUIRED) @@ -16,6 +16,8 @@ if(POLICY CMP0046) cmake_policy(SET CMP0046 NEW) endif() +find_package(YCM 0.12 REQUIRED) + list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) include(AddUninstallTarget) @@ -23,6 +25,33 @@ include(AddUninstallTarget) option(BUILD_TESTING "Run tests for the generated models" OFF) option(COMPILE_ergoCubEmotions "Compile the module controlling the ergoCub emotions" OFF) +if(MSVC) + set(CMAKE_DEBUG_POSTFIX "d") +endif() + +# To build shared libraries in Windows, we set CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS to TRUE. +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + +option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) +# Build position independent code. +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +# Enable RPATH support for installed binaries and libraries +include(AddInstallRPATHSupport) +add_install_rpath_support(LIB_DIRS "${CMAKE_INSTALL_FULL_LIBDIR}" # Libraries + BIN_DIRS "${CMAKE_INSTALL_FULL_BINDIR}" # Binaries + "${CMAKE_INSTALL_FULL_LIBDIR}/yarp" # Plugins + INSTALL_NAME_DIR "${CMAKE_INSTALL_FULL_LIBDIR}" + USE_LINK_PATH) + +# Encourage user to specify a build type (e.g. Release, Debug, etc.), otherwise set it to Release. +if(NOT CMAKE_CONFIGURATION_TYPES) + if(NOT CMAKE_BUILD_TYPE) + message(STATUS "Setting build type to 'Release' as none was specified.") + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release") + endif() +endif() + set(BUILD_PREFIX "ergoCub") add_subdirectory(urdf) From ff25ae8d934cc02d95fe2f5861e3cded68ca9661 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 13 Dec 2023 14:53:23 +0100 Subject: [PATCH 08/15] CouplingXCubHandMk5: make it loadble as plugin --- CMakeLists.txt | 10 +---- .../couplingXCubHandMk5/CMakeLists.txt | 2 - .../CouplingXCubHandMk5.cpp | 7 ---- .../couplingXCubHandMk5/CouplingXCubHandMk5.h | 37 +++++++++---------- 4 files changed, 20 insertions(+), 36 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ca477c54..2619f445 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ cmake_minimum_required(VERSION 3.12) project(ergocub-software LANGUAGES C CXX VERSION 0.6.0) -find_package(YARP 3.7.2 REQUIRED) +find_package(YARP 3.9.0 REQUIRED) # Give error if add_dependencies is called on a non-existing target if(POLICY CMP0046) @@ -72,10 +72,4 @@ if (BUILD_TESTING) endif() add_subdirectory(app) - -if(COMPILE_ergoCubEmotions) - add_subdirectory(src) -endif() - - - +add_subdirectory(src) diff --git a/src/modules/couplingXCubHandMk5/CMakeLists.txt b/src/modules/couplingXCubHandMk5/CMakeLists.txt index 60f9f0f1..a2207635 100644 --- a/src/modules/couplingXCubHandMk5/CMakeLists.txt +++ b/src/modules/couplingXCubHandMk5/CMakeLists.txt @@ -4,8 +4,6 @@ # This software may be modified and distributed under the terms of the # BSD-3-Clause license. See the accompanying LICENSE file for details. -find_package(YARP 3.9.0 REQUIRED) - yarp_configure_plugins_installation(ergocub-software) yarp_prepare_plugin(couplingXCubHandMk5 diff --git a/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp index e14b07fe..12b71a4f 100644 --- a/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp +++ b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp @@ -72,13 +72,6 @@ double CouplingXCubHandMk5::evaluateCoupledJointJacobian(const double& q1, const } -CouplingXCubHandMk5::CouplingXCubHandMk5() -{ - m_couplingSize = 12; -} - - - bool CouplingXCubHandMk5::parseFingerParameters(yarp::os::Searchable& config) { diff --git a/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.h b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.h index 421bf6e5..9e5fdb27 100644 --- a/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.h +++ b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.h @@ -16,13 +16,28 @@ #include #include +/** + * Parameters from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling + */ +struct FingerParameters +{ + double L0x; + double L0y; + double q2bias; + double q1off; + double k; + double d; + double l; + double b; +}; + /** TBD */ class CouplingXCubHandMk5 : public yarp::dev::DeviceDriver, public yarp::dev::ImplementJointCoupling { public: - CouplingXCubHandMk5(); - ~CouplingXCubHandMk5() override; + CouplingXCubHandMk5() = default; + virtual ~CouplingXCubHandMk5() override = default; bool convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) override; bool convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) override; bool convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) override; @@ -32,8 +47,7 @@ class CouplingXCubHandMk5 : public yarp::dev::DeviceDriver, bool convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) override; bool convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) override; - //DeviceDriver - bool close() override; + // //DeviceDriver /** * Configure with a set of options. * @param config The options to use @@ -42,21 +56,6 @@ class CouplingXCubHandMk5 : public yarp::dev::DeviceDriver, bool open(yarp::os::Searchable& config) override; private: - /** - * Parameters from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling - */ - struct FingerParameters - { - double L0x; - double L0y; - double q2bias; - double q1off; - double k; - double d; - double l; - double b; - }; - std::unordered_map mFingerParameters; From f6d3a9ae8fd99d022a3beb98dc79ef7351a83f29 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Fri, 15 Dec 2023 12:07:28 +0100 Subject: [PATCH 09/15] CouplingXCubHandMk5: first working version --- .../CouplingXCubHandMk5.cpp | 42 +++++++++---------- .../couplingXCubHandMk5/CouplingXCubHandMk5.h | 4 ++ .../conf/gazebo_ergocub_left_hand_mk5_0.ini | 5 ++- .../conf/gazebo_ergocub_left_hand_mk5_1.ini | 5 ++- .../conf/gazebo_ergocub_right_hand_mk5_0.ini | 5 ++- .../conf/gazebo_ergocub_right_hand_mk5_1.ini | 5 ++- 6 files changed, 41 insertions(+), 25 deletions(-) diff --git a/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp index 12b71a4f..7445d67a 100644 --- a/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp +++ b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp @@ -8,11 +8,10 @@ #include "CouplingXCubHandMk5.h" #include -#include #include -YARP_DECLARE_LOG_COMPONENT(HANDMK5COUPLINGHANDLER) +YARP_LOG_COMPONENT(COUPLINGXCUBHANDMK5, "yarp.device.couplingXCubHandMk5") double CouplingXCubHandMk5::evaluateCoupledJoint(const double& q1, const std::string& finger_name) @@ -78,7 +77,7 @@ bool CouplingXCubHandMk5::parseFingerParameters(yarp::os::Searchable& config) yarp::os::Bottle& hand_params = config.findGroup("COUPLING_PARAMS"); if(hand_params.isNull()) { - yCError(HANDMK5COUPLINGHANDLER)<<"Missing COUPLING_PARAMS section in the configuration file"; + yCError(COUPLINGXCUBHANDMK5)<<"Missing COUPLING_PARAMS section in the configuration file"; return false; } auto L0x = hand_params.findGroup("L0x"); @@ -96,7 +95,7 @@ bool CouplingXCubHandMk5::parseFingerParameters(yarp::os::Searchable& config) q1off.size()!=nFingers+1 || k.size()!=nFingers+1 || d.size()!=nFingers+1 || l.size()!=nFingers+1 || b.size()!=nFingers+1 ) { - yCError(HANDMK5COUPLINGHANDLER)<<"Invalid hand parameters, check your configuration file"; + yCError(COUPLINGXCUBHANDMK5)<<"Invalid hand parameters, check your configuration file"; return false; } @@ -119,30 +118,31 @@ bool CouplingXCubHandMk5::parseCouplingParameters(yarp::os::Searchable& config) std::vector actuated_axes_names; std::vector> physical_joint_limits; - yarp::os::Bottle physical_joint_names_bottle = config.findGroup("jointNames"); + yarp::os::Bottle& physical_joint_names_bottle = config.findGroup("jointNames"); if (physical_joint_names_bottle.isNull()) { - yCError(HANDMK5COUPLINGHANDLER) << "Error cannot find jointNames." ; + yCError(COUPLINGXCUBHANDMK5) << "Error cannot find jointNames." ; return false; } - for (size_t i = 1; i < physical_joint_names.size(); i++) { + yCDebug(COUPLINGXCUBHANDMK5) << "Requested joints:" << physical_joint_names_bottle.toString(); + + for (size_t i = 1; i < physical_joint_names_bottle.size(); i++) { physical_joint_names.push_back(physical_joint_names_bottle.get(i).asString().c_str()); } yarp::os::Bottle& coupling_params = config.findGroup("COUPLING"); if (coupling_params.size() ==0) { - yCError(HANDMK5COUPLINGHANDLER) << "Missing param in COUPLING section"; + yCError(COUPLINGXCUBHANDMK5) << "Missing param in COUPLING section"; return false; } - yCDebug(HANDMK5COUPLINGHANDLER) << "Requested couplings:" << coupling_params.toString(); - yCDebug(HANDMK5COUPLINGHANDLER) << "Size: " << coupling_params.size(); + yCDebug(COUPLINGXCUBHANDMK5) << "Requested couplings:" << coupling_params.toString(); - yarp::os::Bottle actuated_axes_names_bottle = coupling_params.findGroup("actuatedAxesNames"); + yarp::os::Bottle& actuated_axes_names_bottle = coupling_params.findGroup("actuatedAxesNames"); if (actuated_axes_names_bottle.isNull()) { - yCError(HANDMK5COUPLINGHANDLER) << "Error cannot find actuatedAxesNames." ; + yCError(COUPLINGXCUBHANDMK5) << "Error cannot find actuatedAxesNames." ; return false; } @@ -163,7 +163,7 @@ bool CouplingXCubHandMk5::parseCouplingParameters(yarp::os::Searchable& config) } else { - yCError(HANDMK5COUPLINGHANDLER) << "Failed to parse jntPosMin parameter"; + yCError(COUPLINGXCUBHANDMK5) << "Failed to parse jntPosMin parameter"; return false; } yarp::os::Bottle& pos_limit_max = limits_bottle.findGroup("jntPosMax"); @@ -177,13 +177,13 @@ bool CouplingXCubHandMk5::parseCouplingParameters(yarp::os::Searchable& config) } else { - yCError(HANDMK5COUPLINGHANDLER) << "Failed to parse jntPosMax parameter"; + yCError(COUPLINGXCUBHANDMK5) << "Failed to parse jntPosMax parameter"; return false; } } else { - yCError(HANDMK5COUPLINGHANDLER) << "Failed to parse LIMITS parameter"; + yCError(COUPLINGXCUBHANDMK5) << "Failed to parse LIMITS parameter"; return false; } initialise(coupled_physical_joints, coupled_actuated_axes, physical_joint_names, actuated_axes_names, physical_joint_limits); @@ -195,13 +195,13 @@ bool CouplingXCubHandMk5::open(yarp::os::Searchable& config) { // TODO INVOKE ImplementCoupling::initialise() bool ok = parseFingerParameters(config); if (!ok) { - yCError(HANDMK5COUPLINGHANDLER) << "Error parsing finger parameters"; + yCError(COUPLINGXCUBHANDMK5) << "Error parsing finger parameters"; return false; } ok &= parseCouplingParameters(config); if (!ok) { - yCError(HANDMK5COUPLINGHANDLER) << "Error parsing coupling parameters"; + yCError(COUPLINGXCUBHANDMK5) << "Error parsing coupling parameters"; return false; } @@ -214,7 +214,7 @@ bool CouplingXCubHandMk5::convertFromPhysicalJointsToActuatedAxesPos(const yarp: auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); if (!ok || physJointsPos.size() != nrOfPhysicalJoints || actAxesPos.size() != nrOfActuatedAxes) { - yCError(HANDMK5COUPLINGHANDLER) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size"; + yCError(COUPLINGXCUBHANDMK5) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size"; return false; } @@ -243,7 +243,7 @@ bool CouplingXCubHandMk5::convertFromPhysicalJointsToActuatedAxesVel(const yarp: auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); if (!ok || physJointsVel.size() != nrOfPhysicalJoints || actAxesVel.size() != nrOfActuatedAxes) { - yCError(HANDMK5COUPLINGHANDLER) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size"; + yCError(COUPLINGXCUBHANDMK5) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size"; return false; } /* thumb_add <-- thumb_add */ @@ -281,7 +281,7 @@ bool CouplingXCubHandMk5::convertFromActuatedAxesToPhysicalJointsPos(const yarp: auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); if (!ok || physJointsPos.size() != nrOfPhysicalJoints || actAxesPos.size() != nrOfActuatedAxes) { - yCError(HANDMK5COUPLINGHANDLER) << "convertFromActuatedAxesToPhysicalJointsPos: input or output vectors have wrong size"; + yCError(COUPLINGXCUBHANDMK5) << "convertFromActuatedAxesToPhysicalJointsPos: input or output vectors have wrong size"; return false; } physJointsPos[0] = actAxesPos[0]; @@ -318,7 +318,7 @@ bool CouplingXCubHandMk5::convertFromActuatedAxesToPhysicalJointsVel(const yarp: auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); if (!ok || actAxesPos.size() != nrOfActuatedAxes || physJointsVel.size() != nrOfPhysicalJoints || actAxesVel.size() != nrOfActuatedAxes) { - yCError(HANDMK5COUPLINGHANDLER) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size"; + yCError(COUPLINGXCUBHANDMK5) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size"; return false; } diff --git a/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.h b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.h index 9e5fdb27..d0336886 100644 --- a/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.h +++ b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.h @@ -9,6 +9,7 @@ #ifndef COUPLINGXCUBHANDMK5_H #define COUPLINGXCUBHANDMK5_H +#include #include #include @@ -16,6 +17,9 @@ #include #include + +YARP_DECLARE_LOG_COMPONENT(COUPLINGXCUBHANDMK5) + /** * Parameters from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling */ diff --git a/urdf/ergoCub/conf/gazebo_ergocub_left_hand_mk5_0.ini b/urdf/ergoCub/conf/gazebo_ergocub_left_hand_mk5_0.ini index 7e0b40a5..e70e6ba5 100644 --- a/urdf/ergoCub/conf/gazebo_ergocub_left_hand_mk5_0.ini +++ b/urdf/ergoCub/conf/gazebo_ergocub_left_hand_mk5_0.ini @@ -4,7 +4,10 @@ jointNames l_thumb_add l_thumb_prox l_thumb_dist l_index_add l_index_prox l_inde max_torques 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 [COUPLING] -icub_hand_mk5 (0 1 2 3 4 5 6 7 8 9 10 11) (l_thumb_add l_thumb_oc l_index_add l_index_oc l_middle_oc l_ring_pinky_oc reserved reserved reserved reserved reserved reserved) (0.0 0.0 0.0 0.0 0.0 0.0) (90.0 82.1 15.0 90.0 90.0 90.0) +device couplingXCubHandMk5 +actuatedAxesNames l_thumb_add l_thumb_oc l_index_add l_index_oc l_middle_oc l_ring_pinky_oc +actuatedAxesPosMin 0.0 0.0 0.0 0.0 0.0 0.0 +actuatedAxesPosMax 90.0 82.1 15.0 90.0 90.0 90.0 [COUPLING_PARAMS] L0x -0.00555 -0.0050 -0.0050 -0.0050 -0.0050 diff --git a/urdf/ergoCub/conf/gazebo_ergocub_left_hand_mk5_1.ini b/urdf/ergoCub/conf/gazebo_ergocub_left_hand_mk5_1.ini index 2fd4a31f..15b86b3f 100644 --- a/urdf/ergoCub/conf/gazebo_ergocub_left_hand_mk5_1.ini +++ b/urdf/ergoCub/conf/gazebo_ergocub_left_hand_mk5_1.ini @@ -4,7 +4,10 @@ jointNames l_thumb_add l_thumb_prox l_thumb_dist l_index_add l_index_prox l_inde max_torques 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 [COUPLING] -icub_hand_mk5 (0 1 2 3 4 5 6 7 8 9 10 11) (l_thumb_add l_thumb_oc l_index_add l_index_oc l_middle_oc l_ring_pinky_oc reserved reserved reserved reserved reserved reserved) (0.0 0.0 0.0 0.0 0.0 0.0) (100.0 90.0 15.0 90.0 90.0 90.0) +device couplingXCubHandMk5 +actuatedAxesNames l_thumb_add l_thumb_oc l_index_add l_index_oc l_middle_oc l_ring_pinky_oc +actuatedAxesPosMin 0.0 0.0 0.0 0.0 0.0 0.0 +actuatedAxesPosMax 100.0 90.0 15.0 90.0 90.0 90.0 [COUPLING_PARAMS] L0x -0.0050 -0.0050 -0.0050 -0.0050 -0.0050 diff --git a/urdf/ergoCub/conf/gazebo_ergocub_right_hand_mk5_0.ini b/urdf/ergoCub/conf/gazebo_ergocub_right_hand_mk5_0.ini index 4a6c35e2..de40d0cd 100644 --- a/urdf/ergoCub/conf/gazebo_ergocub_right_hand_mk5_0.ini +++ b/urdf/ergoCub/conf/gazebo_ergocub_right_hand_mk5_0.ini @@ -4,7 +4,10 @@ jointNames r_thumb_add r_thumb_prox r_thumb_dist r_index_add r_index_prox r_inde max_torques 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 [COUPLING] -icub_hand_mk5 (0 1 2 3 4 5 6 7 8 9 10 11) (r_thumb_add r_thumb_oc r_index_add r_index_oc r_middle_oc r_ring_pinky_oc reserved reserved reserved reserved reserved reserved) (0.0 0.0 0.0 0.0 0.0 0.0) (90.0 82.1 15.0 90.0 90.0 90.0) +device couplingXCubHandMk5 +actuatedAxesNames r_thumb_add r_thumb_oc r_index_add r_index_oc r_middle_oc r_ring_pinky_oc +actuatedAxesPosMin 0.0 0.0 0.0 0.0 0.0 0.0 +actuatedAxesPosMax 90.0 82.1 15.0 90.0 90.0 90.0 [COUPLING_PARAMS] L0x -0.00555 -0.0050 -0.0050 -0.0050 -0.0050 diff --git a/urdf/ergoCub/conf/gazebo_ergocub_right_hand_mk5_1.ini b/urdf/ergoCub/conf/gazebo_ergocub_right_hand_mk5_1.ini index 1db4c333..90a786b1 100644 --- a/urdf/ergoCub/conf/gazebo_ergocub_right_hand_mk5_1.ini +++ b/urdf/ergoCub/conf/gazebo_ergocub_right_hand_mk5_1.ini @@ -4,7 +4,10 @@ jointNames r_thumb_add r_thumb_prox r_thumb_dist r_index_add r_index_prox r_inde max_torques 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 [COUPLING] -icub_hand_mk5 (0 1 2 3 4 5 6 7 8 9 10 11) (r_thumb_add r_thumb_oc r_index_add r_index_oc r_middle_oc r_ring_pinky_oc reserved reserved reserved reserved reserved reserved) (0.0 0.0 0.0 0.0 0.0 0.0) (100.0 90.0 15.0 90.0 90.0 90.0) +device couplingXCubHandMk5 +actuatedAxesNames r_thumb_add r_thumb_oc r_index_add r_index_oc r_middle_oc r_ring_pinky_oc +actuatedAxesPosMin 0.0 0.0 0.0 0.0 0.0 0.0 +actuatedAxesPosMax 100.0 90.0 15.0 90.0 90.0 90.0 [COUPLING_PARAMS] L0x -0.0050 -0.0050 -0.0050 -0.0050 -0.0050 From 6d5688016315c50df238a3f1af78fdc23765f679 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 18 Dec 2023 11:55:06 +0100 Subject: [PATCH 10/15] couplingXCubHandMk5: add documentation --- src/modules/couplingXCubHandMk5/README.md | 83 +++++++++++++++++++++++ 1 file changed, 83 insertions(+) create mode 100644 src/modules/couplingXCubHandMk5/README.md diff --git a/src/modules/couplingXCubHandMk5/README.md b/src/modules/couplingXCubHandMk5/README.md new file mode 100644 index 00000000..e38b9586 --- /dev/null +++ b/src/modules/couplingXCubHandMk5/README.md @@ -0,0 +1,83 @@ +![YARP logo](https://raw.githubusercontent.com/robotology/yarp/master/doc/images/yarp-robot-24.png "yarp-device-realsense2") +## couplingXCubHandMk5 + + +This is the device for [YARP](https://www.yarp.it/) for handling the coupling of the [hands mk5](https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling/) + + +### Build and install yarp-device-realsense2 + +```bash +mkdir build +cd build +cmake -DCMAKE_INSTALL_PREFIX= -DENABLE_couplingXCubHandMk5=BOOL:ON .. +make +make install +``` + +In order to make the device detectable, add `/share/yarp` to the `YARP_DATA_DIRS` environment variable of the system. + +Alternatively, if `YARP` has been installed using the [robotology-superbuild](https://github.com/robotology/robotology-superbuild), it is possible to use `/build/install` as the ``. + + +## Device documentation + +This device driver exposes the `yarp::dev::IJointCoupling` interface to getting +quantities from the physical joints to the actuated axes domain and viceversa. +See the documentation for more details about each interface. + + +| YARP device name | +|:----------------:| +| `couplingXCubHandMk5` | + +Parameters used by this device are: + +| Parameter name | SubParameter | Type | Read / write | Units | Default Value | Required | Description | Notes | +|:----------------------------:|:-----------------:|:--------------:|:------------:|:-------:|:-------------:|:---------------:|:-------------------------------------------------------------------------------------:|:---------------------------------------------------------------------:| +| `jointNames` | - | list of strings | Read / write | - | - | Yes | Names of the physical joints | | +| `LIMITS` | - | group | Read / write | - | - | Yes | group containing the physical joint limits | | +| | `jntPosMin` | list of double | Read / write | - | - | Yes | Phyisical joints' position minimum | | +| | `jntPosMax` | list of double | Read / write | - | - | Yes | Phyisical joints' position maximum | | +| `COUPLING` | - | group | Read / write | - | - | Yes | The group containing the coupling description +| | `actuatedAxesNames` | list of strings | Read / write | - | - | Yes | Names of the actuated axes | | +| | `actuatedAxesPosMin` | list of strings | Read / write | - | - | Yes | Actuated axes' position minimum | | +| | `actuatedAxesPosMax` | list of strings | Read / write | - | - | Yes | Actuated axes' position maximum | | +| `COUPLING_PARAMS` | - | group | Read / write | - | - | Yes | The group containing the coupling params | | +| | `L0x` | list of double | Read / write | - | - | Yes | x coordinate of the first end of the lever is applied | The lenght of the list must be 5 | +| | `L0y` | list of double | Read / write | - | - | Yes | y coordinate of the first end of the lever is applied | The lenght of the list must be 5 | +| | `q2bias` | list of double | Read / write | - | - | Yes | Angle of L1 - P1 when the finger is fully open | The lenght of the list must be 5 | +| | `q1off` | list of double | Read / write | - | - | Yes | Angle of P1 - P0 when the finger is fully closed | The lenght of the list must be 5 | +| | `k` | list of double | Read / write | - | - | Yes | Connecting rod length, \|L1-L0\| | The lenght of the list must be 5 | +| | `d` | list of double | Read / write | - | - | Yes | Distance between the two joints, P1 and P0 | The lenght of the list must be 5 | +| | `l` | list of double | Read / write | - | - | Yes | Distance between L1 and P1 | The lenght of the list must be 5 | +| | `b` | list of double | Read / write | - | - | Yes | Distance between L0 and P0 | The lenght of the list must be 5 | + + + +Configuration file using `.ini` format: + +```ini +device couplingXCubHandMk5 +jointNames l_thumb_add l_thumb_prox l_thumb_dist l_index_add l_index_prox l_index_dist l_middle_prox l_middle_dist l_ring_prox l_ring_dist l_pinkie_prox l_pinkie_dist + +[COUPLING] +actuatedAxesNames l_thumb_add l_thumb_oc l_index_add l_index_oc l_middle_oc l_ring_pinky_oc +actuatedAxesPosMin 0.0 0.0 0.0 0.0 0.0 0.0 +actuatedAxesPosMax 90.0 82.1 15.0 90.0 90.0 90.0 + +[COUPLING_PARAMS] +L0x -0.00555 -0.0050 -0.0050 -0.0050 -0.0050 +L0y 0.00285 0.0040 0.0040 0.0040 0.0040 +q2bias -180.0 -173.35 -173.35 -173.35 -170.54 +q1off 4.29 2.86 2.86 2.86 3.43 +k 0.0171 0.02918 0.02918 0.02918 0.02425 +d 0.02006 0.03004 0.03004 0.03004 0.02504 +l 0.0085 0.00604 0.00604 0.00604 0.00608 +b 0.00624 0.0064 0.0064 0.0064 0.0064 + + +[LIMITS] +jntPosMax 90.0 82.1 53.6 15.0 90.0 99.2 90.0 99.2 90.0 99.2 90.0 93.3 +jntPosMin 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 +``` From d4d6d0dea09d6cddb7a4e3d8f291360abeb09478 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 18 Dec 2023 11:57:34 +0100 Subject: [PATCH 11/15] Update CHANGELOG --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 01ed9cf0..32151136 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] - ergoCubV1: fixed the names r/l_upperarm with r/l_upper_arm(https://github.com/icub-tech-iit/ergocub-software/issues/197) - Fixed HF when resetting the world when using DART as PE(https://github.com/icub-tech-iit/ergocub-software/issues/190) +- Added `couplingXCubHandMk5` device ## [0.6.0] - 2023-11-15 From d9fd0d92a213363742d3aece4d3076debc741e95 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 18 Dec 2023 11:59:23 +0100 Subject: [PATCH 12/15] Update README --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index ac6bc8f3..b8bb6e35 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ Main collector of ergoCub specific SW ### Dependencies -Before installing `ergocub-software`, please be sure that you've installed [YARP](https://www.yarp.it/latest//) on your machine. +Before installing `ergocub-software`, please be sure that you've installed [YARP](https://www.yarp.it/latest//) 3.9.0 or higher on your machine. ### Compiling from source @@ -17,7 +17,7 @@ cmake -DCMAKE_INSTALL_PREFIX= .. make (make install) ``` -`ergocub` for Gazebo Classic simulation model needs a gazebo-yarp-plugins >= 4.9.0 . +`ergocub` for Gazebo Classic simulation model needs a gazebo-yarp-plugins latest devel (FIXME put commit). In order to use the model, the following env variables must be configured: ```sh From c0efaa845092f09842d2e636b4b6ae19fa575829 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 20 Dec 2023 14:01:34 +0100 Subject: [PATCH 13/15] Fix readmes --- README.md | 4 ++++ src/modules/couplingXCubHandMk5/README.md | 16 ---------------- 2 files changed, 4 insertions(+), 16 deletions(-) diff --git a/README.md b/README.md index b8bb6e35..d34baac0 100644 --- a/README.md +++ b/README.md @@ -28,6 +28,10 @@ export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:/share/ergoCub/rob export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:/share/ ``` +In order to make the device `couplingXCubHandMk5` detectable, add `/share/yarp` to the `YARP_DATA_DIRS` environment variable of the system. + +Alternatively, if `YARP` has been installed using the [robotology-superbuild](https://github.com/robotology/robotology-superbuild), it is possible to use `/build/install` as the ``. + ### Use conda binary packages This repository is packaged as `ergocub-software` in the `robotology` conda channel, see https://anaconda.org/robotology/ergocub-software . diff --git a/src/modules/couplingXCubHandMk5/README.md b/src/modules/couplingXCubHandMk5/README.md index e38b9586..63402c78 100644 --- a/src/modules/couplingXCubHandMk5/README.md +++ b/src/modules/couplingXCubHandMk5/README.md @@ -4,22 +4,6 @@ This is the device for [YARP](https://www.yarp.it/) for handling the coupling of the [hands mk5](https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling/) - -### Build and install yarp-device-realsense2 - -```bash -mkdir build -cd build -cmake -DCMAKE_INSTALL_PREFIX= -DENABLE_couplingXCubHandMk5=BOOL:ON .. -make -make install -``` - -In order to make the device detectable, add `/share/yarp` to the `YARP_DATA_DIRS` environment variable of the system. - -Alternatively, if `YARP` has been installed using the [robotology-superbuild](https://github.com/robotology/robotology-superbuild), it is possible to use `/build/install` as the ``. - - ## Device documentation This device driver exposes the `yarp::dev::IJointCoupling` interface to getting From 71c5d7afb76e013e2c4b74b0c77c247d396d8ca1 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 20 Dec 2023 14:02:05 +0100 Subject: [PATCH 14/15] couplingXCubHandMk5: make it compiled by default --- src/modules/couplingXCubHandMk5/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/couplingXCubHandMk5/CMakeLists.txt b/src/modules/couplingXCubHandMk5/CMakeLists.txt index a2207635..5201c40c 100644 --- a/src/modules/couplingXCubHandMk5/CMakeLists.txt +++ b/src/modules/couplingXCubHandMk5/CMakeLists.txt @@ -10,7 +10,7 @@ yarp_prepare_plugin(couplingXCubHandMk5 CATEGORY device TYPE CouplingXCubHandMk5 INCLUDE CouplingXCubHandMk5.h - DEFAULT OFF) + DEFAULT ON) if(ENABLE_couplingXCubHandMk5) From d1e4db15e3a7b4fcb37a83629cdca05ede444c9e Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 20 Dec 2023 14:10:09 +0100 Subject: [PATCH 15/15] couplinXCubHandMk5: fix compilation --- src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp index 7445d67a..6ee16c1e 100644 --- a/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp +++ b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp @@ -9,6 +9,7 @@ #include "CouplingXCubHandMk5.h" #include #include +#include YARP_LOG_COMPONENT(COUPLINGXCUBHANDMK5, "yarp.device.couplingXCubHandMk5")