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 diff --git a/CMakeLists.txt b/CMakeLists.txt index 9049ac3f..2619f445 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,16 +6,18 @@ 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) +find_package(YARP 3.9.0 REQUIRED) # Give error if add_dependencies is called on a non-existing target 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) @@ -43,9 +72,4 @@ if (BUILD_TESTING) endif() add_subdirectory(app) - -if(COMPILE_ergoCubEmotions) - add_subdirectory(src) -endif() - - +add_subdirectory(src) diff --git a/README.md b/README.md index ac6bc8f3..d34baac0 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 @@ -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/CMakeLists.txt b/src/modules/CMakeLists.txt index 79ebdfb1..845ff50e 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(couplingXCubHandMk5) diff --git a/src/modules/couplingXCubHandMk5/CMakeLists.txt b/src/modules/couplingXCubHandMk5/CMakeLists.txt new file mode 100644 index 00000000..5201c40c --- /dev/null +++ b/src/modules/couplingXCubHandMk5/CMakeLists.txt @@ -0,0 +1,41 @@ +# 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. + +yarp_configure_plugins_installation(ergocub-software) + +yarp_prepare_plugin(couplingXCubHandMk5 + CATEGORY device + TYPE CouplingXCubHandMk5 + INCLUDE CouplingXCubHandMk5.h + DEFAULT ON) + +if(ENABLE_couplingXCubHandMk5) + + yarp_add_plugin(yarp_couplingXCubHandMk5) + + if(MSVC) + add_definitions(-D_USE_MATH_DEFINES) + endif() + + target_sources(yarp_couplingXCubHandMk5 PRIVATE CouplingXCubHandMk5.cpp + CouplingXCubHandMk5.h) + + 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_couplingXCubHandMk5 + 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_couplingXCubHandMk5 PROPERTY FOLDER "Plugins/Device") +endif() diff --git a/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp new file mode 100644 index 00000000..6ee16c1e --- /dev/null +++ b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.cpp @@ -0,0 +1,374 @@ +/* + * 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 "CouplingXCubHandMk5.h" +#include +#include +#include + + +YARP_LOG_COMPONENT(COUPLINGXCUBHANDMK5, "yarp.device.couplingXCubHandMk5") + + +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 + */ + 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 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 + */ + 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; +} + + +bool CouplingXCubHandMk5::parseFingerParameters(yarp::os::Searchable& config) +{ + + yarp::os::Bottle& hand_params = config.findGroup("COUPLING_PARAMS"); + if(hand_params.isNull()) + { + yCError(COUPLINGXCUBHANDMK5)<<"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"); + 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 ) + { + yCError(COUPLINGXCUBHANDMK5)<<"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 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; + 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(COUPLINGXCUBHANDMK5) << "Error cannot find jointNames." ; + return false; + } + + 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(COUPLINGXCUBHANDMK5) << "Missing param in COUPLING section"; + return false; + } + yCDebug(COUPLINGXCUBHANDMK5) << "Requested couplings:" << coupling_params.toString(); + + yarp::os::Bottle& actuated_axes_names_bottle = coupling_params.findGroup("actuatedAxesNames"); + + if (actuated_axes_names_bottle.isNull()) { + yCError(COUPLINGXCUBHANDMK5) << "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(COUPLINGXCUBHANDMK5) << "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(COUPLINGXCUBHANDMK5) << "Failed to parse jntPosMax parameter"; + return false; + } + } + else + { + 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); + return true; +} + +bool CouplingXCubHandMk5::open(yarp::os::Searchable& config) { + + // TODO INVOKE ImplementCoupling::initialise() + bool ok = parseFingerParameters(config); + if (!ok) { + yCError(COUPLINGXCUBHANDMK5) << "Error parsing finger parameters"; + return false; + } + + ok &= parseCouplingParameters(config); + if (!ok) { + yCError(COUPLINGXCUBHANDMK5) << "Error parsing coupling parameters"; + return false; + } + + return ok; +} + +bool CouplingXCubHandMk5::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(COUPLINGXCUBHANDMK5) << "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 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); + ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); + if (!ok || physJointsVel.size() != nrOfPhysicalJoints || actAxesVel.size() != nrOfActuatedAxes) { + yCError(COUPLINGXCUBHANDMK5) << "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 CouplingXCubHandMk5::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) { + return false; +} + +bool CouplingXCubHandMk5::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) { + return false; +} + + +bool CouplingXCubHandMk5::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(COUPLINGXCUBHANDMK5) << "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 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); + ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); + if (!ok || actAxesPos.size() != nrOfActuatedAxes || physJointsVel.size() != nrOfPhysicalJoints || actAxesVel.size() != nrOfActuatedAxes) { + yCError(COUPLINGXCUBHANDMK5) << "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 CouplingXCubHandMk5::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) { + return false; +} +bool CouplingXCubHandMk5::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) { + return false; +} diff --git a/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.h b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.h new file mode 100644 index 00000000..d0336886 --- /dev/null +++ b/src/modules/couplingXCubHandMk5/CouplingXCubHandMk5.h @@ -0,0 +1,89 @@ +/* + * 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 COUPLINGXCUBHANDMK5_H +#define COUPLINGXCUBHANDMK5_H + +#include +#include +#include + +#include +#include +#include + + +YARP_DECLARE_LOG_COMPONENT(COUPLINGXCUBHANDMK5) + +/** + * 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() = 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; + 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 + /** + * 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: + + std::unordered_map mFingerParameters; + + + /* + * 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::Searchable& config); + bool parseCouplingParameters(yarp::os::Searchable& config); +}; + +#endif // COUPLINGXCUBHANDMK5_H \ No newline at end of file diff --git a/src/modules/couplingXCubHandMk5/README.md b/src/modules/couplingXCubHandMk5/README.md new file mode 100644 index 00000000..63402c78 --- /dev/null +++ b/src/modules/couplingXCubHandMk5/README.md @@ -0,0 +1,67 @@ +![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/) + +## 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 +``` 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