diff --git a/src/devices/CMakeLists.txt b/src/devices/CMakeLists.txt index 56893070b7..96659a8dbf 100644 --- a/src/devices/CMakeLists.txt +++ b/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) diff --git a/src/devices/fakeJointCoupling/CMakeLists.txt b/src/devices/fakeJointCoupling/CMakeLists.txt new file mode 100644 index 0000000000..a96d380415 --- /dev/null +++ b/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() diff --git a/src/devices/fakeJointCoupling/fakeJointCoupling.cpp b/src/devices/fakeJointCoupling/fakeJointCoupling.cpp new file mode 100644 index 0000000000..6df6ea3505 --- /dev/null +++ b/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 + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +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 coupled_physical_joints {2,3}; + yarp::sig::VectorOf coupled_actuated_axes {2}; + std::vector physical_joint_names{"phys_joint_0", "phys_joint_1", "phys_joint_2", "phys_joint_3"}; + std::vector actuated_axes_names{"act_axes_0", "act_axes_1", "act_axes_2"}; + std::vector> 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 < +#include +#include +#include +#include +#include + +#include + + +/** + * @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 diff --git a/src/devices/fakeJointCoupling/tests/CMakeLists.txt b/src/devices/fakeJointCoupling/tests/CMakeLists.txt new file mode 100644 index 0000000000..7f9bae63ee --- /dev/null +++ b/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) diff --git a/src/devices/fakeJointCoupling/tests/fakeJointCoupling_test.cpp b/src/devices/fakeJointCoupling/tests/fakeJointCoupling_test.cpp new file mode 100644 index 0000000000..a4ea4802d5 --- /dev/null +++ b/src/devices/fakeJointCoupling/tests/fakeJointCoupling_test.cpp @@ -0,0 +1,143 @@ +/* + * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include + +#include +#include + +using namespace yarp::dev; +using namespace yarp::os; + +TEST_CASE("dev::fakeJointCoupling", "[yarp::dev]") +{ + YARP_REQUIRE_PLUGIN("fakeJointCoupling", "device"); + + Network::setLocalMode(true); + + SECTION("Checking fakeJointCoupling device") + { + PolyDriver ddjc; + IJointCoupling* ijc=nullptr; + { + Property p_cfg; + p_cfg.put("device", "fakeJointCoupling"); + REQUIRE(ddjc.open(p_cfg)); + } + + REQUIRE(ddjc.view(ijc)); + REQUIRE(ijc!=nullptr); + yarp::sig::VectorOf coupled_physical_joints; + coupled_physical_joints.clear(); + CHECK(ijc->getCoupledPhysicalJoints(coupled_physical_joints)); + CHECK(coupled_physical_joints.size() == 2); + CHECK(coupled_physical_joints[0] == 2); + CHECK(coupled_physical_joints[1] == 3); + + yarp::sig::VectorOf coupled_actuated_axes; + coupled_actuated_axes.clear(); + CHECK(ijc->getCoupledActuatedAxes(coupled_actuated_axes)); + CHECK(coupled_actuated_axes.size() == 1); + CHECK(coupled_actuated_axes[0] == 2); + + size_t nr_of_physical_joints{0}; + CHECK(ijc->getNrOfPhysicalJoints(nr_of_physical_joints)); + CHECK(nr_of_physical_joints == 4); + + size_t nr_of_actuated_axes{0}; + CHECK(ijc->getNrOfActuatedAxes(nr_of_actuated_axes)); + CHECK(nr_of_actuated_axes == 3); + + std::string physical_joint_name; + CHECK(ijc->getPhysicalJointName(0, physical_joint_name)); + CHECK(physical_joint_name == "phys_joint_0"); + CHECK(ijc->getPhysicalJointName(1, physical_joint_name)); + CHECK(physical_joint_name == "phys_joint_1"); + CHECK(ijc->getPhysicalJointName(2, physical_joint_name)); + CHECK(physical_joint_name == "phys_joint_2"); + CHECK(ijc->getPhysicalJointName(3, physical_joint_name)); + CHECK(physical_joint_name == "phys_joint_3"); + CHECK_FALSE(ijc->getPhysicalJointName(4, physical_joint_name)); + + std::string actuated_axis_name; + CHECK(ijc->getActuatedAxisName(0, actuated_axis_name)); + CHECK(actuated_axis_name == "act_axes_0"); + CHECK(ijc->getActuatedAxisName(1, actuated_axis_name)); + CHECK(actuated_axis_name == "act_axes_1"); + CHECK(ijc->getActuatedAxisName(2, actuated_axis_name)); + CHECK(actuated_axis_name == "act_axes_2"); + CHECK_FALSE(ijc->getActuatedAxisName(3, actuated_axis_name)); + + yarp::sig::Vector physJointsPos{0.0, 1.0, 2.0, 3.0}; + yarp::sig::Vector actAxesPos{0.0, 0.0, 0.0}; + yarp::sig::Vector physJointsVel{0.0, 1.0, 2.0, 3.0}; + yarp::sig::Vector actAxesVel{0.0, 0.0, 0.0}; + yarp::sig::Vector physJointsAcc{0.0, 1.0, 2.0, 3.0}; + yarp::sig::Vector actAxesAcc{0.0, 0.0, 0.0};; + + CHECK(ijc->convertFromPhysicalJointsToActuatedAxesPos(physJointsPos, actAxesPos)); + CHECK(actAxesPos[0] == 0.0); + CHECK(actAxesPos[1] == 1.0); + CHECK(actAxesPos[2] == 5.0); + + CHECK(ijc->convertFromPhysicalJointsToActuatedAxesVel(physJointsPos, physJointsVel, actAxesVel)); + CHECK(actAxesVel[0] == 0.0); + CHECK(actAxesVel[1] == 1.0); + CHECK(actAxesVel[2] == 10.0); + + CHECK(ijc->convertFromPhysicalJointsToActuatedAxesAcc(physJointsPos, physJointsVel, physJointsAcc, actAxesAcc)); + CHECK(actAxesAcc[0] == 0.0); + CHECK(actAxesAcc[1] == 1.0); + CHECK(actAxesAcc[2] == 15.0); + + CHECK_FALSE(ijc->convertFromPhysicalJointsToActuatedAxesTrq(physJointsPos, physJointsAcc, actAxesAcc)); + + CHECK(ijc->convertFromActuatedAxesToPhysicalJointsPos(actAxesPos, physJointsPos)); + CHECK(physJointsPos[0] == 0.0); + CHECK(physJointsPos[1] == 1.0); + CHECK(physJointsPos[2] == 2.5); + CHECK(physJointsPos[3] == 2.5); + + CHECK(ijc->convertFromActuatedAxesToPhysicalJointsVel(actAxesPos, actAxesVel, physJointsVel)); + CHECK(physJointsVel[0] == 0.0); + CHECK(physJointsVel[1] == 1.0); + CHECK(physJointsVel[2] == -2.5); + CHECK(physJointsVel[3] == 7.5); + + CHECK(ijc->convertFromActuatedAxesToPhysicalJointsAcc(actAxesPos, actAxesVel, actAxesAcc, physJointsAcc)); + CHECK(physJointsAcc[0] == 0.0); + CHECK(physJointsAcc[1] == 1.0); + CHECK(physJointsAcc[2] == -10.0); + CHECK(physJointsAcc[3] == 15.0); + + CHECK_FALSE(ijc->convertFromActuatedAxesToPhysicalJointsTrq(actAxesPos, actAxesAcc, physJointsAcc)); + + double min{0.0}, max{0.0}; + CHECK(ijc->getPhysicalJointLimits(0, min, max)); + CHECK(min == -30.0); + CHECK(max == 30.0); + CHECK(ijc->getPhysicalJointLimits(1, min, max)); + CHECK(min == -10.0); + CHECK(max == 10.0); + CHECK(ijc->getPhysicalJointLimits(2, min, max)); + CHECK(min == -32.0); + CHECK(max == 33.0); + CHECK(ijc->getPhysicalJointLimits(3, min, max)); + CHECK(min == 0.0); + CHECK(max == 120.0); + + + + + //"Close all polydrivers and check" + { + CHECK(ddjc.close()); + } + } + + Network::setLocalMode(false); +} diff --git a/src/libYARP_dev/src/CMakeLists.txt b/src/libYARP_dev/src/CMakeLists.txt index 62d6d429f0..8704a8480c 100644 --- a/src/libYARP_dev/src/CMakeLists.txt +++ b/src/libYARP_dev/src/CMakeLists.txt @@ -58,6 +58,7 @@ set(YARP_dev_HDRS yarp/dev/IHapticDevice.h yarp/dev/IImpedanceControl.h yarp/dev/IInteractionMode.h + yarp/dev/IJointCoupling.h yarp/dev/IJoypadController.h yarp/dev/IJointFault.h yarp/dev/ILLM.h @@ -94,6 +95,7 @@ set(YARP_dev_HDRS yarp/dev/ImplementEncodersTimed.h yarp/dev/ImplementImpedanceControl.h yarp/dev/ImplementInteractionMode.h + yarp/dev/ImplementJointCoupling.h yarp/dev/ImplementJointFault.h yarp/dev/ImplementMotor.h yarp/dev/ImplementMotorEncoders.h @@ -203,6 +205,7 @@ set(YARP_dev_SRCS yarp/dev/ImplementEncodersTimed.cpp yarp/dev/ImplementImpedanceControl.cpp yarp/dev/ImplementInteractionMode.cpp + yarp/dev/ImplementJointCoupling.cpp yarp/dev/ImplementJointFault.cpp yarp/dev/ImplementMotor.cpp yarp/dev/ImplementMotorEncoders.cpp diff --git a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h new file mode 100644 index 0000000000..d11a93a0d4 --- /dev/null +++ b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h @@ -0,0 +1,241 @@ +/* + * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef YARP_DEV_IJOINTCOUPLING_H +#define YARP_DEV_IJOINTCOUPLING_H + +#include +#include +#include + +namespace yarp::dev { +class IJointCoupling; +} +/** + * @ingroup dev_iface_motor + * + * Interface for joint coupling. It contains the methods to convert from Physical Joints to Actuated Axes and viceversa. + * + * $$ + * \theta \in \mathbb{R}^{m} : \text{Actuated Axes positions} + * $$ + * + * $$ + * q \in \mathbb{R}^{n} : \text{Physical Joints positions} + * $$ + * + * In general, we have $m \leq n$. + * + * There is unique value of Actuated Axes corresponding to a given Physical Joints position, while in general there may be multiple Physical Joints position corresponding to a given Actuated Axes. + * + * So, the mapping from Physical Joints $q$ to Actuated Axes $\theta$ is defined as: + * + * $$ + * \theta = f(q) + * $$ + * + * while the mapping from Actuated Axes $\theta$ to Physical Joints $q$ is defined as: + * + * $$ + * q = g(\theta) + * $$ + * + * We have the following property: + * + * $$ + * f(g(\theta)) = \theta + * $$ + * + * while in general it is not always true that the inverse holds, i.e. + * + * $$ + * g(f(q)) \neq q + * $$ + * + * In the rest of the section, we assume that $\theta(t) = f(q(t))$ and $\dot{\theta}(t)^T \tau_{\theta}(t) = \dot{q}(t)^T \tau_{q}(t)$ + */ +class YARP_dev_API yarp::dev::IJointCoupling +{ +public: + /** + * Destructor. + */ + virtual ~IJointCoupling() {} + + /** + * @brief Convert from Physical Joints to Actuated Axes position. + * This method implements $f(q)$. + * + * @param[in] physJointsPos Physical Joints position + * @param[out] actAxesPos Actuated Axes position + * @return, true/false on success/failure + */ + virtual bool convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) = 0; + + /** + * @brief Convert from Physical Joints to Actuated Axes velocity. + * This method implements $f'$ that can be used to compute $\dot{\theta}(t)$, defined such that: + * + * $$ + * \dot{\theta}(t) = \frac{\partial}{\partial q} f(q(t)) \dot{q}(t) = f'( q(t), \dot{q}(t)) + * $$ + * + * @param[in] physJointsPos Physical Joints position + * @param[in] physJointsVel Physical Joints velocity + * @param[out] actAxesVel Actuated Axes velocity + * @return, true/false on success/failure + */ + virtual bool convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) = 0; + + /** + * @brief Convert from Physical Joints to Actuated Axes acceleration. + * This method implements $f''$ that can be used to compute $\ddot{\theta}(t)$, defined such that: + * + * $$ + * \ddot{\theta}(t) = \frac{\partial}{\partial q} f'(q(t), \dot{q}(t)) \dot{q}(t) + \frac{\partial}{\partial \dot{q}} f'(q(t), \dot{q}(t)) \ddot{q}(t) = f''( q(t), \dot{q}(t), \ddot{q}(t)) + * $$ + * + * @param[in] physJointsPos Physical Joints position + * @param[in] physJointsVel Physical Joints velocity + * @param[in] physJointsAcc Physical Joints acceleration + * @param[out] actAxesAcc Actuated Axes acceleration + * @return, true/false on success/failure + */ + virtual bool convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) = 0; + + /** + * @brief Convert from Physical Joints to Actuated Axes torque + * + * @param[in] physJointsPos Physical Joints position + * @param[in] physJointsTrq Physical Joints torque + * @param[out] actAxesTrq Actuated Axes torque + * @return true/false on success/failure + */ + virtual bool convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) = 0; + + /** + * @brief Convert from Actuated Axes to Physical Joints position + * This method implements $g(\theta)$. + * @param[in] actAxesPos Actuated Axes position + * @param[out] physJointsPos Physical Joints position + * @return true/false on success/failure + */ + virtual bool convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) = 0; + + /** + * @brief Convert from Actuated Axes to Physical Joints velocity + * This method implements $g'$ that can be used to compute $\dot{q}(t)$, defined such that: + * + * $$ + * \dot{q}(t) = \frac{\partial}{\partial \theta} g(\theta(t)) \dot{\theta}(t) = g'( \theta(t), \dot{\theta}(t)) + * $$ + * + * @param[in] actAxesPos Actuated Axes position + * @param[in] actAxesVel Actuated Axes velocity + * @param[out] physJointsVel Physical Joints velocity + * @return true/false on success/failure + */ + virtual bool convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) = 0; + + /** + * @brief Convert from Actuated Axes to Physical Joints acceleration + * + * This method implements $f''$ that can be used to compute $\ddot{\theta}(t)$, defined such that: + * + * $$ + * \ddot{q}(t) = \frac{\partial}{\partial \theta} g'(\theta(t), \dot{\theta}(t)) \dot{\theta}(t) + \frac{\partial}{\partial \dot{\theta}} g'(\theta(t), \dot{\theta}(t)) \ddot{\theta}(t) = g''( \theta(t), \dot{\theta}(t), \ddot{\theta}(t)) + * $$ + * + * @param[in] actAxesPos Actuated Axes position + * @param[in] actAxesVel Actuated Axes velocity + * @param[in] actAxesAcc Actuated Axes acceleration + * @param[out] physJointsAcc Physical Joints acceleration + * @return true/false on success/failure + */ + virtual bool convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) = 0; + + /** + * @brief Convert from Actuated Axes to Physical Joints acceleration + * + * @param[in] actAxesPos Actuated Axes position + * @param[in] actAxesTrq Actuated Axes torque + * @param[out] physJointsTrq Physical Joints torque + * @return true/false on success/failure + */ + virtual bool convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) = 0; + + // In some case, for a given coupling several "physical joints" ad "actuated axis" + // may be related in a obvious way, i.e. the position and torque of given physical + // joint could be equal to the position and torque of given actuated axis. + // In that case "physical joints" ad "actuated axis" are typically identified by the + // same name. The getCoupled***() methods return the indices of the "actuated axis" + // and "physical joints" that are coupled in a non-obvious way + + /** + * @brief Get the number of physical joints + * + * @param[out] nrOfPhysicalJoints The number of physical joints + * @return true/false on success/failure + */ + virtual bool getNrOfPhysicalJoints(size_t& nrOfPhysicalJoints) = 0; + + /** + * @brief Get the number of actuated axes + * + * @param nrOfActuatedAxes The number of actuated axes + * @return true/false on success/failure + */ + virtual bool getNrOfActuatedAxes(size_t& nrOfActuatedAxes) = 0; + + /** + * @brief Return the vector of "physical joints indices" (i.e. numbers from 0 to n-1) + * that are related to actuated axis in a non-obvious way + * @param[out] coupPhysJointsIndexes the vector of "physical joints indices" + * @return true/false on success/failure + */ + virtual bool getCoupledPhysicalJoints(yarp::sig::VectorOf& coupPhysJointsIndexes)=0; + + /** + * @brief Return the vector of "actuator axis indices" (i.e. numbers from 0 to m-1) + * that are related to physical joints in a non-obvious way + * + * @param[out] coupActAxesIndexes the vector of "actuator axis indices" + * @return true/false on success/failure + */ + virtual bool getCoupledActuatedAxes(yarp::sig::VectorOf& coupActAxesIndexes)=0; + + /** + * @brief Get the name of an actuated axis + * + * @param[in] actuatedAxisIndex the number from 0 to m-1 that identifies + * the location of a "actuated axis" in a actuated axis vector. + * @param[out] actuatedAxisName the actuated axis name + * @return true/false on success/failure + */ + virtual bool getActuatedAxisName(size_t actuatedAxisIndex, std::string& actuatedAxisName)=0; + + /** + * @brief Get the name of a physical joint + * + * @param[in] physicalJointIndex the number from 0 to n-1 that identifies + * the location of a "physical joint" in a physical joint vector + * @param[out] physicalJointName the physical joint name + * @return true/false on success/failure + */ + virtual bool getPhysicalJointName(size_t physicalJointIndex, std::string& physicalJointName)=0; + + /** + * @brief Get the Physical Joint Limit object + * + * @param[in] physicalJointIndex the number from 0 to n-1 that identifies + * the location of a "physical joint" in a physical joint vector + * @param[out] min minimum value + * @param[out] max maximum value + * @return true/false on success/failure + */ + virtual bool getPhysicalJointLimits(size_t physicalJointIndex, double& min, double& max)=0; +}; + +#endif // YARP_DEV_IJOINTCOUPLING_H diff --git a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp new file mode 100644 index 0000000000..0dd469f1d8 --- /dev/null +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp @@ -0,0 +1,81 @@ +/* + * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + + +#include "ImplementJointCoupling.h" +#include + + +using namespace yarp::dev; + + +void ImplementJointCoupling::initialise(yarp::sig::VectorOf coupled_physical_joints, + yarp::sig::VectorOf coupled_actuated_axes, + std::vector physical_joint_names, + std::vector actuated_axes_names, + std::vector> physical_joint_limits) +{ + m_coupledPhysicalJoints = coupled_physical_joints; + m_coupledActuatedAxes = coupled_actuated_axes; + m_physicalJointNames = physical_joint_names; + m_actuatedAxesNames = actuated_axes_names; + m_physicalJointLimits = physical_joint_limits; +} + +bool ImplementJointCoupling::getNrOfPhysicalJoints(size_t& nrOfPhysicalJoints) { + nrOfPhysicalJoints = m_physicalJointNames.size(); + return true; +} +bool ImplementJointCoupling::getNrOfActuatedAxes(size_t& nrOfActuatedAxes){ + nrOfActuatedAxes = m_actuatedAxesNames.size(); + return true; +} + +bool ImplementJointCoupling::getCoupledPhysicalJoints(yarp::sig::VectorOf& coupPhysJointsIndexes) { + coupPhysJointsIndexes = m_coupledPhysicalJoints; + return true; +} + +bool ImplementJointCoupling::getCoupledActuatedAxes(yarp::sig::VectorOf& coupActAxesIndexes) { + coupActAxesIndexes = m_coupledActuatedAxes; + return true; +} + +bool ImplementJointCoupling::getPhysicalJointName(size_t physicalJointIndex, std::string& physicalJointName){ + if(physicalJointIndex >= m_physicalJointNames.size()) { + return false; + } + else { + physicalJointName=m_physicalJointNames[physicalJointIndex]; + return true; + } + +} + +bool ImplementJointCoupling::getActuatedAxisName(size_t actuatedAxisIndex, std::string& actuatedAxisName){ + if(actuatedAxisIndex >= m_actuatedAxesNames.size()) { + return false; + } + else { + actuatedAxisName=m_actuatedAxesNames[actuatedAxisIndex]; + return true; + } +} + +bool ImplementJointCoupling::checkPhysicalJointIsCoupled(size_t physicalJointIndex){ + return std::find(m_coupledPhysicalJoints.begin(), m_coupledPhysicalJoints.end(), physicalJointIndex) != m_coupledPhysicalJoints.end(); +} + +bool ImplementJointCoupling::getPhysicalJointLimits(size_t physicalJointIndex, double& min, double& max){ + size_t nrOfPhysicalJoints; + auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); + if (ok && physicalJointIndex < nrOfPhysicalJoints) + { + min = m_physicalJointLimits[physicalJointIndex].first; + max = m_physicalJointLimits[physicalJointIndex].second; + return true; + } + return false; +} diff --git a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h new file mode 100644 index 0000000000..45279a1854 --- /dev/null +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h @@ -0,0 +1,56 @@ +/* + * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef YARP_DEV_IJOINTCOUPLINGIMPL_H +#define YARP_DEV_IJOINTCOUPLINGIMPL_H + +#include +#include + +#include +#include + +namespace yarp::dev { +class ImplementJointCoupling; +} + +class YARP_dev_API yarp::dev::ImplementJointCoupling: public IJointCoupling +{ +public: + /* Constructor. + */ + ImplementJointCoupling() = default; + + /** + * Destructor. Perform uninitialize if needed. + */ + virtual ~ImplementJointCoupling() = default; + + void initialise(yarp::sig::VectorOf coupled_physical_joints, + yarp::sig::VectorOf coupled_actuated_axes, + std::vector physical_joint_names, + std::vector actuated_axes_names, + std::vector> coupled_physical_joint_limits); + bool getNrOfPhysicalJoints(size_t& nrOfPhysicalJoints) override final; + bool getNrOfActuatedAxes(size_t& nrOfActuatedAxes) override final; + bool getCoupledPhysicalJoints(yarp::sig::VectorOf& coupPhysJointsIndexes) override final; + bool getCoupledActuatedAxes(yarp::sig::VectorOf& coupActAxesIndexes) override final; + bool getPhysicalJointName(size_t physicalJointIndex, std::string& physicalJointName) override final; + bool getActuatedAxisName(size_t actuatedAxisIndex, std::string& actuatedAxisName) override final; + bool getPhysicalJointLimits(size_t physicalJointIndex, double& min, double& max) override final; +protected: + bool checkPhysicalJointIsCoupled(size_t physicalJointIndex); + + yarp::sig::VectorOf m_coupledPhysicalJoints; + yarp::sig::VectorOf m_coupledActuatedAxes; + std::vector m_physicalJointNames; + std::vector m_actuatedAxesNames; + std::vector> m_physicalJointLimits; + unsigned int m_controllerPeriod; + unsigned int m_couplingSize; + +}; + +#endif // YARP_DEV_IJOINTCOUPLINGIMPL_H