From 5d9e3c3f217a3578983bee5ddf1246923108248f Mon Sep 17 00:00:00 2001 From: Nicogene Date: Fri, 3 Nov 2023 15:18:52 +0100 Subject: [PATCH] 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;