Add couplingICubHandMk2 device coupling handler

src/libraries/icubmod/CMakeLists.txt

@@ -54,6 +54,7 @@ yarp_begin_plugin_library(icubmod QUIET)
   add_subdirectory(rawValuesPublisherClient)
   add_subdirectory(rawValuesPublisherServer)
   add_subdirectory(fakeRawValuesPublisher)
+  add_subdirectory(couplingICubHandMk2)
 if (ICUB_ICUBINTERFACE_EXPERIMENTAL)
   add_subdirectory(imu3DM_GX3)
 endif()

src/libraries/icubmod/couplingICubHandMk2/CMakeLists.txt

@@ -0,0 +1,42 @@
+# Copyright (C) 2006-2024 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_prepare_plugin(couplingICubHandMk2
+                    CATEGORY device
+                    TYPE CouplingICubHandMk2
+                    INCLUDE CouplingICubHandMk2.h
+                    GENERATE_PARSER
+                    DEFAULT ON)
+
+if(ENABLE_couplingICubHandMk2)
+  option(ALLOW_DEVICE_PARAM_PARSER_GENERATION "Generate the param parser for couplingICubHandMk2 device" OFF)
+  yarp_add_plugin(yarp_couplingICubHandMk2)
+
+  if(MSVC)
+    add_definitions(-D_USE_MATH_DEFINES)
+  endif()
+
+  target_sources(yarp_couplingICubHandMk2 PRIVATE CouplingICubHandMk2.cpp
+                                                  CouplingICubHandMk2.h
+                                                  CouplingICubHandMk2_ParamsParser.cpp
+                                                  CouplingICubHandMk2_ParamsParser.h)
+
+  target_link_libraries(yarp_couplingICubHandMk2 PRIVATE YARP::YARP_os
+                                                         YARP::YARP_dev)
+  list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS YARP_os
+                                                      YARP_dev)
+
+  yarp_install(TARGETS yarp_couplingICubHandMk2
+               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_couplingICubHandMk2 PROPERTY FOLDER "Plugins/Device")
+endif()

src/libraries/icubmod/couplingICubHandMk2/CouplingICubHandMk2.cpp

@@ -0,0 +1,313 @@
+/*
+ * Copyright (C) 2006-2024 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 "CouplingICubHandMk2.h"
+#include <yarp/os/LogStream.h>
+#include <cmath>
+#include <array>
+#include <numeric>
+
+
+YARP_LOG_COMPONENT(COUPLINGICUBHANDMK2, "yarp.device.couplingICubHandMk2")
+
+
+bool CouplingICubHandMk2::populateCouplingParameters() {
+    yarp::sig::VectorOf<size_t> coupled_physical_joints, coupled_actuated_axes;
+    coupled_physical_joints.resize(m_jointNames.size());
+    coupled_actuated_axes.resize(m_COUPLING_actuatedAxesNames.size());
+    std::iota(coupled_physical_joints.begin(), coupled_physical_joints.end(), 0);
+    std::iota(coupled_actuated_axes.begin(), coupled_actuated_axes.end(), 0);
+    std::vector<std::pair<double, double>> physical_joint_limits;
+
+    physical_joint_limits.resize(m_jointNames.size());
+    for (int i = 0; i< m_jointNames.size(); i++)
+    {
+        physical_joint_limits[i] = std::make_pair(m_LIMITS_jntPosMin[i], m_LIMITS_jntPosMax[i]);
+    }
+    initialise(coupled_physical_joints, coupled_actuated_axes, m_jointNames, m_COUPLING_actuatedAxesNames, physical_joint_limits);
+    return true;
+}
+
+bool CouplingICubHandMk2::open(yarp::os::Searchable& config) {
+
+    if(!parseParams(config)) {
+        yCError(COUPLINGICUBHANDMK2) << "Error parsing parameters";
+        return false;
+    }
+
+    if(!populateCouplingParameters()) {
+        yCError(COUPLINGICUBHANDMK2) << "Error populating coupling parameters";
+        return false;
+    }
+
+    yCDebug(COUPLINGICUBHANDMK2) << "Opening CouplingICubHandMk2"<<config.toString();
+    return true;
+}
+
+bool CouplingICubHandMk2::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(COUPLINGICUBHANDMK2) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
+        return false;
+    }
+
+    // l/r_hand_finger
+    actAxesPos[0] = (20.0 - physJointsPos[12])*3;
+    // l/r_thumb_oppose
+    actAxesPos[1] = physJointsPos[0];
+    // l/r_thumb_proximal
+    actAxesPos[2] = physJointsPos[1];
+    // l/r_thumb_distal
+    actAxesPos[3] = physJointsPos[2] + physJointsPos[3];
+    // l/r_index_proximal
+    actAxesPos[4] = physJointsPos[5];
+    // l/r_index_distal
+    actAxesPos[5] = physJointsPos[6] + physJointsPos[7];
+    // l/r_middle_proximal
+    actAxesPos[6] = physJointsPos[9];
+    // l/r_middle_distal
+    actAxesPos[7] = physJointsPos[10] + physJointsPos[11];
+    // l/r_pinky
+    actAxesPos[8] = physJointsPos[13] + physJointsPos[14] + physJointsPos[15];
+    return true;
+}
+
+bool CouplingICubHandMk2::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(COUPLINGICUBHANDMK2) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size";
+        return false;
+    }
+    // l/r_hand_finger
+    actAxesVel[0] = -physJointsVel[12]*3;
+    // l/r_thumb_oppose
+    actAxesVel[1] = physJointsVel[0];
+    // l/r_thumb_proximal
+    actAxesVel[2] = physJointsVel[1];
+    // l/r_thumb_distal
+    actAxesVel[3] = physJointsVel[2] + physJointsVel[3];
+    // l/r_index_proximal
+    actAxesVel[4] = physJointsVel[5];
+    // l/r_index_distal
+    actAxesVel[5] = physJointsVel[6] + physJointsVel[7];
+    // l/r_middle_proximal
+    actAxesVel[6] = physJointsVel[9];
+    // l/r_middle_distal
+    actAxesVel[7] = physJointsVel[10] + physJointsVel[11];
+    // l/r_pinky
+    actAxesVel[8] = physJointsVel[13] + physJointsVel[14] + physJointsVel[15];
+    return true;
+}
+
+
+bool CouplingICubHandMk2::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) {
+    size_t nrOfPhysicalJoints;
+    size_t nrOfActuatedAxes;
+    auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
+    ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
+    if (!ok || physJointsAcc.size() != nrOfPhysicalJoints || actAxesAcc.size() != nrOfActuatedAxes) {
+        yCError(COUPLINGICUBHANDMK2) << "convertFromPhysicalJointsToActuatedAxesAcc: input or output vectors have wrong size";
+        return false;
+    }
+    // l/r_hand_finger
+    actAxesAcc[0] = -physJointsAcc[12]*3;
+    // l/r_thumb_oppose
+    actAxesAcc[1] = physJointsAcc[0];
+    // l/r_thumb_proximal
+    actAxesAcc[2] = physJointsAcc[1];
+    // l/r_thumb_distal
+    actAxesAcc[3] = physJointsAcc[2] + physJointsAcc[3];
+    // l/r_index_proximal
+    actAxesAcc[4] = physJointsAcc[5];
+    // l/r_index_distal
+    actAxesAcc[5] = physJointsAcc[6] + physJointsAcc[7];
+    // l/r_middle_proximal
+    actAxesAcc[6] = physJointsAcc[9];
+    // l/r_middle_distal
+    actAxesAcc[7] = physJointsAcc[10] + physJointsAcc[11];
+    // l/r_pinky
+    actAxesAcc[8] = physJointsAcc[13] + physJointsAcc[14] + physJointsAcc[15];
+    return false;
+}
+
+bool CouplingICubHandMk2::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) {
+    return false;
+}
+
+
+bool CouplingICubHandMk2::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(COUPLINGICUBHANDMK2) << "convertFromActuatedAxesToPhysicalJointsPos: input or output vectors have wrong size";
+        return false;
+    }
+
+    // l/r_hand_thumb_0
+    physJointsPos[0] = actAxesPos[1];
+    // l/r_hand_thumb_1
+    physJointsPos[1] = actAxesPos[2];
+    // l/r_hand_thumb_2
+    physJointsPos[2] = actAxesPos[3]/2;
+    // l/r_hand_thumb_3
+    physJointsPos[3] = actAxesPos[3]/2;
+    // l/r_hand_index_0
+    physJointsPos[4] = -(20.0 - actAxesPos[0]/3);
+    // l/r_hand_index_1
+    physJointsPos[5] = actAxesPos[4];
+    // l/r_hand_index_2
+    physJointsPos[6] = actAxesPos[5]/2;
+    // l/r_hand_index_3
+    physJointsPos[7] = actAxesPos[5]/2;
+    // l/r_hand_middle_0
+    physJointsPos[8] = 0.0;
+    // l/r_hand_middle_1
+    physJointsPos[9] = actAxesPos[6];
+    // l/r_hand_middle_2
+    physJointsPos[10] = actAxesPos[7]/2;
+    // l/r_hand_middle_3
+    physJointsPos[11] = actAxesPos[7]/2;
+    // l/r_hand_ring_0
+    physJointsPos[12] = 20.0 - actAxesPos[0]/3;
+    // l/r_hand_ring_1
+    physJointsPos[13] = actAxesPos[8]/3;
+    // l/r_hand_ring_2
+    physJointsPos[14] = actAxesPos[8]/3;
+    // l/r_hand_ring_3
+    physJointsPos[15] = actAxesPos[8]/3;
+    // l/r_hand_little_0
+    physJointsPos[16] = 20.0 - actAxesPos[0]/3;
+    // l/r_hand_little_1
+    physJointsPos[17] = actAxesPos[8]/3;
+    // l/r_hand_little_2
+    physJointsPos[18] = actAxesPos[8]/3;
+    // l/r_hand_little_3
+    physJointsPos[19] = actAxesPos[8]/3;
+
+    return true;
+}
+
+
+bool CouplingICubHandMk2::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(COUPLINGICUBHANDMK2) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size";
+        return false;
+    }
+
+    // l/r_hand_thumb_0
+    physJointsVel[0] = actAxesVel[1];
+    // l/r_hand_thumb_1
+    physJointsVel[1] = actAxesVel[2];
+    // l/r_hand_thumb_2
+    physJointsVel[2] = actAxesVel[3]/2;
+    // l/r_hand_thumb_3
+    physJointsVel[3] = actAxesVel[3]/2;
+    // l/r_hand_index_0
+    physJointsVel[4] = actAxesVel[0]/3;
+    // l/r_hand_index_1
+    physJointsVel[5] = actAxesVel[4];
+    // l/r_hand_index_2
+    physJointsVel[6] = actAxesVel[5]/2;
+    // l/r_hand_index_3
+    physJointsVel[7] = actAxesVel[5]/2;
+    // l/r_hand_middle_0
+    physJointsVel[8] = 0.0;
+    // l/r_hand_middle_1
+    physJointsVel[9] = actAxesVel[6];
+    // l/r_hand_middle_2
+    physJointsVel[10] = actAxesVel[7]/2;
+    // l/r_hand_middle_3
+    physJointsVel[11] = actAxesVel[7]/2;
+    // l/r_hand_ring_0
+    physJointsVel[12] = -actAxesVel[0]/3;
+    // l/r_hand_ring_1
+    physJointsVel[13] = actAxesVel[8]/3;
+    // l/r_hand_ring_2
+    physJointsVel[14] = actAxesVel[8]/3;
+    // l/r_hand_ring_3
+    physJointsVel[15] = actAxesVel[8]/3;
+    // l/r_hand_little_0
+    physJointsVel[16] = - actAxesVel[0]/3;
+    // l/r_hand_little_1
+    physJointsVel[17] = actAxesVel[8]/3;
+    // l/r_hand_little_2
+    physJointsVel[18] = actAxesVel[8]/3;
+    // l/r_hand_little_3
+    physJointsVel[19] = actAxesVel[8]/3;
+
+    return true;
+}
+
+bool CouplingICubHandMk2::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) {
+    size_t nrOfPhysicalJoints;
+    size_t nrOfActuatedAxes;
+    auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
+    ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
+    if (!ok || actAxesPos.size() != nrOfActuatedAxes || physJointsAcc.size() != nrOfPhysicalJoints || actAxesAcc.size() != nrOfActuatedAxes) {
+        yCError(COUPLINGICUBHANDMK2) << "convertFromPhysicalJointsToActuatedAxesAcc: input or output vectors have wrong size";
+        return false;
+    }
+
+    // l/r_hand_thumb_0
+    physJointsAcc[0] = actAxesAcc[1];
+    // l/r_hand_thumb_1
+    physJointsAcc[1] = actAxesAcc[2];
+    // l/r_hand_thumb_2
+    physJointsAcc[2] = actAxesAcc[3]/2;
+    // l/r_hand_thumb_3
+    physJointsAcc[3] = actAxesAcc[3]/2;
+    // l/r_hand_index_0
+    physJointsAcc[4] = actAxesAcc[0]/3;
+    // l/r_hand_index_1
+    physJointsAcc[5] = actAxesAcc[4];
+    // l/r_hand_index_2
+    physJointsAcc[6] = actAxesAcc[5]/2;
+    // l/r_hand_index_3
+    physJointsAcc[7] = actAxesAcc[5]/2;
+    // l/r_hand_middle_0
+    physJointsAcc[8] = 0.0;
+    // l/r_hand_middle_1
+    physJointsAcc[9] = actAxesAcc[6];
+    // l/r_hand_middle_2
+    physJointsAcc[10] = actAxesAcc[7]/2;
+    // l/r_hand_middle_3
+    physJointsAcc[11] = actAxesAcc[7]/2;
+    // l/r_hand_ring_0
+    physJointsAcc[12] = - actAxesAcc[0]/3;
+    // l/r_hand_ring_1
+    physJointsAcc[13] = actAxesAcc[8]/3;
+    // l/r_hand_ring_2
+    physJointsAcc[14] = actAxesAcc[8]/3;
+    // l/r_hand_ring_3
+    physJointsAcc[15] = actAxesAcc[8]/3;
+    // l/r_hand_little_0
+    physJointsAcc[16] = - actAxesAcc[0]/3;
+    // l/r_hand_little_1
+    physJointsAcc[17] = actAxesAcc[8]/3;
+    // l/r_hand_little_2
+    physJointsAcc[18] = actAxesAcc[8]/3;
+    // l/r_hand_little_3
+    physJointsAcc[19] = actAxesAcc[8]/3;
+
+    return false;
+}
+bool CouplingICubHandMk2::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) {
+    return false;
+}