From 4613608eacbede5fc03fda00854e1f77186de834 Mon Sep 17 00:00:00 2001 From: Nicola Piga Date: Wed, 28 Feb 2018 11:17:10 +0100 Subject: [PATCH 1/4] Move private struct yarp::dev::GazeboYarpControlBoardriver::Range in a seperate header --- plugins/controlboard/CMakeLists.txt | 3 ++- .../include/yarp/dev/ControlBoardDriver.h | 7 +------ .../include/yarp/dev/ControlBoardDriverRange.h | 16 ++++++++++++++++ 3 files changed, 19 insertions(+), 7 deletions(-) create mode 100644 plugins/controlboard/include/yarp/dev/ControlBoardDriverRange.h diff --git a/plugins/controlboard/CMakeLists.txt b/plugins/controlboard/CMakeLists.txt index 07e82b592..11abd14a2 100644 --- a/plugins/controlboard/CMakeLists.txt +++ b/plugins/controlboard/CMakeLists.txt @@ -34,7 +34,8 @@ set(controlBoard_source src/ControlBoard.cc set(controlBoard_headers include/gazebo/ControlBoard.hh include/yarp/dev/ControlBoardDriver.h include/yarp/dev/ControlBoardDriverTrajectory.h - include/yarp/dev/ControlBoardDriverCoupling.h) + include/yarp/dev/ControlBoardDriverCoupling.h + include/yarp/dev/ControlBoardDriverRange.h) diff --git a/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h b/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h index cc77414fe..96aad7e63 100644 --- a/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h +++ b/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h @@ -27,6 +27,7 @@ #include #include #include +#include #include #include @@ -349,12 +350,6 @@ class yarp::dev::GazeboYarpControlBoardDriver: JointType_Prismatic }; - struct Range { - Range() : min(0), max(0){} - double min; - double max; - }; - std::string m_deviceName; gazebo::physics::Model* m_robot; diff --git a/plugins/controlboard/include/yarp/dev/ControlBoardDriverRange.h b/plugins/controlboard/include/yarp/dev/ControlBoardDriverRange.h new file mode 100644 index 000000000..098177eb2 --- /dev/null +++ b/plugins/controlboard/include/yarp/dev/ControlBoardDriverRange.h @@ -0,0 +1,16 @@ +/* + * Copyright (C) 2013-2015 Fondazione Istituto Italiano di Tecnologia RBCS & iCub Facility & ADVR + * Authors: see AUTHORS file. + * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT + */ + +#ifndef GAZEBOYARP_CONTROLBOARDDRIVERRANGE_HH +#define GAZEBOYARP_CONTROLBOARDDRIVERRANGE_HH + +struct Range { + Range() : min(0), max(0){} + double min; + double max; +}; + +#endif From 01796a2787784eb220b69dfd850cb21b1be54cde Mon Sep 17 00:00:00 2001 From: Nicola Piga Date: Wed, 28 Feb 2018 11:19:49 +0100 Subject: [PATCH 2/4] Implement method decouplePosLimits within decoupling handlers --- .../yarp/dev/ControlBoardDriverCoupling.h | 18 +++++++ .../src/ControlBoardDriverCoupling.cpp | 47 +++++++++++++++++++ 2 files changed, 65 insertions(+) diff --git a/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h b/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h index f1ccb1178..ad8d93cb4 100644 --- a/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h +++ b/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h @@ -9,6 +9,8 @@ #include +#include + namespace yarp { namespace dev { enum CouplingType @@ -36,6 +38,8 @@ class BaseCouplingHandler virtual bool decoupleAcc(yarp::sig::Vector& current_acc) = 0; virtual bool decoupleTrq(yarp::sig::Vector& current_trq) = 0; + virtual bool decouplePosLimits(std::vector& pos_limits) = 0; + virtual yarp::sig::VectorOf getCoupledJoints(); virtual std::string getCoupledJointName (int joint); virtual bool checkJointIsCoupled(int joint); @@ -57,6 +61,8 @@ class EyesCouplingHandler : public BaseCouplingHandler bool decoupleAcc (yarp::sig::Vector& current_acc); bool decoupleTrq (yarp::sig::Vector& current_trq); + bool decouplePosLimits(std::vector& pos_limits); + yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref); yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref); yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref); @@ -74,6 +80,8 @@ class ThumbCouplingHandler : public BaseCouplingHandler bool decoupleAcc (yarp::sig::Vector& current_acc); bool decoupleTrq (yarp::sig::Vector& current_trq); + bool decouplePosLimits(std::vector& pos_limits); + yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref); yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref); yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref); @@ -91,6 +99,8 @@ class IndexCouplingHandler : public BaseCouplingHandler bool decoupleAcc (yarp::sig::Vector& current_acc); bool decoupleTrq (yarp::sig::Vector& current_trq); + bool decouplePosLimits(std::vector& pos_limits); + yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref); yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref); yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref); @@ -108,6 +118,8 @@ class MiddleCouplingHandler : public BaseCouplingHandler bool decoupleAcc (yarp::sig::Vector& current_acc); bool decoupleTrq (yarp::sig::Vector& current_trq); + bool decouplePosLimits(std::vector& pos_limits); + yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref); yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref); yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref); @@ -125,6 +137,8 @@ class PinkyCouplingHandler : public BaseCouplingHandler bool decoupleAcc (yarp::sig::Vector& current_acc); bool decoupleTrq (yarp::sig::Vector& current_trq); + bool decouplePosLimits(std::vector& pos_limits); + yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref); yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref); yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref); @@ -142,6 +156,8 @@ class FingersAbductionCouplingHandler : public BaseCouplingHandler bool decoupleAcc (yarp::sig::Vector& current_acc); bool decoupleTrq (yarp::sig::Vector& current_trq); + bool decouplePosLimits(std::vector& pos_limits); + yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref); yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref); yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref); @@ -159,6 +175,8 @@ class CerHandCouplingHandler : public BaseCouplingHandler bool decoupleAcc (yarp::sig::Vector& current_acc); bool decoupleTrq (yarp::sig::Vector& current_trq); + bool decouplePosLimits(std::vector& pos_limits); + yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref); yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref); yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref); diff --git a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp index e8d075606..3af36a3cd 100644 --- a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp +++ b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp @@ -107,6 +107,11 @@ bool EyesCouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq) return false; } +bool EyesCouplingHandler::decouplePosLimits (std::vector& pos_limits) // NOT IMPLEMENTED +{ + return false; +} + yarp::sig::Vector EyesCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref) { yarp::sig::Vector out = pos_ref; @@ -171,6 +176,14 @@ bool ThumbCouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq) return false; } +bool ThumbCouplingHandler::decouplePosLimits (std::vector& pos_limits) +{ + if (m_coupledJoints.size() != m_couplingSize) return false; + pos_limits[m_coupledJoints[2]].min = pos_limits[m_coupledJoints[2]].min + pos_limits[m_coupledJoints[3]].min; + pos_limits[m_coupledJoints[2]].max = pos_limits[m_coupledJoints[2]].max + pos_limits[m_coupledJoints[3]].max; + return true; +} + yarp::sig::Vector ThumbCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref) { yarp::sig::Vector out = pos_ref; @@ -241,6 +254,14 @@ bool IndexCouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq) return false; } +bool IndexCouplingHandler::decouplePosLimits (std::vector& pos_limits) +{ + if (m_coupledJoints.size() != m_couplingSize) return false; + pos_limits[m_coupledJoints[1]].min = pos_limits[m_coupledJoints[1]].min + pos_limits[m_coupledJoints[2]].min; + pos_limits[m_coupledJoints[1]].max = pos_limits[m_coupledJoints[1]].max + pos_limits[m_coupledJoints[2]].max; + return true; +} + yarp::sig::Vector IndexCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref) { yarp::sig::Vector out = pos_ref; @@ -308,6 +329,14 @@ bool MiddleCouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq) return false; } +bool MiddleCouplingHandler::decouplePosLimits (std::vector& pos_limits) +{ + if (m_coupledJoints.size() != m_couplingSize) return false; + pos_limits[m_coupledJoints[1]].min = pos_limits[m_coupledJoints[1]].min + pos_limits[m_coupledJoints[2]].min; + pos_limits[m_coupledJoints[1]].max = pos_limits[m_coupledJoints[1]].max + pos_limits[m_coupledJoints[2]].max; + return true; +} + yarp::sig::Vector MiddleCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref) { yarp::sig::Vector out = pos_ref; @@ -375,6 +404,14 @@ bool PinkyCouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq) return false; } +bool PinkyCouplingHandler::decouplePosLimits (std::vector& pos_limits) +{ + if (m_coupledJoints.size() != m_couplingSize) return false; + pos_limits[m_coupledJoints[0]].min = pos_limits[m_coupledJoints[0]].min + pos_limits[m_coupledJoints[1]].min + pos_limits[m_coupledJoints[2]].min; + pos_limits[m_coupledJoints[0]].max = pos_limits[m_coupledJoints[0]].max + pos_limits[m_coupledJoints[1]].max + pos_limits[m_coupledJoints[2]].max; + return true; +} + yarp::sig::Vector PinkyCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref) { yarp::sig::Vector out = pos_ref; @@ -452,6 +489,11 @@ bool FingersAbductionCouplingHandler::decoupleTrq (yarp::sig::Vector& current_tr return false; } +bool FingersAbductionCouplingHandler::decouplePosLimits (std::vector& pos_limits) // NOT IMPLEMENTED +{ + return false; +} + yarp::sig::Vector FingersAbductionCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref) { yarp::sig::Vector out = pos_ref; @@ -532,6 +574,11 @@ bool CerHandCouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq) return false; } +bool CerHandCouplingHandler::decouplePosLimits (std::vector& pos_limits) // NOT IMPLEMENTED +{ + return false; +} + yarp::sig::Vector CerHandCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref) { yarp::sig::Vector out = pos_ref; From 8d436227cdbc030c9515203c2e3ab64789ab52a2 Mon Sep 17 00:00:00 2001 From: Nicola Piga Date: Wed, 28 Feb 2018 11:20:35 +0100 Subject: [PATCH 3/4] Take into account coupling while evaluating joint limits in GazeboYarpControlBoardDriver::setMinMaxPos() --- plugins/controlboard/src/ControlBoardDriver.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp index e3109db60..0bb3a28bb 100644 --- a/plugins/controlboard/src/ControlBoardDriver.cpp +++ b/plugins/controlboard/src/ControlBoardDriver.cpp @@ -686,6 +686,15 @@ bool GazeboYarpControlBoardDriver::setMinMaxPos() yWarning() << "Missing LIMITS section"; } + // handle coupling + for (size_t cpl_cnt = 0; cpl_cnt < m_coupling_handler.size(); cpl_cnt++) + { + if (m_coupling_handler[cpl_cnt]) + { + m_coupling_handler[cpl_cnt]->decouplePosLimits(m_jointPosLimits); + } + } + return true; } From f55cb7aa4581945254cc1729e443e491aa8f080c Mon Sep 17 00:00:00 2001 From: Nicola Piga Date: Thu, 26 Mar 2020 23:39:41 +0100 Subject: [PATCH 4/4] Update CHANGELOG.md --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index a4a8e99cf..c35f5cf4a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,10 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo ### Added - Add the possibility to create different types of joints with the `linkattacher` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/461). +- Add the possibility to decouple joint limits using method BaseCouplingHandler::decouplePosLimits(), within the 'controlboard' plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/472). + +### Fixed +- Fix evaluation of joints position limits for physically coupled DoFs in method GazeboYarpControlBoardDriver::setMinMaxPos(), within the 'controlboard' plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/472). ## [3.3.0] - 2019-12-13