From 0ee56ff494800c1476d1ccbcdda8779a5eecfc07 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 21 Sep 2023 16:06:00 +0200 Subject: [PATCH 01/12] First draft of IJointCoupling --- src/libYARP_dev/src/CMakeLists.txt | 1 + src/libYARP_dev/src/yarp/dev/IJointCoupling.h | 47 +++++++++++++++++++ 2 files changed, 48 insertions(+) create mode 100644 src/libYARP_dev/src/yarp/dev/IJointCoupling.h diff --git a/src/libYARP_dev/src/CMakeLists.txt b/src/libYARP_dev/src/CMakeLists.txt index 62d6d429f0..4e89170dfb 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 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..61329179c6 --- /dev/null +++ b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h @@ -0,0 +1,47 @@ +/* + * 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 a generic control board device implementing position control. + */ +class YARP_dev_API yarp::dev::IJointCoupling +{ +public: + /** + * Destructor. + */ + virtual ~IJointCoupling() {} + + virtual bool decouplePos(yarp::sig::Vector& current_pos) = 0; + virtual bool decoupleVel(yarp::sig::Vector& current_vel) = 0; + virtual bool decoupleAcc(yarp::sig::Vector& current_acc) = 0; + virtual bool decoupleTrq(yarp::sig::Vector& current_trq) = 0; + virtual yarp::sig::Vector decoupleRefPos(yarp::sig::Vector& pos_ref) = 0; + virtual yarp::sig::Vector decoupleRefVel(yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) = 0; + virtual yarp::sig::Vector decoupleRefTrq(yarp::sig::Vector& trq_ref) = 0; + + + virtual yarp::sig::VectorOf getCoupledJoints()=0; + virtual std::string getCoupledJointName(int joint)=0; + virtual bool checkJointIsCoupled(int joint)=0; + + + virtual void setCoupledJointLimit(int joint, const double& min, const double& max)=0; + virtual void getCoupledJointLimit(int joint, double& min, double& max)=0; +}; + +#endif // YARP_DEV_IJOINTCOUPLING_H From fe5a7dae4839c1bc6b514c4582006f5033e4e79e Mon Sep 17 00:00:00 2001 From: Nicogene Date: Fri, 22 Sep 2023 16:04:54 +0200 Subject: [PATCH 02/12] Second draft of IJointCoupling --- src/libYARP_dev/src/CMakeLists.txt | 2 + src/libYARP_dev/src/yarp/dev/IJointCoupling.h | 19 ++--- .../src/yarp/dev/ImplementJointCoupling.cpp | 78 +++++++++++++++++++ .../src/yarp/dev/ImplementJointCoupling.h | 48 ++++++++++++ 4 files changed, 136 insertions(+), 11 deletions(-) create mode 100644 src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp create mode 100644 src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h diff --git a/src/libYARP_dev/src/CMakeLists.txt b/src/libYARP_dev/src/CMakeLists.txt index 4e89170dfb..8704a8480c 100644 --- a/src/libYARP_dev/src/CMakeLists.txt +++ b/src/libYARP_dev/src/CMakeLists.txt @@ -95,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 @@ -204,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 index 61329179c6..7073dec940 100644 --- a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h @@ -26,20 +26,17 @@ class YARP_dev_API yarp::dev::IJointCoupling */ virtual ~IJointCoupling() {} - virtual bool decouplePos(yarp::sig::Vector& current_pos) = 0; - virtual bool decoupleVel(yarp::sig::Vector& current_vel) = 0; - virtual bool decoupleAcc(yarp::sig::Vector& current_acc) = 0; - virtual bool decoupleTrq(yarp::sig::Vector& current_trq) = 0; - virtual yarp::sig::Vector decoupleRefPos(yarp::sig::Vector& pos_ref) = 0; - virtual yarp::sig::Vector decoupleRefVel(yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) = 0; - virtual yarp::sig::Vector decoupleRefTrq(yarp::sig::Vector& trq_ref) = 0; - - + virtual bool convertFromPhysicalJointPosToActuatedAxisPos(const yarp::sig::Vector& physJointPos, yarp::sig::Vector& actAxisPos) = 0; + virtual bool convertFromPhysicalJointVelToActuatedAxisVel(const yarp::sig::Vector& physJointPos, const yarp::sig::Vector& physJointVel, yarp::sig::Vector& actAxisVel) = 0; + virtual bool convertFromPhysicalJointPosToActuatedAxisAcc(const yarp::sig::Vector& physJointPos, const yarp::sig::Vector& physJointVel, const yarp::sig::Vector& physJointAcc, yarp::sig::Vector& actAxisAcc) = 0; + virtual bool convertFromPhysicalJointTrqToActuatedAxisTrq(const yarp::sig::Vector& physJointPos, const yarp::sig::Vector& physJointTrq, yarp::sig::Vector& actAxisTrq) = 0; + virtual bool convertFromActuatedAxisToPhysicalJointPos(const yarp::sig::Vector& actAxisPos, yarp::sig::Vector& physJointPos) = 0; + virtual bool convertFromActuatedAxisToPhysicalJointVel(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisVel, yarp::sig::Vector& physJointVel) = 0; + virtual bool convertFromActuatedAxisToPhysicalJointAcc(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisVel, const yarp::sig::Vector& actAxisAcc, yarp::sig::Vector& physJointAcc) = 0; + virtual bool convertFromActuatedAxisToPhysicalJointTrq(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisTrq, yarp::sig::Vector& physJointTrq) = 0; virtual yarp::sig::VectorOf getCoupledJoints()=0; virtual std::string getCoupledJointName(int joint)=0; virtual bool checkJointIsCoupled(int joint)=0; - - virtual void setCoupledJointLimit(int joint, const double& min, const double& max)=0; virtual void getCoupledJointLimit(int joint, double& min, double& max)=0; }; 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..33c93b3b12 --- /dev/null +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp @@ -0,0 +1,78 @@ +/* + * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + + +#include "ImplementJointCoupling.h" + + +using namespace yarp::dev; + + +void ImplementJointCoupling::initialise(yarp::sig::VectorOf coupled_joints, std::vector coupled_joint_names, std::vector> coupled_joint_limits) { + m_coupledJoints=coupled_joints; + m_coupledJointNames=coupled_joint_names; + + // Configure a map between coupled joints and limits + for (std::size_t i = 0, j = 0; i < coupled_joints.size(); i++) + { + const int coupled_joint_index = coupled_joints(i); + const std::string coupled_joint_name = getCoupledJointName(coupled_joint_index); + if (coupled_joint_name != "invalid" && coupled_joint_name != "reserved") + { + m_coupledJointLimits[coupled_joints[i]] = coupled_joint_limits[j]; + j++; + } + } + + +} + + +yarp::sig::VectorOf ImplementJointCoupling::getCoupledJoints() { + return m_coupledJoints; +} + +std::string ImplementJointCoupling::getCoupledJointName(int joint){ + int c_joint = -1; + for (size_t i = 0; i < m_coupledJoints.size(); ++i) + { + if (m_coupledJoints[i]==joint) c_joint = i; + } + + if (c_joint >= 0 && static_cast(c_joint) < m_coupledJoints.size()) + { + return m_coupledJointNames[c_joint]; + } + else + { + return std::string("invalid"); + } +} + +bool ImplementJointCoupling::checkJointIsCoupled(int joint){ + for (size_t i = 0; i < m_coupledJoints.size(); ++i) + { + if (m_coupledJoints[i]==joint) return true; + } + return false; +} +void ImplementJointCoupling::setCoupledJointLimit(int joint, const double& min, const double& max){ + const std::string coupled_joint_name = getCoupledJointName(joint); + + if (coupled_joint_name != "reserved" && coupled_joint_name != "invalid") + { + m_coupledJointLimits.at(joint).first = min; + m_coupledJointLimits.at(joint).second = max; + } +} +void ImplementJointCoupling::getCoupledJointLimit(int joint, double& min, double& max){ + const std::string coupled_joint_name = getCoupledJointName(joint); + + if (coupled_joint_name != "reserved" && coupled_joint_name != "gyp_invalid") + { + min = m_coupledJointLimits.at(joint).first; + max = m_coupledJointLimits.at(joint).second; + } +} \ No newline at end of file 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..7ad0514d85 --- /dev/null +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h @@ -0,0 +1,48 @@ +/* + * 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 +#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_joints, std::vector coupled_joint_names, std::vector> coupled_joint_limits); + + yarp::sig::VectorOf getCoupledJoints() override final; + std::string getCoupledJointName(int joint) override final; + bool checkJointIsCoupled(int joint) override final; + void setCoupledJointLimit(int joint, const double& min, const double& max) override final; + void getCoupledJointLimit(int joint, double& min, double& max) override final; +protected: + yarp::sig::VectorOf m_coupledJoints; + std::vector m_coupledJointNames; + std::unordered_map> m_coupledJointLimits; + unsigned int m_controllerPeriod; + unsigned int m_couplingSize; + +}; + +#endif // YARP_DEV_IJOINTCOUPLINGIMPL_H \ No newline at end of file From ad9b1edf13a4725e41a0b76127c2187b5cd74186 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Fri, 20 Oct 2023 16:17:29 +0200 Subject: [PATCH 03/12] Third draft of iJointCoupling --- src/libYARP_dev/src/yarp/dev/IJointCoupling.h | 181 +++++++++++++++++- .../src/yarp/dev/ImplementJointCoupling.cpp | 62 +++--- .../src/yarp/dev/ImplementJointCoupling.h | 18 +- 3 files changed, 217 insertions(+), 44 deletions(-) diff --git a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h index 7073dec940..f8673ea09b 100644 --- a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h @@ -16,7 +16,45 @@ class IJointCoupling; /** * @ingroup dev_iface_motor * - * Interface for a generic control board device implementing position control. + * Interface for joint coupling. It contains the methods to convert from Physical Joint 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 Joint position, while in general there may be multiple Physical Joint position corresponding to a given Actuated Axes. + * + * So, the mapping from Physical Joint $q$ to Actuated Axes $\theta$ is defined as: + * + * $$ + * \theta = f(q) + * $$ + * + * while the mapping from Actuated Axes $\theta$ to Physical Joint $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 { @@ -26,19 +64,150 @@ class YARP_dev_API yarp::dev::IJointCoupling */ virtual ~IJointCoupling() {} + /** + * @brief Convert from Physical Joint to Actuated Axes position. + * This method implements $f(q)$. + * + * @param[in] physJointPos Physical Joint position + * @param[out] actAxisPos Actuated Axes position + * @return, true/false on success/failure + */ virtual bool convertFromPhysicalJointPosToActuatedAxisPos(const yarp::sig::Vector& physJointPos, yarp::sig::Vector& actAxisPos) = 0; + + /** + * @brief Convert from Physical Joint 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] physJointPos Physical Joint position + * @param[in] physJointVel Physical Joint velocity + * @param[out] actAxisVel Actuated Axes velocity + * @return, true/false on success/failure + */ virtual bool convertFromPhysicalJointVelToActuatedAxisVel(const yarp::sig::Vector& physJointPos, const yarp::sig::Vector& physJointVel, yarp::sig::Vector& actAxisVel) = 0; + + /** + * @brief Convert from Physical Joint 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] physJointPos Physical Joint position + * @param[in] physJointVel Physical Joint velocity + * @param[in] physJointAcc Physical Joint acceleration + * @param[out] actAxisAcc Actuated Axes acceleration + * @return, true/false on success/failure + */ virtual bool convertFromPhysicalJointPosToActuatedAxisAcc(const yarp::sig::Vector& physJointPos, const yarp::sig::Vector& physJointVel, const yarp::sig::Vector& physJointAcc, yarp::sig::Vector& actAxisAcc) = 0; + + /** + * @brief Convert from Physical Joint to Actuated Axes torque + * + * @param[in] physJointPos Physical Joint position + * @param[in] physJointTrq Physical Joint torque + * @param[out] actAxisTrq Actuated Axes torque + * @return true/false on success/failure + */ virtual bool convertFromPhysicalJointTrqToActuatedAxisTrq(const yarp::sig::Vector& physJointPos, const yarp::sig::Vector& physJointTrq, yarp::sig::Vector& actAxisTrq) = 0; + + /** + * @brief Convert from Actuated Axes to Physical Joint position + * This method implements $g(\theta)$. + * @param[in] actAxisPos Actuated Axes position + * @param[out] physJointPos Physical Joint position + * @return true/false on success/failure + */ virtual bool convertFromActuatedAxisToPhysicalJointPos(const yarp::sig::Vector& actAxisPos, yarp::sig::Vector& physJointPos) = 0; + + /** + * @brief Convert from Actuated Axes to Physical Joint 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] actAxisPos Actuated Axes position + * @param[in] actAxisVel Actuated Axes velocity + * @param[out] physJointVel Physical Joint velocity + * @return true/false on success/failure + */ virtual bool convertFromActuatedAxisToPhysicalJointVel(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisVel, yarp::sig::Vector& physJointVel) = 0; + + /** + * @brief Convert from Actuated Axes to Physical Joint 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] actAxisPos Actuated Axes position + * @param[in] actAxisVel Actuated Axes velocity + * @param[in] actAxisAcc Actuated Axes acceleration + * @param[out] physJointAcc Physical Joint acceleration + * @return true/false on success/failure + */ virtual bool convertFromActuatedAxisToPhysicalJointAcc(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisVel, const yarp::sig::Vector& actAxisAcc, yarp::sig::Vector& physJointAcc) = 0; + + /** + * @brief Convert from Actuated Axes to Physical Joint acceleration + * + * @param[in] actAxisPos Actuated Axes position + * @param[in] actAxisTrq Actuated Axes torque + * @param[out] physJointTrq Physical Joint torque + * @return true/false on success/failure + */ virtual bool convertFromActuatedAxisToPhysicalJointTrq(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisTrq, yarp::sig::Vector& physJointTrq) = 0; - virtual yarp::sig::VectorOf getCoupledJoints()=0; - virtual std::string getCoupledJointName(int joint)=0; - virtual bool checkJointIsCoupled(int joint)=0; - virtual void setCoupledJointLimit(int joint, const double& min, const double& max)=0; - virtual void getCoupledJointLimit(int joint, double& min, double& max)=0; + + /** + * @brief Get the physical joints object + * + * @return yarp::sig::VectorOf + */ + virtual yarp::sig::VectorOf getPhysicalJoints()=0; + + /** + * @brief Get the name of a physical joint + * + * @param joint index of the joint + * @return name of the joint + */ + virtual std::string getPhysicalJointName(size_t joint)=0; + + /** + * @brief Check if a physical joint is coupled + * + * @param joint index of the joint + * @return true/false on coupled/not coupled + */ + virtual bool checkPhysicalJointIsCoupled(size_t joint)=0; + + /** + * @brief Set limts for a physical joint + * + * @param[in] joint index of the joint + * @param[in] min minimum value + * @param[in] max maximum value + * @return true/false on success/failure + */ + virtual bool setPhysicalJointLimits(size_t joint, const double& min, const double& max)=0; + + /** + * @brief Get the Physical Joint Limit object + * + * @param[in] joint index of the joint + * @param[out] min minimum value + * @param[out] max maximum value + * @return true/false on success/failure + */ + virtual bool getPhysicalJointLimits(size_t joint, 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 index 33c93b3b12..d3b6ec6089 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp @@ -10,18 +10,18 @@ using namespace yarp::dev; -void ImplementJointCoupling::initialise(yarp::sig::VectorOf coupled_joints, std::vector coupled_joint_names, std::vector> coupled_joint_limits) { - m_coupledJoints=coupled_joints; - m_coupledJointNames=coupled_joint_names; +void ImplementJointCoupling::initialise(yarp::sig::VectorOf physical_joints, std::vector physical_joint_names, std::vector> physical_joint_limits) { + m_physicalJoints=physical_joints; + m_physicalJointNames=physical_joint_names; - // Configure a map between coupled joints and limits - for (std::size_t i = 0, j = 0; i < coupled_joints.size(); i++) + // Configure a map between physical joints and limits + for (std::size_t i = 0, j = 0; i < physical_joints.size(); i++) { - const int coupled_joint_index = coupled_joints(i); - const std::string coupled_joint_name = getCoupledJointName(coupled_joint_index); - if (coupled_joint_name != "invalid" && coupled_joint_name != "reserved") + const int physical_joint_index = physical_joints(i); + const std::string physical_joint_name = getPhysicalJointName(physical_joint_index); + if (physical_joint_name != "invalid" && physical_joint_name != "reserved") { - m_coupledJointLimits[coupled_joints[i]] = coupled_joint_limits[j]; + m_physicalJointLimits[physical_joints[i]] = physical_joint_limits[j]; j++; } } @@ -30,20 +30,20 @@ void ImplementJointCoupling::initialise(yarp::sig::VectorOf coupled_joints, } -yarp::sig::VectorOf ImplementJointCoupling::getCoupledJoints() { - return m_coupledJoints; +yarp::sig::VectorOf ImplementJointCoupling::getPhysicalJoints() { + return m_physicalJoints; } -std::string ImplementJointCoupling::getCoupledJointName(int joint){ +std::string ImplementJointCoupling::getPhysicalJointName(size_t joint){ int c_joint = -1; - for (size_t i = 0; i < m_coupledJoints.size(); ++i) + for (size_t i = 0; i < m_physicalJoints.size(); ++i) { - if (m_coupledJoints[i]==joint) c_joint = i; + if (m_physicalJoints[i]==joint) c_joint = i; } - if (c_joint >= 0 && static_cast(c_joint) < m_coupledJoints.size()) + if (c_joint >= 0 && static_cast(c_joint) < m_physicalJoints.size()) { - return m_coupledJointNames[c_joint]; + return m_physicalJointNames[c_joint]; } else { @@ -51,28 +51,32 @@ std::string ImplementJointCoupling::getCoupledJointName(int joint){ } } -bool ImplementJointCoupling::checkJointIsCoupled(int joint){ - for (size_t i = 0; i < m_coupledJoints.size(); ++i) +bool ImplementJointCoupling::checkPhysicalJointIsCoupled(size_t joint){ + for (size_t i = 0; i < m_physicalJoints.size(); ++i) { - if (m_coupledJoints[i]==joint) return true; + if (m_physicalJoints[i]==joint) return true; } return false; } -void ImplementJointCoupling::setCoupledJointLimit(int joint, const double& min, const double& max){ - const std::string coupled_joint_name = getCoupledJointName(joint); +bool ImplementJointCoupling::setPhysicalJointLimits(size_t joint, const double& min, const double& max){ + const std::string physical_joint_name = getPhysicalJointName(joint); - if (coupled_joint_name != "reserved" && coupled_joint_name != "invalid") + if (physical_joint_name != "reserved" && physical_joint_name != "invalid") { - m_coupledJointLimits.at(joint).first = min; - m_coupledJointLimits.at(joint).second = max; + m_physicalJointLimits.at(joint).first = min; + m_physicalJointLimits.at(joint).second = max; + return true; } + return false; } -void ImplementJointCoupling::getCoupledJointLimit(int joint, double& min, double& max){ - const std::string coupled_joint_name = getCoupledJointName(joint); +bool ImplementJointCoupling::getPhysicalJointLimits(size_t joint, double& min, double& max){ + const std::string physical_joint_name = getPhysicalJointName(joint); - if (coupled_joint_name != "reserved" && coupled_joint_name != "gyp_invalid") + if (physical_joint_name != "reserved" && physical_joint_name != "gyp_invalid") { - min = m_coupledJointLimits.at(joint).first; - max = m_coupledJointLimits.at(joint).second; + min = m_physicalJointLimits.at(joint).first; + max = m_physicalJointLimits.at(joint).second; + return true; } + return false; } \ No newline at end of file diff --git a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h index 7ad0514d85..a97964a1c6 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h @@ -29,17 +29,17 @@ class YARP_dev_API yarp::dev::ImplementJointCoupling: public IJointCoupling */ virtual ~ImplementJointCoupling() = default; - void initialise(yarp::sig::VectorOf coupled_joints, std::vector coupled_joint_names, std::vector> coupled_joint_limits); + void initialise(yarp::sig::VectorOf physical_joints, std::vector physical_joint_names, std::vector> physical_joint_limits); - yarp::sig::VectorOf getCoupledJoints() override final; - std::string getCoupledJointName(int joint) override final; - bool checkJointIsCoupled(int joint) override final; - void setCoupledJointLimit(int joint, const double& min, const double& max) override final; - void getCoupledJointLimit(int joint, double& min, double& max) override final; + yarp::sig::VectorOf getPhysicalJoints() override final; + std::string getPhysicalJointName(size_t joint) override final; + bool checkPhysicalJointIsCoupled(size_t joint) override final; + bool setPhysicalJointLimits(size_t joint, const double& min, const double& max) override final; + bool getPhysicalJointLimits(size_t joint, double& min, double& max) override final; protected: - yarp::sig::VectorOf m_coupledJoints; - std::vector m_coupledJointNames; - std::unordered_map> m_coupledJointLimits; + yarp::sig::VectorOf m_physicalJoints; + std::vector m_physicalJointNames; + std::unordered_map> m_physicalJointLimits; unsigned int m_controllerPeriod; unsigned int m_couplingSize; From 0b9712e4c48343d686a8e938f7cf5fcc9928add9 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 23 Oct 2023 12:37:00 +0200 Subject: [PATCH 04/12] IJointCoupling: correct function names --- src/libYARP_dev/src/yarp/dev/IJointCoupling.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h index f8673ea09b..bb408aa3b4 100644 --- a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h @@ -103,7 +103,7 @@ class YARP_dev_API yarp::dev::IJointCoupling * @param[out] actAxisAcc Actuated Axes acceleration * @return, true/false on success/failure */ - virtual bool convertFromPhysicalJointPosToActuatedAxisAcc(const yarp::sig::Vector& physJointPos, const yarp::sig::Vector& physJointVel, const yarp::sig::Vector& physJointAcc, yarp::sig::Vector& actAxisAcc) = 0; + virtual bool convertFromPhysicalJointAccToActuatedAxisAcc(const yarp::sig::Vector& physJointPos, const yarp::sig::Vector& physJointVel, const yarp::sig::Vector& physJointAcc, yarp::sig::Vector& actAxisAcc) = 0; /** * @brief Convert from Physical Joint to Actuated Axes torque @@ -122,7 +122,7 @@ class YARP_dev_API yarp::dev::IJointCoupling * @param[out] physJointPos Physical Joint position * @return true/false on success/failure */ - virtual bool convertFromActuatedAxisToPhysicalJointPos(const yarp::sig::Vector& actAxisPos, yarp::sig::Vector& physJointPos) = 0; + virtual bool convertFromActuatedAxisPosToPhysicalJointPos(const yarp::sig::Vector& actAxisPos, yarp::sig::Vector& physJointPos) = 0; /** * @brief Convert from Actuated Axes to Physical Joint velocity @@ -137,7 +137,7 @@ class YARP_dev_API yarp::dev::IJointCoupling * @param[out] physJointVel Physical Joint velocity * @return true/false on success/failure */ - virtual bool convertFromActuatedAxisToPhysicalJointVel(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisVel, yarp::sig::Vector& physJointVel) = 0; + virtual bool convertFromActuatedAxisVelToPhysicalJointVel(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisVel, yarp::sig::Vector& physJointVel) = 0; /** * @brief Convert from Actuated Axes to Physical Joint acceleration @@ -154,7 +154,7 @@ class YARP_dev_API yarp::dev::IJointCoupling * @param[out] physJointAcc Physical Joint acceleration * @return true/false on success/failure */ - virtual bool convertFromActuatedAxisToPhysicalJointAcc(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisVel, const yarp::sig::Vector& actAxisAcc, yarp::sig::Vector& physJointAcc) = 0; + virtual bool convertFromActuatedAxisAccToPhysicalJointAcc(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisVel, const yarp::sig::Vector& actAxisAcc, yarp::sig::Vector& physJointAcc) = 0; /** * @brief Convert from Actuated Axes to Physical Joint acceleration @@ -164,7 +164,7 @@ class YARP_dev_API yarp::dev::IJointCoupling * @param[out] physJointTrq Physical Joint torque * @return true/false on success/failure */ - virtual bool convertFromActuatedAxisToPhysicalJointTrq(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisTrq, yarp::sig::Vector& physJointTrq) = 0; + virtual bool convertFromActuatedAxisTrqToPhysicalJointTrq(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisTrq, yarp::sig::Vector& physJointTrq) = 0; /** * @brief Get the physical joints object From 95daaaa74490bba17bf16c5b6fe1112c81167786 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Tue, 24 Oct 2023 15:32:05 +0200 Subject: [PATCH 05/12] IJointCoupling: fourth draft --- src/libYARP_dev/src/yarp/dev/IJointCoupling.h | 135 ++++++++++-------- .../src/yarp/dev/ImplementJointCoupling.cpp | 55 +++---- .../src/yarp/dev/ImplementJointCoupling.h | 23 ++- 3 files changed, 117 insertions(+), 96 deletions(-) diff --git a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h index bb408aa3b4..0aedd3217d 100644 --- a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h @@ -16,7 +16,7 @@ class IJointCoupling; /** * @ingroup dev_iface_motor * - * Interface for joint coupling. It contains the methods to convert from Physical Joint to Actuated Axes and viceversa. + * 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} @@ -28,15 +28,15 @@ class IJointCoupling; * * In general, we have $m \leq n$. * - * There is unique value of Actuated Axes corresponding to a given Physical Joint position, while in general there may be multiple Physical Joint position corresponding to a given Actuated Axes. + * 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 Joint $q$ to Actuated Axes $\theta$ is defined as: + * 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 Joint $q$ is defined as: + * while the mapping from Actuated Axes $\theta$ to Physical Joints $q$ is defined as: * * $$ * q = g(\theta) @@ -65,82 +65,82 @@ class YARP_dev_API yarp::dev::IJointCoupling virtual ~IJointCoupling() {} /** - * @brief Convert from Physical Joint to Actuated Axes position. + * @brief Convert from Physical Joints to Actuated Axes position. * This method implements $f(q)$. * - * @param[in] physJointPos Physical Joint position - * @param[out] actAxisPos Actuated Axes position + * @param[in] physJointsPos Physical Joints position + * @param[out] actAxesPos Actuated Axes position * @return, true/false on success/failure */ - virtual bool convertFromPhysicalJointPosToActuatedAxisPos(const yarp::sig::Vector& physJointPos, yarp::sig::Vector& actAxisPos) = 0; + virtual bool convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) = 0; /** - * @brief Convert from Physical Joint to Actuated Axes velocity. + * @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] physJointPos Physical Joint position - * @param[in] physJointVel Physical Joint velocity - * @param[out] actAxisVel Actuated Axes velocity + * @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 convertFromPhysicalJointVelToActuatedAxisVel(const yarp::sig::Vector& physJointPos, const yarp::sig::Vector& physJointVel, yarp::sig::Vector& actAxisVel) = 0; + virtual bool convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) = 0; /** - * @brief Convert from Physical Joint to Actuated Axes acceleration. + * @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] physJointPos Physical Joint position - * @param[in] physJointVel Physical Joint velocity - * @param[in] physJointAcc Physical Joint acceleration - * @param[out] actAxisAcc Actuated Axes acceleration + * @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 convertFromPhysicalJointAccToActuatedAxisAcc(const yarp::sig::Vector& physJointPos, const yarp::sig::Vector& physJointVel, const yarp::sig::Vector& physJointAcc, yarp::sig::Vector& actAxisAcc) = 0; + 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 Joint to Actuated Axes torque + * @brief Convert from Physical Joints to Actuated Axes torque * - * @param[in] physJointPos Physical Joint position - * @param[in] physJointTrq Physical Joint torque - * @param[out] actAxisTrq 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 convertFromPhysicalJointTrqToActuatedAxisTrq(const yarp::sig::Vector& physJointPos, const yarp::sig::Vector& physJointTrq, yarp::sig::Vector& actAxisTrq) = 0; + 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 Joint position + * @brief Convert from Actuated Axes to Physical Joints position * This method implements $g(\theta)$. - * @param[in] actAxisPos Actuated Axes position - * @param[out] physJointPos Physical Joint position + * @param[in] actAxesPos Actuated Axes position + * @param[out] physJointsPos Physical Joints position * @return true/false on success/failure */ - virtual bool convertFromActuatedAxisPosToPhysicalJointPos(const yarp::sig::Vector& actAxisPos, yarp::sig::Vector& physJointPos) = 0; + virtual bool convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) = 0; /** - * @brief Convert from Actuated Axes to Physical Joint velocity + * @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] actAxisPos Actuated Axes position - * @param[in] actAxisVel Actuated Axes velocity - * @param[out] physJointVel Physical Joint velocity + * @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 convertFromActuatedAxisVelToPhysicalJointVel(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisVel, yarp::sig::Vector& physJointVel) = 0; + 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 Joint acceleration + * @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: * @@ -148,66 +148,75 @@ class YARP_dev_API yarp::dev::IJointCoupling * \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] actAxisPos Actuated Axes position - * @param[in] actAxisVel Actuated Axes velocity - * @param[in] actAxisAcc Actuated Axes acceleration - * @param[out] physJointAcc Physical Joint acceleration + * @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 convertFromActuatedAxisAccToPhysicalJointAcc(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisVel, const yarp::sig::Vector& actAxisAcc, yarp::sig::Vector& physJointAcc) = 0; + 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 Joint acceleration + * @brief Convert from Actuated Axes to Physical Joints acceleration * - * @param[in] actAxisPos Actuated Axes position - * @param[in] actAxisTrq Actuated Axes torque - * @param[out] physJointTrq Physical Joint torque + * @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 convertFromActuatedAxisTrqToPhysicalJointTrq(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisTrq, yarp::sig::Vector& physJointTrq) = 0; + 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 physical joints object + * @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 * - * @return yarp::sig::VectorOf + * @return the vector of "physical joints indices" */ - virtual yarp::sig::VectorOf getPhysicalJoints()=0; + virtual yarp::sig::VectorOf getCoupledPhysicalJoints()=0; /** - * @brief Get the name of a physical joint + * @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 joint index of the joint - * @return name of the joint + * @return teh vector of "actuator axis indices" */ - virtual std::string getPhysicalJointName(size_t joint)=0; + virtual yarp::sig::VectorOf getCoupledActuatedAxes()=0; /** - * @brief Check if a physical joint is coupled + * @brief Get the name of an actuated axis * - * @param joint index of the joint - * @return true/false on coupled/not coupled + * @param[in] actuatedAxisIndex the number from 0 to m-1 that identifies + * the location of a "actuated axis" in a actuated axis vector. + * @return the actuated axis name */ - virtual bool checkPhysicalJointIsCoupled(size_t joint)=0; + virtual std::string getActuatedAxisName(size_t actuatedAxisIndex)=0; /** - * @brief Set limts for a physical joint + * @brief Get the name of a physical joint * - * @param[in] joint index of the joint - * @param[in] min minimum value - * @param[in] max maximum value - * @return true/false on success/failure + * @param[in] physicalJointIndex the number from 0 to n-1 that identifies + * the location of a "physical joint" in a physical joint vector + * @return the physical joint name */ - virtual bool setPhysicalJointLimits(size_t joint, const double& min, const double& max)=0; + virtual std::string getPhysicalJointName(size_t physicalJointIndex)=0; /** * @brief Get the Physical Joint Limit object * - * @param[in] joint index of the 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] min minimum value * @param[out] max maximum value * @return true/false on success/failure */ - virtual bool getPhysicalJointLimits(size_t joint, double& min, double& max)=0; + 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 index d3b6ec6089..618a3ce88b 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp @@ -5,15 +5,22 @@ #include "ImplementJointCoupling.h" +#include using namespace yarp::dev; -void ImplementJointCoupling::initialise(yarp::sig::VectorOf physical_joints, std::vector physical_joint_names, std::vector> physical_joint_limits) { - m_physicalJoints=physical_joints; - m_physicalJointNames=physical_joint_names; - +void ImplementJointCoupling::initialise(yarp::sig::VectorOf physical_joints, + yarp::sig::VectorOf actuated_axes, + std::vector physical_joint_names, + std::vector actuated_axes_names, + std::vector> physical_joint_limits) +{ + m_physicalJoints = physical_joints; + m_actuatedAxes = actuated_axes; + m_physicalJointNames = physical_joint_names; + m_actuatedAxesNames = actuated_axes_names; // Configure a map between physical joints and limits for (std::size_t i = 0, j = 0; i < physical_joints.size(); i++) { @@ -30,15 +37,20 @@ void ImplementJointCoupling::initialise(yarp::sig::VectorOf physical_joi } -yarp::sig::VectorOf ImplementJointCoupling::getPhysicalJoints() { +yarp::sig::VectorOf ImplementJointCoupling::getCoupledPhysicalJoints() { return m_physicalJoints; } -std::string ImplementJointCoupling::getPhysicalJointName(size_t joint){ +yarp::sig::VectorOf ImplementJointCoupling::getCoupledActuatedAxes() { + return m_actuatedAxes; +} + +std::string ImplementJointCoupling::getPhysicalJointName(size_t physicalJointIndex){ int c_joint = -1; + // TODO refactor also here for (size_t i = 0; i < m_physicalJoints.size(); ++i) { - if (m_physicalJoints[i]==joint) c_joint = i; + if (m_physicalJoints[i]==physicalJointIndex) c_joint = i; } if (c_joint >= 0 && static_cast(c_joint) < m_physicalJoints.size()) @@ -51,31 +63,22 @@ std::string ImplementJointCoupling::getPhysicalJointName(size_t joint){ } } -bool ImplementJointCoupling::checkPhysicalJointIsCoupled(size_t joint){ - for (size_t i = 0; i < m_physicalJoints.size(); ++i) - { - if (m_physicalJoints[i]==joint) return true; - } - return false; +std::string ImplementJointCoupling::getActuatedAxisName(size_t actuatedAxisIndex){ + // TODO is it right? + return m_actuatedAxesNames[actuatedAxisIndex]; } -bool ImplementJointCoupling::setPhysicalJointLimits(size_t joint, const double& min, const double& max){ - const std::string physical_joint_name = getPhysicalJointName(joint); - if (physical_joint_name != "reserved" && physical_joint_name != "invalid") - { - m_physicalJointLimits.at(joint).first = min; - m_physicalJointLimits.at(joint).second = max; - return true; - } - return false; +bool ImplementJointCoupling::checkPhysicalJointIsCoupled(size_t physicalJointIndex){ + return std::find(m_physicalJoints.begin(), m_physicalJoints.end(), physicalJointIndex) != m_physicalJoints.end(); } -bool ImplementJointCoupling::getPhysicalJointLimits(size_t joint, double& min, double& max){ - const std::string physical_joint_name = getPhysicalJointName(joint); + +bool ImplementJointCoupling::getPhysicalJointLimits(size_t physicalJointIndex, double& min, double& max){ + const std::string physical_joint_name = getPhysicalJointName(physicalJointIndex); if (physical_joint_name != "reserved" && physical_joint_name != "gyp_invalid") { - min = m_physicalJointLimits.at(joint).first; - max = m_physicalJointLimits.at(joint).second; + min = m_physicalJointLimits.at(physicalJointIndex).first; + max = m_physicalJointLimits.at(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 index a97964a1c6..0f7c7f5f5f 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h @@ -29,16 +29,25 @@ class YARP_dev_API yarp::dev::ImplementJointCoupling: public IJointCoupling */ virtual ~ImplementJointCoupling() = default; - void initialise(yarp::sig::VectorOf physical_joints, std::vector physical_joint_names, std::vector> physical_joint_limits); - - yarp::sig::VectorOf getPhysicalJoints() override final; - std::string getPhysicalJointName(size_t joint) override final; - bool checkPhysicalJointIsCoupled(size_t joint) override final; - bool setPhysicalJointLimits(size_t joint, const double& min, const double& max) override final; - bool getPhysicalJointLimits(size_t joint, double& min, double& max) override final; + void initialise(yarp::sig::VectorOf physical_joints, + yarp::sig::VectorOf actuated_axes, + std::vector physical_joint_names, + std::vector actuated_axes_names, + std::vector> physical_joint_limits); + + yarp::sig::VectorOf getCoupledPhysicalJoints() override final; + yarp::sig::VectorOf getCoupledActuatedAxes() override final; + std::string getPhysicalJointName(size_t physicalJointIndex) override final; + std::string getActuatedAxisName(size_t actuatedAxisIndex) override final; + + bool getPhysicalJointLimits(size_t physicalJointIndex, double& min, double& max) override final; protected: + bool checkPhysicalJointIsCoupled(size_t physicalJointIndex); + yarp::sig::VectorOf m_physicalJoints; + yarp::sig::VectorOf m_actuatedAxes; std::vector m_physicalJointNames; + std::vector m_actuatedAxesNames; std::unordered_map> m_physicalJointLimits; unsigned int m_controllerPeriod; unsigned int m_couplingSize; From d47ae629fb8f2e07d24fe13587a8d9c1359be05b Mon Sep 17 00:00:00 2001 From: Nicogene Date: Tue, 24 Oct 2023 16:05:12 +0200 Subject: [PATCH 06/12] ImplementJointCoupling Fix style --- src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp | 2 +- src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp index 618a3ce88b..f058218be6 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp @@ -82,4 +82,4 @@ bool ImplementJointCoupling::getPhysicalJointLimits(size_t physicalJointIndex, d return true; } return false; -} \ No newline at end of file +} diff --git a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h index 0f7c7f5f5f..c6e2cbfa4c 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h @@ -54,4 +54,4 @@ class YARP_dev_API yarp::dev::ImplementJointCoupling: public IJointCoupling }; -#endif // YARP_DEV_IJOINTCOUPLINGIMPL_H \ No newline at end of file +#endif // YARP_DEV_IJOINTCOUPLINGIMPL_H From a73e48fd3cd1c0c1667aa5a5f9a48f36974c9dc4 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Fri, 27 Oct 2023 11:37:34 +0200 Subject: [PATCH 07/12] IJointCoupling: make all methods return bools --- src/libYARP_dev/src/yarp/dev/IJointCoupling.h | 21 ++++++------ .../src/yarp/dev/ImplementJointCoupling.cpp | 32 +++++++++++-------- .../src/yarp/dev/ImplementJointCoupling.h | 8 ++--- 3 files changed, 35 insertions(+), 26 deletions(-) diff --git a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h index 0aedd3217d..4fabc76f7e 100644 --- a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h @@ -176,36 +176,39 @@ class YARP_dev_API yarp::dev::IJointCoupling /** * @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 - * - * @return the vector of "physical joints indices" + * @param[out] coupPhysJointsIndexes the vector of "physical joints indices" + * @return true/false on success/failure */ - virtual yarp::sig::VectorOf getCoupledPhysicalJoints()=0; + 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 * - * @return teh vector of "actuator axis indices" + * @param[out] coupActAxesIndexes the vector of "actuator axis indices" + * @return true/false on success/failure */ - virtual yarp::sig::VectorOf getCoupledActuatedAxes()=0; + 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. - * @return the actuated axis name + * @param[out] actuatedAxisName the actuated axis name + * @return true/false on success/failure */ - virtual std::string getActuatedAxisName(size_t actuatedAxisIndex)=0; + 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 - * @return the physical joint name + * @param[out] physicalJointName the physical joint name + * @return true/false on success/failure */ - virtual std::string getPhysicalJointName(size_t physicalJointIndex)=0; + virtual bool getPhysicalJointName(size_t physicalJointIndex, std::string& physicalJointName)=0; /** * @brief Get the Physical Joint Limit object diff --git a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp index f058218be6..f455d95da5 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp @@ -25,7 +25,8 @@ void ImplementJointCoupling::initialise(yarp::sig::VectorOf physical_joi for (std::size_t i = 0, j = 0; i < physical_joints.size(); i++) { const int physical_joint_index = physical_joints(i); - const std::string physical_joint_name = getPhysicalJointName(physical_joint_index); + std::string physical_joint_name {""}; + auto ok = getPhysicalJointName(physical_joint_index, physical_joint_name); if (physical_joint_name != "invalid" && physical_joint_name != "reserved") { m_physicalJointLimits[physical_joints[i]] = physical_joint_limits[j]; @@ -36,16 +37,17 @@ void ImplementJointCoupling::initialise(yarp::sig::VectorOf physical_joi } - -yarp::sig::VectorOf ImplementJointCoupling::getCoupledPhysicalJoints() { - return m_physicalJoints; +bool ImplementJointCoupling::getCoupledPhysicalJoints(yarp::sig::VectorOf& coupPhysJointsIndexes) { + coupPhysJointsIndexes = m_physicalJoints; + return true; } -yarp::sig::VectorOf ImplementJointCoupling::getCoupledActuatedAxes() { - return m_actuatedAxes; +bool ImplementJointCoupling::getCoupledActuatedAxes(yarp::sig::VectorOf& coupActAxesIndexes) { + coupActAxesIndexes = m_actuatedAxes; + return true; } -std::string ImplementJointCoupling::getPhysicalJointName(size_t physicalJointIndex){ +bool ImplementJointCoupling::getPhysicalJointName(size_t physicalJointIndex, std::string& physicalJointName){ int c_joint = -1; // TODO refactor also here for (size_t i = 0; i < m_physicalJoints.size(); ++i) @@ -55,17 +57,20 @@ std::string ImplementJointCoupling::getPhysicalJointName(size_t physicalJointInd if (c_joint >= 0 && static_cast(c_joint) < m_physicalJoints.size()) { - return m_physicalJointNames[c_joint]; + physicalJointName = m_physicalJointNames[c_joint]; + return true; } else { - return std::string("invalid"); + physicalJointName = "invalid"; + return false; } } -std::string ImplementJointCoupling::getActuatedAxisName(size_t actuatedAxisIndex){ +bool ImplementJointCoupling::getActuatedAxisName(size_t actuatedAxisIndex, std::string& actuatedAxisName){ // TODO is it right? - return m_actuatedAxesNames[actuatedAxisIndex]; + actuatedAxisName = m_actuatedAxesNames[actuatedAxisIndex]; + return true; } bool ImplementJointCoupling::checkPhysicalJointIsCoupled(size_t physicalJointIndex){ @@ -73,9 +78,10 @@ bool ImplementJointCoupling::checkPhysicalJointIsCoupled(size_t physicalJointInd } bool ImplementJointCoupling::getPhysicalJointLimits(size_t physicalJointIndex, double& min, double& max){ - const std::string physical_joint_name = getPhysicalJointName(physicalJointIndex); + std::string physical_joint_name{""}; + auto ok = getPhysicalJointName(physicalJointIndex, physical_joint_name); - if (physical_joint_name != "reserved" && physical_joint_name != "gyp_invalid") + if (physical_joint_name != "reserved" && physical_joint_name != "invalid") { min = m_physicalJointLimits.at(physicalJointIndex).first; max = m_physicalJointLimits.at(physicalJointIndex).second; diff --git a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h index c6e2cbfa4c..2ab4128dc4 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h @@ -35,10 +35,10 @@ class YARP_dev_API yarp::dev::ImplementJointCoupling: public IJointCoupling std::vector actuated_axes_names, std::vector> physical_joint_limits); - yarp::sig::VectorOf getCoupledPhysicalJoints() override final; - yarp::sig::VectorOf getCoupledActuatedAxes() override final; - std::string getPhysicalJointName(size_t physicalJointIndex) override final; - std::string getActuatedAxisName(size_t actuatedAxisIndex) 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: From ac5f362b6f987ee71fc7b410e3071d55896754d6 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Fri, 27 Oct 2023 15:01:14 +0200 Subject: [PATCH 08/12] iJointCoupling: add test device --- src/devices/CMakeLists.txt | 1 + src/devices/fakeJointCoupling/CMakeLists.txt | 52 +++++++++ .../fakeJointCoupling/fakeJointCoupling.cpp | 103 ++++++++++++++++++ .../fakeJointCoupling/fakeJointCoupling.h | 54 +++++++++ .../fakeJointCoupling/tests/CMakeLists.txt | 4 + .../tests/fakeJointCoupling_test.cpp | 41 +++++++ 6 files changed, 255 insertions(+) create mode 100644 src/devices/fakeJointCoupling/CMakeLists.txt create mode 100644 src/devices/fakeJointCoupling/fakeJointCoupling.cpp create mode 100644 src/devices/fakeJointCoupling/fakeJointCoupling.h create mode 100644 src/devices/fakeJointCoupling/tests/CMakeLists.txt create mode 100644 src/devices/fakeJointCoupling/tests/fakeJointCoupling_test.cpp 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..466f6c935b --- /dev/null +++ b/src/devices/fakeJointCoupling/fakeJointCoupling.cpp @@ -0,0 +1,103 @@ +/* + * 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) { + return true; +} +bool FakeJointCoupling::close() { + return true; +} + +bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) { + if (physJointsPos.size() != actAxesPos.size()) { + yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input and output vectors have different size"; + return false; + } + std::transform(physJointsPos.begin(), physJointsPos.end(), actAxesPos.begin(), [](double pos) { return pos * 2; }); + return true; +} +bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) { + if(physJointsPos.size() != physJointsVel.size() || physJointsPos.size() != actAxesVel.size()) { + yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesVel: input and output vectors have different size"; + return false; + } + for(size_t i = 0; i < physJointsPos.size(); i++) { + actAxesVel[i] = 2 * physJointsPos[i] * + 2 * physJointsVel[i]; + } + return true; +} +bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, + const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) { + if(physJointsPos.size() != physJointsVel.size() || physJointsPos.size() != physJointsAcc.size() || physJointsPos.size() != actAxesAcc.size()) { + yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesAcc: input and output vectors have different size"; + return false; + } + for(size_t i = 0; i < physJointsPos.size(); i++) { + actAxesAcc[i] = 2 * physJointsPos[i] + 2 * physJointsVel[i] + 2* physJointsAcc[i]; + } + 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) { + if(actAxesPos.size() != physJointsPos.size()) { + yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsPos: input and output vectors have different size"; + return false; + } + std::transform(actAxesPos.begin(), actAxesPos.end(), physJointsPos.begin(), [](double pos) { return pos / 2.0; }); + return true; +} +bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) { + if(actAxesPos.size() != actAxesVel.size() || actAxesPos.size() != physJointsVel.size()) { + yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsVel: input and output vectors have different size"; + return false; + } + for(size_t i = 0; i < actAxesPos.size(); i++) { + physJointsVel[i] = actAxesPos[i] / 2.0 + actAxesVel[i] / 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) { + if(actAxesPos.size() != actAxesVel.size() || actAxesPos.size() != actAxesAcc.size() || actAxesPos.size() != physJointsAcc.size()) { + yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsAcc: input and output vectors have different size"; + return false; + } + for(size_t i = 0; i < actAxesPos.size(); i++) { + physJointsAcc[i] = actAxesPos[i] / 2.0 + actAxesVel[i] / 2.0 + actAxesAcc[i] / 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; +} + diff --git a/src/devices/fakeJointCoupling/fakeJointCoupling.h b/src/devices/fakeJointCoupling/fakeJointCoupling.h new file mode 100644 index 0000000000..4ece250a8c --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +#include + + +/** + * @ingroup dev_impl_fake dev_impl_motor + * + * \brief `fakeMotionControl`: 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..f7f66b1217 --- /dev/null +++ b/src/devices/fakeJointCoupling/tests/fakeJointCoupling_test.cpp @@ -0,0 +1,41 @@ +/* + * 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)); + + //"Close all polydrivers and check" + { + CHECK(ddjc.close()); + } + } + + Network::setLocalMode(false); +} From c3794d5decb9717a2435555c90e1fdbb8ba33b9d Mon Sep 17 00:00:00 2001 From: Nicogene Date: Fri, 27 Oct 2023 15:06:30 +0200 Subject: [PATCH 09/12] fakeJointCoupling: fix style --- src/devices/fakeJointCoupling/fakeJointCoupling.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/devices/fakeJointCoupling/fakeJointCoupling.cpp b/src/devices/fakeJointCoupling/fakeJointCoupling.cpp index 466f6c935b..db334a035e 100644 --- a/src/devices/fakeJointCoupling/fakeJointCoupling.cpp +++ b/src/devices/fakeJointCoupling/fakeJointCoupling.cpp @@ -100,4 +100,3 @@ bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::s yCWarning(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsTrq: not implemented yet"; return false; } - From 0d6a37ef53bee7ebf5bee98f051b5f97af4f328c Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 30 Oct 2023 16:11:53 +0100 Subject: [PATCH 10/12] fakeJointCouplingTest: first implementation and check of functions --- .../fakeJointCoupling/fakeJointCoupling.cpp | 6 +++ .../tests/fakeJointCoupling_test.cpp | 39 +++++++++++++++++++ .../src/yarp/dev/ImplementJointCoupling.cpp | 30 +++++++------- .../src/yarp/dev/ImplementJointCoupling.h | 10 ++--- 4 files changed, 65 insertions(+), 20 deletions(-) diff --git a/src/devices/fakeJointCoupling/fakeJointCoupling.cpp b/src/devices/fakeJointCoupling/fakeJointCoupling.cpp index db334a035e..4dcf1bc386 100644 --- a/src/devices/fakeJointCoupling/fakeJointCoupling.cpp +++ b/src/devices/fakeJointCoupling/fakeJointCoupling.cpp @@ -28,6 +28,12 @@ YARP_LOG_COMPONENT(FAKEJOINTCOUPLING, "yarp.device.fakeJointCoupling") } bool FakeJointCoupling::open(yarp::os::Searchable &par) { + yarp::sig::VectorOf coupled_physical_joints {3,4}; + yarp::sig::VectorOf coupled_actuated_axes {2}; + std::vector physical_joint_names{"phys_joint_0", "phys_joint_1", "phys_joint_2", "phys_joint_3", "phys_joint_4"}; + 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}, {-20.0, 180.0}}; + initialise(coupled_physical_joints, coupled_actuated_axes, physical_joint_names, actuated_axes_names, physical_joint_limits); return true; } bool FakeJointCoupling::close() { diff --git a/src/devices/fakeJointCoupling/tests/fakeJointCoupling_test.cpp b/src/devices/fakeJointCoupling/tests/fakeJointCoupling_test.cpp index f7f66b1217..6a0ef2084a 100644 --- a/src/devices/fakeJointCoupling/tests/fakeJointCoupling_test.cpp +++ b/src/devices/fakeJointCoupling/tests/fakeJointCoupling_test.cpp @@ -35,6 +35,45 @@ TEST_CASE("dev::fakeJointCoupling", "[yarp::dev]") { CHECK(ddjc.close()); } + + yarp::sig::VectorOf coupled_physical_joints; + CHECK(ijc->getCoupledPhysicalJoints(coupled_physical_joints)); + CHECK(coupled_physical_joints.size() == 2); + CHECK(coupled_physical_joints[0] == 3); + CHECK(coupled_physical_joints[1] == 4); + + yarp::sig::VectorOf coupled_actuated_axes; + CHECK(ijc->getCoupledActuatedAxes(coupled_actuated_axes)); + CHECK(coupled_actuated_axes.size() == 1); + CHECK(coupled_actuated_axes[0] == 2); + + 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"); + + 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"); + + + + + // virtual bool convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) = 0; + // virtual bool convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) = 0; + // virtual bool convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) = 0; + // virtual bool convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) = 0; + // virtual bool convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) = 0; + // virtual bool convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) = 0; + // virtual bool convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) = 0; + // virtual bool convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) = 0; + // virtual bool getCoupledActuatedAxes(yarp::sig::VectorOf& coupActAxesIndexes)=0; + // virtual bool getPhysicalJointLimits(size_t physicalJointIndex, double& min, double& max)=0; } Network::setLocalMode(false); diff --git a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp index f455d95da5..a64d86aa76 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp @@ -11,25 +11,25 @@ using namespace yarp::dev; -void ImplementJointCoupling::initialise(yarp::sig::VectorOf physical_joints, - yarp::sig::VectorOf actuated_axes, +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_physicalJoints = physical_joints; - m_actuatedAxes = actuated_axes; - m_physicalJointNames = physical_joint_names; - m_actuatedAxesNames = actuated_axes_names; + m_coupledPhysicalJoints = coupled_physical_joints; + m_coupledActuatedAxes = coupled_actuated_axes; + m_physicalJointNames = physical_joint_names; + m_actuatedAxesNames = actuated_axes_names; // Configure a map between physical joints and limits - for (std::size_t i = 0, j = 0; i < physical_joints.size(); i++) + for (std::size_t i = 0, j = 0; i < coupled_physical_joints.size(); i++) { - const int physical_joint_index = physical_joints(i); + const int physical_joint_index = coupled_physical_joints(i); std::string physical_joint_name {""}; auto ok = getPhysicalJointName(physical_joint_index, physical_joint_name); if (physical_joint_name != "invalid" && physical_joint_name != "reserved") { - m_physicalJointLimits[physical_joints[i]] = physical_joint_limits[j]; + m_physicalJointLimits[coupled_physical_joints[i]] = physical_joint_limits[j]; j++; } } @@ -38,24 +38,24 @@ void ImplementJointCoupling::initialise(yarp::sig::VectorOf physical_joi } bool ImplementJointCoupling::getCoupledPhysicalJoints(yarp::sig::VectorOf& coupPhysJointsIndexes) { - coupPhysJointsIndexes = m_physicalJoints; + coupPhysJointsIndexes = m_coupledPhysicalJoints; return true; } bool ImplementJointCoupling::getCoupledActuatedAxes(yarp::sig::VectorOf& coupActAxesIndexes) { - coupActAxesIndexes = m_actuatedAxes; + coupActAxesIndexes = m_coupledActuatedAxes; return true; } bool ImplementJointCoupling::getPhysicalJointName(size_t physicalJointIndex, std::string& physicalJointName){ int c_joint = -1; // TODO refactor also here - for (size_t i = 0; i < m_physicalJoints.size(); ++i) + for (size_t i = 0; i < m_coupledPhysicalJoints.size(); ++i) { - if (m_physicalJoints[i]==physicalJointIndex) c_joint = i; + if (m_coupledPhysicalJoints[i]==physicalJointIndex) c_joint = i; } - if (c_joint >= 0 && static_cast(c_joint) < m_physicalJoints.size()) + if (c_joint >= 0 && static_cast(c_joint) < m_coupledPhysicalJoints.size()) { physicalJointName = m_physicalJointNames[c_joint]; return true; @@ -74,7 +74,7 @@ bool ImplementJointCoupling::getActuatedAxisName(size_t actuatedAxisIndex, std:: } bool ImplementJointCoupling::checkPhysicalJointIsCoupled(size_t physicalJointIndex){ - return std::find(m_physicalJoints.begin(), m_physicalJoints.end(), physicalJointIndex) != m_physicalJoints.end(); + return std::find(m_coupledPhysicalJoints.begin(), m_coupledPhysicalJoints.end(), physicalJointIndex) != m_coupledPhysicalJoints.end(); } bool ImplementJointCoupling::getPhysicalJointLimits(size_t physicalJointIndex, double& min, double& max){ diff --git a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h index 2ab4128dc4..5168b2807a 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h @@ -29,11 +29,11 @@ class YARP_dev_API yarp::dev::ImplementJointCoupling: public IJointCoupling */ virtual ~ImplementJointCoupling() = default; - void initialise(yarp::sig::VectorOf physical_joints, - yarp::sig::VectorOf actuated_axes, + 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> physical_joint_limits); + std::vector> coupled_physical_joint_limits); bool getCoupledPhysicalJoints(yarp::sig::VectorOf& coupPhysJointsIndexes) override final; bool getCoupledActuatedAxes(yarp::sig::VectorOf& coupActAxesIndexes) override final; @@ -44,8 +44,8 @@ class YARP_dev_API yarp::dev::ImplementJointCoupling: public IJointCoupling protected: bool checkPhysicalJointIsCoupled(size_t physicalJointIndex); - yarp::sig::VectorOf m_physicalJoints; - yarp::sig::VectorOf m_actuatedAxes; + yarp::sig::VectorOf m_coupledPhysicalJoints; + yarp::sig::VectorOf m_coupledActuatedAxes; std::vector m_physicalJointNames; std::vector m_actuatedAxesNames; std::unordered_map> m_physicalJointLimits; From c09d23108d1e52a0b690473c2ef4925822259a44 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 2 Nov 2023 15:46:11 +0100 Subject: [PATCH 11/12] IJointCoupling: add getNrOfPhysicalJoints/ActuatedAxes --- .../tests/fakeJointCoupling_test.cpp | 20 +++++++++++++++---- src/libYARP_dev/src/yarp/dev/IJointCoupling.h | 16 +++++++++++++++ .../src/yarp/dev/ImplementJointCoupling.cpp | 11 ++++++++++ .../src/yarp/dev/ImplementJointCoupling.h | 4 ++-- 4 files changed, 45 insertions(+), 6 deletions(-) diff --git a/src/devices/fakeJointCoupling/tests/fakeJointCoupling_test.cpp b/src/devices/fakeJointCoupling/tests/fakeJointCoupling_test.cpp index 6a0ef2084a..9ad80aec98 100644 --- a/src/devices/fakeJointCoupling/tests/fakeJointCoupling_test.cpp +++ b/src/devices/fakeJointCoupling/tests/fakeJointCoupling_test.cpp @@ -31,22 +31,29 @@ TEST_CASE("dev::fakeJointCoupling", "[yarp::dev]") REQUIRE(ddjc.view(ijc)); - //"Close all polydrivers and check" - { - CHECK(ddjc.close()); - } + 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] == 3); CHECK(coupled_physical_joints[1] == 4); 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 == 5); + + 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"); @@ -61,6 +68,11 @@ TEST_CASE("dev::fakeJointCoupling", "[yarp::dev]") CHECK(ijc->getActuatedAxisName(1, actuated_axis_name)); CHECK(actuated_axis_name == "act_axes_1"); + //"Close all polydrivers and check" + { + CHECK(ddjc.close()); + } + diff --git a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h index 4fabc76f7e..1e733bed24 100644 --- a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h @@ -173,6 +173,22 @@ class YARP_dev_API yarp::dev::IJointCoupling // 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 diff --git a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp index a64d86aa76..f14ded5d6d 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp @@ -37,6 +37,17 @@ void ImplementJointCoupling::initialise(yarp::sig::VectorOf coupled_phys } +bool ImplementJointCoupling::getNrOfPhysicalJoints(size_t* nrOfPhysicalJoints) { + //TODO is it right? + *nrOfPhysicalJoints = m_physicalJointLimits.size(); + return true; +} +bool ImplementJointCoupling::getNrOfActuatedAxes(size_t* nrOfActuatedAxes){ + // TODO is it right? + *nrOfActuatedAxes = m_actuatedAxesNames.size(); + return true; +} + bool ImplementJointCoupling::getCoupledPhysicalJoints(yarp::sig::VectorOf& coupPhysJointsIndexes) { coupPhysJointsIndexes = m_coupledPhysicalJoints; return true; diff --git a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h index 5168b2807a..cc4f9620fe 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h @@ -34,12 +34,12 @@ class YARP_dev_API yarp::dev::ImplementJointCoupling: public IJointCoupling 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); From 5d9e3c3f217a3578983bee5ddf1246923108248f Mon Sep 17 00:00:00 2001 From: Nicogene Date: Fri, 3 Nov 2023 15:18:52 +0100 Subject: [PATCH 12/12] IJointCoupling: final draft --- .../fakeJointCoupling/fakeJointCoupling.cpp | 90 ++++++++++++------ .../fakeJointCoupling/fakeJointCoupling.h | 2 +- .../tests/fakeJointCoupling_test.cpp | 95 ++++++++++++++----- src/libYARP_dev/src/yarp/dev/IJointCoupling.h | 4 +- .../src/yarp/dev/ImplementJointCoupling.cpp | 65 +++++-------- .../src/yarp/dev/ImplementJointCoupling.h | 7 +- 6 files changed, 162 insertions(+), 101 deletions(-) diff --git a/src/devices/fakeJointCoupling/fakeJointCoupling.cpp b/src/devices/fakeJointCoupling/fakeJointCoupling.cpp index 4dcf1bc386..6df6ea3505 100644 --- a/src/devices/fakeJointCoupling/fakeJointCoupling.cpp +++ b/src/devices/fakeJointCoupling/fakeJointCoupling.cpp @@ -28,11 +28,11 @@ YARP_LOG_COMPONENT(FAKEJOINTCOUPLING, "yarp.device.fakeJointCoupling") } bool FakeJointCoupling::open(yarp::os::Searchable &par) { - yarp::sig::VectorOf coupled_physical_joints {3,4}; + 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", "phys_joint_4"}; + 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}, {-20.0, 180.0}}; + 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; } @@ -41,32 +41,47 @@ bool FakeJointCoupling::close() { } bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) { - if (physJointsPos.size() != actAxesPos.size()) { - yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input and output vectors have different size"; + 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 < coupled_physical_joints; coupled_physical_joints.clear(); CHECK(ijc->getCoupledPhysicalJoints(coupled_physical_joints)); CHECK(coupled_physical_joints.size() == 2); - CHECK(coupled_physical_joints[0] == 3); - CHECK(coupled_physical_joints[1] == 4); + CHECK(coupled_physical_joints[0] == 2); + CHECK(coupled_physical_joints[1] == 3); yarp::sig::VectorOf coupled_actuated_axes; coupled_actuated_axes.clear(); @@ -47,11 +45,11 @@ TEST_CASE("dev::fakeJointCoupling", "[yarp::dev]") 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 == 5); + 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(ijc->getNrOfActuatedAxes(nr_of_actuated_axes)); CHECK(nr_of_actuated_axes == 3); std::string physical_joint_name; @@ -61,31 +59,84 @@ TEST_CASE("dev::fakeJointCoupling", "[yarp::dev]") 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()); } - - - - - // virtual bool convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) = 0; - // virtual bool convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) = 0; - // virtual bool convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) = 0; - // virtual bool convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) = 0; - // virtual bool convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) = 0; - // virtual bool convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) = 0; - // virtual bool convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) = 0; - // virtual bool convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) = 0; - // virtual bool getCoupledActuatedAxes(yarp::sig::VectorOf& coupActAxesIndexes)=0; - // virtual bool getPhysicalJointLimits(size_t physicalJointIndex, double& min, double& max)=0; } Network::setLocalMode(false); diff --git a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h index 1e733bed24..d11a93a0d4 100644 --- a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h @@ -179,7 +179,7 @@ class YARP_dev_API yarp::dev::IJointCoupling * @param[out] nrOfPhysicalJoints The number of physical joints * @return true/false on success/failure */ - virtual bool getNrOfPhysicalJoints(size_t* nrOfPhysicalJoints) = 0; + virtual bool getNrOfPhysicalJoints(size_t& nrOfPhysicalJoints) = 0; /** * @brief Get the number of actuated axes @@ -187,7 +187,7 @@ class YARP_dev_API yarp::dev::IJointCoupling * @param nrOfActuatedAxes The number of actuated axes * @return true/false on success/failure */ - virtual bool getNrOfActuatedAxes(size_t* nrOfActuatedAxes) = 0; + virtual bool getNrOfActuatedAxes(size_t& nrOfActuatedAxes) = 0; /** * @brief Return the vector of "physical joints indices" (i.e. numbers from 0 to n-1) diff --git a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp index f14ded5d6d..0dd469f1d8 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp @@ -21,30 +21,15 @@ void ImplementJointCoupling::initialise(yarp::sig::VectorOf coupled_phys m_coupledActuatedAxes = coupled_actuated_axes; m_physicalJointNames = physical_joint_names; m_actuatedAxesNames = actuated_axes_names; - // Configure a map between physical joints and limits - for (std::size_t i = 0, j = 0; i < coupled_physical_joints.size(); i++) - { - const int physical_joint_index = coupled_physical_joints(i); - std::string physical_joint_name {""}; - auto ok = getPhysicalJointName(physical_joint_index, physical_joint_name); - if (physical_joint_name != "invalid" && physical_joint_name != "reserved") - { - m_physicalJointLimits[coupled_physical_joints[i]] = physical_joint_limits[j]; - j++; - } - } - - + m_physicalJointLimits = physical_joint_limits; } -bool ImplementJointCoupling::getNrOfPhysicalJoints(size_t* nrOfPhysicalJoints) { - //TODO is it right? - *nrOfPhysicalJoints = m_physicalJointLimits.size(); +bool ImplementJointCoupling::getNrOfPhysicalJoints(size_t& nrOfPhysicalJoints) { + nrOfPhysicalJoints = m_physicalJointNames.size(); return true; } -bool ImplementJointCoupling::getNrOfActuatedAxes(size_t* nrOfActuatedAxes){ - // TODO is it right? - *nrOfActuatedAxes = m_actuatedAxesNames.size(); +bool ImplementJointCoupling::getNrOfActuatedAxes(size_t& nrOfActuatedAxes){ + nrOfActuatedAxes = m_actuatedAxesNames.size(); return true; } @@ -59,29 +44,24 @@ bool ImplementJointCoupling::getCoupledActuatedAxes(yarp::sig::VectorOf& } bool ImplementJointCoupling::getPhysicalJointName(size_t physicalJointIndex, std::string& physicalJointName){ - int c_joint = -1; - // TODO refactor also here - for (size_t i = 0; i < m_coupledPhysicalJoints.size(); ++i) - { - if (m_coupledPhysicalJoints[i]==physicalJointIndex) c_joint = i; + if(physicalJointIndex >= m_physicalJointNames.size()) { + return false; } - - if (c_joint >= 0 && static_cast(c_joint) < m_coupledPhysicalJoints.size()) - { - physicalJointName = m_physicalJointNames[c_joint]; + else { + physicalJointName=m_physicalJointNames[physicalJointIndex]; return true; } - else - { - physicalJointName = "invalid"; - return false; - } + } bool ImplementJointCoupling::getActuatedAxisName(size_t actuatedAxisIndex, std::string& actuatedAxisName){ - // TODO is it right? - actuatedAxisName = m_actuatedAxesNames[actuatedAxisIndex]; - return true; + if(actuatedAxisIndex >= m_actuatedAxesNames.size()) { + return false; + } + else { + actuatedAxisName=m_actuatedAxesNames[actuatedAxisIndex]; + return true; + } } bool ImplementJointCoupling::checkPhysicalJointIsCoupled(size_t physicalJointIndex){ @@ -89,13 +69,12 @@ bool ImplementJointCoupling::checkPhysicalJointIsCoupled(size_t physicalJointInd } bool ImplementJointCoupling::getPhysicalJointLimits(size_t physicalJointIndex, double& min, double& max){ - std::string physical_joint_name{""}; - auto ok = getPhysicalJointName(physicalJointIndex, physical_joint_name); - - if (physical_joint_name != "reserved" && physical_joint_name != "invalid") + size_t nrOfPhysicalJoints; + auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); + if (ok && physicalJointIndex < nrOfPhysicalJoints) { - min = m_physicalJointLimits.at(physicalJointIndex).first; - max = m_physicalJointLimits.at(physicalJointIndex).second; + 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 index cc4f9620fe..45279a1854 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h @@ -10,7 +10,6 @@ #include #include -#include #include namespace yarp::dev { @@ -34,8 +33,8 @@ class YARP_dev_API yarp::dev::ImplementJointCoupling: public IJointCoupling 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 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; @@ -48,7 +47,7 @@ class YARP_dev_API yarp::dev::ImplementJointCoupling: public IJointCoupling yarp::sig::VectorOf m_coupledActuatedAxes; std::vector m_physicalJointNames; std::vector m_actuatedAxesNames; - std::unordered_map> m_physicalJointLimits; + std::vector> m_physicalJointLimits; unsigned int m_controllerPeriod; unsigned int m_couplingSize;