Merge pull request #3026 from Nicogene/addIJointCoupling

IJointCoupling: an interface for handling coupled joints

src/devices/CMakeLists.txt

@@ -26,6 +26,7 @@ yarp_begin_plugin_library(yarpmod
   add_subdirectory(fakeAnalogSensor)
   add_subdirectory(fakeBattery)
   add_subdirectory(fakeIMU)
+  add_subdirectory(fakeJointCoupling)
   add_subdirectory(fakeJoypad)
   add_subdirectory(fakeLLMDevice)
   add_subdirectory(fakeOdometry2D)

src/devices/fakeJointCoupling/CMakeLists.txt

@@ -0,0 +1,52 @@
+# SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT)
+# SPDX-License-Identifier: BSD-3-Clause
+
+if (YARP_COMPILE_ALL_FAKE_DEVICES)
+  set(ENABLE_yarpmod_fakeJointCoupling ON CACHE BOOL "" FORCE)
+endif()
+
+yarp_prepare_plugin(fakeJointCoupling
+  CATEGORY device
+  TYPE FakeJointCoupling
+  INCLUDE fakeJointCoupling.h
+)
+
+if(NOT SKIP_fakeJointCoupling)
+  yarp_add_plugin(yarp_fakeJointCoupling)
+
+  target_sources(yarp_fakeJointCoupling
+    PRIVATE
+      fakeJointCoupling.cpp
+      fakeJointCoupling.h)
+
+  target_link_libraries(yarp_fakeJointCoupling
+    PRIVATE
+      YARP::YARP_os
+      YARP::YARP_sig
+      YARP::YARP_dev
+  )
+  list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS
+    YARP_os
+    YARP_sig
+    YARP_dev
+    YARP_math
+  )
+
+  yarp_install(
+    TARGETS yarp_fakeJointCoupling
+    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_fakeJointCoupling PROPERTY FOLDER "Plugins/Device/Fake")
+
+  if(YARP_COMPILE_TESTS)
+    add_subdirectory(tests)
+  endif()
+
+endif()

src/devices/fakeJointCoupling/fakeJointCoupling.cpp

@@ -0,0 +1,140 @@
+/*
+ * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "fakeJointCoupling.h"
+
+#include <yarp/conf/environment.h>
+
+#include <yarp/os/Bottle.h>
+#include <yarp/os/Time.h>
+#include <yarp/os/LogComponent.h>
+#include <yarp/os/LogStream.h>
+#include <yarp/os/NetType.h>
+#include <yarp/dev/Drivers.h>
+
+#include <sstream>
+#include <cstring>
+#include <algorithm>
+
+using namespace yarp::dev;
+using namespace yarp::os;
+using namespace yarp::os::impl;
+
+
+namespace {
+YARP_LOG_COMPONENT(FAKEJOINTCOUPLING, "yarp.device.fakeJointCoupling")
+}
+
+bool FakeJointCoupling::open(yarp::os::Searchable &par) {
+    yarp::sig::VectorOf<size_t> coupled_physical_joints {2,3};
+    yarp::sig::VectorOf<size_t> coupled_actuated_axes {2};
+    std::vector<std::string> physical_joint_names{"phys_joint_0", "phys_joint_1", "phys_joint_2", "phys_joint_3"};
+    std::vector<std::string> actuated_axes_names{"act_axes_0", "act_axes_1", "act_axes_2"};
+    std::vector<std::pair<double, double>> physical_joint_limits{{-30.0, 30.0}, {-10.0, 10.0}, {-32.0, 33.0}, {0.0, 120.0}};
+    initialise(coupled_physical_joints, coupled_actuated_axes, physical_joint_names, actuated_axes_names, physical_joint_limits);
+    return true;
+}
+bool FakeJointCoupling::close() {
+    return true;
+}
+
+bool FakeJointCoupling::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) {
+        // yCDebug(FAKEJOINTCOUPLING) << ok <<physJointsPos.size()<<nrOfPhysicalJoints<<actAxesPos.size()<<nrOfActuatedAxes;
+        yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
+        return false;
+    }
+    actAxesPos[0] = physJointsPos[0];
+    actAxesPos[1] = physJointsPos[1];
+    actAxesPos[2] = physJointsPos[2] + physJointsPos[3];
+    return true;
+}
+bool FakeJointCoupling::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 || physJointsPos.size() != nrOfPhysicalJoints || physJointsVel.size() != nrOfPhysicalJoints || actAxesVel.size() != nrOfActuatedAxes) {
+        yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
+        return false;
+    }
+    actAxesVel[0] = physJointsVel[0];
+    actAxesVel[1] = physJointsVel[1];
+    actAxesVel[2] = physJointsPos[2] + physJointsPos[3] + physJointsVel[2] + physJointsVel[3];
+    return true;
+}
+bool FakeJointCoupling::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 || physJointsPos.size() != nrOfPhysicalJoints || physJointsVel.size() != nrOfPhysicalJoints || physJointsAcc.size() != nrOfPhysicalJoints || actAxesAcc.size() != nrOfActuatedAxes) {
+        yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
+        return false;
+    }
+    actAxesAcc[0] = physJointsAcc[0];
+    actAxesAcc[1] = physJointsAcc[1];
+    actAxesAcc[2] = physJointsPos[2] + physJointsPos[3] + physJointsVel[2] + physJointsVel[3] + physJointsAcc[2] + physJointsAcc[3];
+    return true;
+}
+bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) {
+    yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesTrq: not implemented yet";
+    return false;
+}
+bool FakeJointCoupling::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 || actAxesPos.size() != nrOfActuatedAxes || physJointsPos.size() != nrOfPhysicalJoints) {
+        yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsPos: input or output vectors have wrong size";
+        return false;
+    }
+    physJointsPos[0] = actAxesPos[0];
+    physJointsPos[1] = actAxesPos[1];
+    physJointsPos[2] = actAxesPos[2] / 2.0;
+    physJointsPos[3] = actAxesPos[2] / 2.0;
+    return true;
+}
+bool FakeJointCoupling::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 || actAxesVel.size() != nrOfActuatedAxes || physJointsVel.size() != nrOfPhysicalJoints) {
+        yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsVel: input or output vectors have wrong size";
+        return false;
+    }
+    physJointsVel[0] = actAxesVel[0];
+    physJointsVel[1] = actAxesVel[1];
+    physJointsVel[2] = actAxesPos[2] / 2.0 - actAxesVel[2] / 2.0;
+    physJointsVel[3] = actAxesPos[2] / 2.0 + actAxesVel[2] / 2.0;
+    return true;
+
+}
+bool FakeJointCoupling::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 || actAxesVel.size() != nrOfActuatedAxes || actAxesAcc.size() != nrOfActuatedAxes || physJointsAcc.size() != nrOfPhysicalJoints) {
+        yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsAcc: input or output vectors have wrong size";
+        return false;
+    }
+    physJointsAcc[0] = actAxesAcc[0];
+    physJointsAcc[1] = actAxesAcc[1];
+    physJointsAcc[2] = actAxesPos[2] / 2.0 - actAxesVel[2] / 2.0 - actAxesAcc[2] / 2.0;
+    physJointsAcc[3] = actAxesPos[2] / 2.0 + actAxesVel[2] / 2.0 + actAxesAcc[2] / 2.0;
+    return true;
+}
+bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) {
+    yCWarning(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsTrq: not implemented yet";
+    return false;
+}

src/devices/fakeJointCoupling/fakeJointCoupling.h

@@ -0,0 +1,54 @@
+/*
+ * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef YARP_DEVICE_FAKE_JOINTCOUPLING
+#define YARP_DEVICE_FAKE_JOINTCOUPLING
+
+#include <yarp/os/Time.h>
+#include <yarp/os/Bottle.h>
+#include <yarp/sig/Vector.h>
+#include <yarp/dev/DeviceDriver.h>
+#include <yarp/dev/IJointCoupling.h>
+#include <yarp/dev/ImplementJointCoupling.h>
+
+#include <mutex>
+
+
+/**
+ * @ingroup dev_impl_fake dev_impl_motor
+ *
+ * \brief `fakeJointCoupling`: Documentation to be added
+ *
+ * The aim of this device is to mimic the expected behavior of a
+ * joint coupling device to help testing the high level software.
+ *
+ * WIP - it is very basic now, not all interfaces are implemented yet.
+ */
+class FakeJointCoupling :
+        public yarp::dev::DeviceDriver,
+        public yarp::dev::ImplementJointCoupling
+{
+private:
+
+public:
+
+    FakeJointCoupling() = default;
+    virtual ~FakeJointCoupling() = default;
+    // Device Driver
+    bool open(yarp::os::Searchable &par) override;
+    bool close() override;
+    // IJointCoupling
+    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;
+
+};
+
+#endif  // YARP_DEVICE_FAKE_JOINTCOUPLING

src/devices/fakeJointCoupling/tests/CMakeLists.txt

@@ -0,0 +1,4 @@
+# SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
+# SPDX-License-Identifier: BSD-3-Clause
+
+create_device_test(fakeJointCoupling)