Add couplingICubEye device (#997)

robotology · Dec 18, 2024 · ac3beae · ac3beae
1 parent fb7f977
commit ac3beae
Show file tree

Hide file tree

Showing 8 changed files with 731 additions and 0 deletions.
diff --git a/src/libraries/icubmod/CMakeLists.txt b/src/libraries/icubmod/CMakeLists.txt
@@ -55,6 +55,7 @@ yarp_begin_plugin_library(icubmod QUIET)
   add_subdirectory(rawValuesPublisherServer)
   add_subdirectory(fakeRawValuesPublisher)
   add_subdirectory(couplingICubHandMk2)
+  add_subdirectory(couplingICubEye)
 if (ICUB_ICUBINTERFACE_EXPERIMENTAL)
   add_subdirectory(imu3DM_GX3)
 endif()

diff --git a/src/libraries/icubmod/couplingICubEye/CMakeLists.txt b/src/libraries/icubmod/couplingICubEye/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(couplingICubEye
+                    CATEGORY device
+                    TYPE CouplingICubEye
+                    INCLUDE CouplingICubEye.h
+                    GENERATE_PARSER
+                    DEFAULT ON)
+
+if(ENABLE_couplingICubEye)
+    option(ALLOW_DEVICE_PARAM_PARSER_GENERATION "Generate the param parser for couplingICubEye device" OFF)
+    yarp_add_plugin(yarp_couplingICubEye)
+
+    if(MSVC)
+        add_definitions(-D_USE_MATH_DEFINES)
+    endif()
+
+    target_sources(yarp_couplingICubEye PRIVATE CouplingICubEye.cpp
+                                                CouplingICubEye.h
+                                                CouplingICubEye_ParamsParser.cpp
+                                                CouplingICubEye_ParamsParser.h)
+
+    target_link_libraries(yarp_couplingICubEye PRIVATE   YARP::YARP_os
+                                                            YARP::YARP_dev)
+    list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS YARP_os
+                                                        YARP_dev)
+
+    yarp_install(TARGETS yarp_couplingICubEye
+                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_couplingICubEye PROPERTY FOLDER "Plugins/Device")
+endif()
diff --git a/src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp
@@ -0,0 +1,174 @@
+/*
+ * 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 "CouplingICubEye.h"
+#include <yarp/os/LogStream.h>
+#include <cmath>
+#include <array>
+#include <numeric>
+
+
+YARP_LOG_COMPONENT(COUPLINGICUBEYE, "yarp.device.couplingICubEye")
+
+
+bool CouplingICubEye::populateCouplingParameters() {
+    yarp::sig::VectorOf<size_t> coupled_physical_joints{1, 2};
+    yarp::sig::VectorOf<size_t> coupled_actuated_axes{1, 2};
+    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 CouplingICubEye::open(yarp::os::Searchable& config) {
+
+    if(!parseParams(config)) {
+        yCError(COUPLINGICUBEYE) << "Error parsing parameters";
+        return false;
+    }
+
+    if(!populateCouplingParameters()) {
+        yCError(COUPLINGICUBEYE) << "Error populating coupling parameters";
+        return false;
+    }
+
+    yCDebug(COUPLINGICUBEYE) << "Opening couplingICubEye" << config.toString();
+    return true;
+}
+
+bool CouplingICubEye::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(COUPLINGICUBEYE) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
+        return false;
+    }
+
+    // eyes_tilt
+    actAxesPos[0] = physJointsPos[0];
+    // eyes_version
+    actAxesPos[1] = (physJointsPos[1] + physJointsPos[2])/2;
+    // eyes_vergence
+    actAxesPos[2] = (physJointsPos[1] - physJointsPos[2]);
+
+    return true;
+}
+
+bool CouplingICubEye::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(COUPLINGICUBEYE) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size";
+        return false;
+    }
+    // eyes_tilt
+    actAxesVel[0] = physJointsVel[0];
+    // eyes_version
+    actAxesVel[1] = (physJointsVel[1] + physJointsVel[2])/2;
+    // eyes_vergence
+    actAxesVel[2] = (physJointsVel[1] - physJointsVel[2]);
+
+    return true;
+}
+
+bool CouplingICubEye::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(COUPLINGICUBEYE) << "convertFromPhysicalJointsToActuatedAxesAcc: input or output vectors have wrong size";
+        return false;
+    }
+
+    // eyes_tilt
+    actAxesAcc[0] = physJointsAcc[0];
+    // eyes_version
+    actAxesAcc[1] = (physJointsAcc[1] + physJointsAcc[2])/2;
+    // eyes_vergence
+    actAxesAcc[2] = (physJointsAcc[1] - physJointsAcc[2]);
+
+    return true;
+}
+
+bool CouplingICubEye::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) {
+    return false;
+}
+
+bool CouplingICubEye::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(COUPLINGICUBEYE) << "convertFromActuatedAxesToPhysicalJointsPos: input or output vectors have wrong size";
+        return false;
+    }
+
+    // eyes_tilt
+    physJointsPos[0] = actAxesPos[0];
+    // l_eye_pan_joint
+    physJointsPos[1] = actAxesPos[1] + actAxesPos[2]/2;
+    // r_eye_pan_joint
+    physJointsPos[2] = actAxesPos[1] - actAxesPos[2]/2;
+
+    return true;
+}
+
+bool CouplingICubEye::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(COUPLINGICUBEYE) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size";
+        return false;
+    }
+
+    // eyes_tilt
+    physJointsVel[0] = actAxesVel[0];
+    // l_eye_pan_joint
+    physJointsVel[1] = actAxesVel[1] + actAxesVel[2]/2;
+    // r_eye_pan_joint
+    physJointsVel[2] = actAxesVel[1] - actAxesVel[2]/2;
+
+    return true;
+}
+
+bool CouplingICubEye::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(COUPLINGICUBEYE) << "convertFromPhysicalJointsToActuatedAxesAcc: input or output vectors have wrong size";
+        return false;
+    }
+
+    // eyes_tilt
+    physJointsAcc[0] = actAxesAcc[0];
+    // l_eye_pan_joint
+    physJointsAcc[1] = actAxesAcc[1] + actAxesAcc[2]/2;
+    // r_eye_pan_joint
+    physJointsAcc[2] = actAxesAcc[1] - actAxesAcc[2]/2;
+
+    return true;
+}
+
+bool CouplingICubEye::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) {
+    return false;
+}
diff --git a/src/libraries/icubmod/couplingICubEye/CouplingICubEye.h b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.h
@@ -0,0 +1,55 @@
+/*
+ * 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.
+ */
+
+#ifndef COUPLINGICUBEYE_H
+#define COUPLINGICUBEYE_H
+
+#include <yarp/os/LogComponent.h>
+#include <yarp/dev/DeviceDriver.h>
+#include <yarp/dev/ImplementJointCoupling.h>
+#include "CouplingICubEye_ParamsParser.h"
+
+#include <unordered_map>
+#include <vector>
+#include <string>
+
+
+YARP_DECLARE_LOG_COMPONENT(COUPLINGICUBEYE)
+
+/**
+ * Coupling law from https://icub-tech-iit.github.io/documentation/icub_kinematics/icub-vergence-version/icub-vergence-version/
+ */
+
+class CouplingICubEye :  public yarp::dev::DeviceDriver,
+                            public yarp::dev::ImplementJointCoupling,
+                            public CouplingICubEye_ParamsParser {
+public:
+    CouplingICubEye() = default;
+    virtual ~CouplingICubEye() 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:
+
+    bool populateCouplingParameters();
+};
+
+#endif // COUPLINGICUBEYE_H