From c89280295d53279049bb7521cf6b6b3400130f23 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nicol=C3=B2=20Genesio=20=28=E4=BA=8C=E3=82=B3=E3=82=B2?= =?UTF-8?q?=E3=83=8D=29?= Date: Thu, 21 Dec 2023 11:52:12 +0100 Subject: [PATCH] ControlBoardDriver: support use of IJointCoupling devices in gazebo_yarp_controlboard and drop support for hardcoded HandMK5 coupling (#671) Co-authored-by: Silvio Traversaro --- CHANGELOG.md | 7 + CMakeLists.txt | 4 +- .../include/yarp/dev/ControlBoardDriver.h | 9 +- .../yarp/dev/ControlBoardDriverCoupling.h | 57 --- .../controlboard/src/ControlBoardDriver.cpp | 328 +++++++++++------- .../src/ControlBoardDriverControlMode.cpp | 28 +- .../src/ControlBoardDriverCoupling.cpp | 258 -------------- .../src/ControlBoardDriverDeviceDriver.cpp | 16 +- .../src/ControlBoardDriverInteractionMode.cpp | 23 +- .../src/ControlBoardDriverOthers.cpp | 45 ++- 10 files changed, 300 insertions(+), 475 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c6c492a40..fac215bb2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,13 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo ## [Unreleased] +### Added +- The `gazebo_yarp_controlboard` plugin gained support to load at runtime arbitrary couplings specified by a yarp device that exposes the [`yarp::dev::IJointCoupling`](https://github.com/robotology/yarp/blob/v3.9.0/src/libYARP_dev/src/yarp/dev/IJointCoupling.h#L16) interface. The name of the yarp device used to load the coupling is passed via the `device` parameter in the `COUPLING` group. For an example of PR that uses this new feature, check https://github.com/icub-tech-iit/ergocub-software/pull/178 . + +### Removed + +- The support for passing the `icub_hand_mk5` parameter in the `COUPLING` the `gazebo_yarp_controlboard` plugin was removed. This breaks compatibility with models contained in ergocub-software <= 0.6.0, to use those models use a newer version of ergocub-software. + ## [4.9.0] - 2023-10-31 ### Changed diff --git a/CMakeLists.txt b/CMakeLists.txt index 26b539757..f62a564c7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,7 +46,7 @@ if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE) list(APPEND YARP_ADDITIONAL_COMPONENTS_REQUIRED "robotinterface") endif() -find_package(YARP 3.6 REQUIRED COMPONENTS os sig dev math idl_tools ${YARP_ADDITIONAL_COMPONENTS_REQUIRED}) +find_package(YARP 3.9 REQUIRED COMPONENTS os sig dev math idl_tools ${YARP_ADDITIONAL_COMPONENTS_REQUIRED}) find_package(Gazebo REQUIRED) if (Gazebo_VERSION_MAJOR LESS 11.0) message(status "Gazebo version : " ${Gazebo_VERSION_MAJOR}.${Gazebo_VERSION_MINOR}.${Gazebo_VERSION_PATCH}) @@ -60,7 +60,7 @@ if(MSVC) # It is tipically better to use target_include_directories in place of # global include_directories, but in this case it is more compact to use include_directories include_directories(${OGRE_Paging_INCLUDE_DIRS}) - + # Workaround for https://github.com/robotology/gazebo-yarp-plugins/issues/482 add_definitions(-D__TBB_NO_IMPLICIT_LINKAGE=1) endif() diff --git a/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h b/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h index fdd3d880b..d96438855 100644 --- a/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h +++ b/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -372,6 +373,9 @@ class yarp::dev::GazeboYarpControlBoardDriver: std::vector m_jointPosLimits; std::vector m_jointVelLimits; + std::vector m_actuatedAxesPosLimits; + std::vector m_actuatedAxesVelLimits; + /** * The zero position is the position of the GAZEBO joint that will be read as the starting one * i.e. getEncoder(j)=m_zeroPosition+gazebo.getEncoder(j); @@ -410,11 +414,14 @@ class yarp::dev::GazeboYarpControlBoardDriver: //trajectory generator std::vector m_trajectory_generator; - std::vector m_coupling_handler; + BaseCouplingHandler* m_coupling_handler{nullptr}; std::vector m_speed_ramp_handler; std::vector m_velocity_watchdog; std::vector m_velocityControl; + yarp::dev::IJointCoupling* m_ijointcoupling{nullptr}; + yarp::dev::PolyDriver m_coupling_driver; + yarp::sig::Vector m_trajectoryGenerationReferencePosition; /**< reference position for trajectory generation in position mode [Degrees] */ yarp::sig::Vector m_trajectoryGenerationReferenceSpeed; /**< reference speed for trajectory generation in position mode [Degrees/Seconds]*/ diff --git a/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h b/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h index 9e3382bb7..e114f4d9e 100644 --- a/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h +++ b/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h @@ -223,61 +223,4 @@ class HandMk4CouplingHandler : public BaseCouplingHandler std::vector pinkie_lut; }; -class HandMk5CouplingHandler : public BaseCouplingHandler -{ - -public: - HandMk5CouplingHandler (gazebo::physics::Model* model, yarp::sig::VectorOf coupled_joints, std::vector coupled_joint_names, std::vector coupled_joint_limits); - -public: - - bool parseFingerParameters(yarp::os::Bottle& hand_params); - bool decouplePos (yarp::sig::Vector& current_pos); - bool decoupleVel (yarp::sig::Vector& current_vel); - bool decoupleAcc (yarp::sig::Vector& current_acc); - bool decoupleTrq (yarp::sig::Vector& current_trq); - - yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref); - yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback); - yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref); - -private: - /** - * Parameters from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling - */ - struct FingerParameters - { - double L0x; - double L0y; - double q2bias; - double q1off; - double k; - double d; - double l; - double b; - }; - - std::map mFingerParameters; - - /* - * This method implements the law q2 = q2(q1) from - * https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling, - * i.e., the absolute angle of the distal joint q2 with respect to the palm. - * - * The inputs q1 and the return value of the function are in degrees. - */ - double evaluateCoupledJoint(const double& q1, const std::string& finger_name); - - /* - * This method implements the law \frac{\partial{q2}}{\partial{q1}} from - * https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling, - * i.e., the jacobian of the absolute angle of the distal joint q2 measured from the palm, - * with respect to the proximal joint q1. - * - * The input q1 is in degrees. - */ - double evaluateCoupledJointJacobian(const double& q1, const std::string& finger_name); - -}; - #endif //GAZEBOYARP_COUPLING_H diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp index 5c0fba41e..96685ae05 100644 --- a/plugins/controlboard/src/ControlBoardDriver.cpp +++ b/plugins/controlboard/src/ControlBoardDriver.cpp @@ -121,7 +121,7 @@ bool GazeboYarpControlBoardDriver::gazebo_init() m_initTime = true; // Set to initialize the simulation time to Gazebo simTime on the first call to onUpdate(). m_trajectory_generator.resize(m_numberOfJoints, NULL); - m_coupling_handler.clear(); + m_coupling_handler = nullptr; m_speed_ramp_handler.resize(m_numberOfJoints, NULL); m_velocity_watchdog.resize(m_numberOfJoints, NULL); @@ -209,19 +209,19 @@ bool GazeboYarpControlBoardDriver::gazebo_init() yCDebug(GAZEBOCONTROLBOARD) << "Requested couplings:" << coupling_group_bottle.toString(); yCDebug(GAZEBOCONTROLBOARD) << "Size: " << coupling_group_bottle.size(); - for (int cnt=1; cnt coupled_joints; + std::vector coupled_joint_names; + std::vector coupled_joint_limits; + // Only the legacy coupling handler requires this kind of parsing + // In the "new" one is made by the device itself + if (!coupling_bottle->check("device")) { if (coupling_bottle == 0 || (coupling_bottle->size() != 3 && coupling_bottle->size() != 5)) { yCError(GAZEBOCONTROLBOARD) << "Error parsing coupling parameter"; return false; } - //yCDebug(GAZEBOCONTROLBOARD) << "Requested coupling:" << cnt << " / " << coupling_bottle->size(); - //yCDebug(GAZEBOCONTROLBOARD) << "Requested coupling:" << coupling_bottle->toString(); - yarp::sig::VectorOf coupled_joints; - std::vector coupled_joint_names; Bottle* b = coupling_bottle->get(1).asList(); if (b==0 || b->size()==0) { yCError(GAZEBOCONTROLBOARD) << "Error parsing coupling parameter, wrong size of the joints numbers list"; @@ -246,7 +246,6 @@ bool GazeboYarpControlBoardDriver::gazebo_init() } // Fill limits for coupled joints - std::vector coupled_joint_limits; if (coupling_bottle->size() == 5) { // Min limits of coupled joints. @@ -283,90 +282,130 @@ bool GazeboYarpControlBoardDriver::gazebo_init() coupled_joint_limits.push_back(m_jointPosLimits[coupled_joints[i]]); } - if (coupling_bottle->get(0).asString()=="eyes_vergence_control") - { - BaseCouplingHandler* cpl = new EyesCouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); - m_coupling_handler.push_back(cpl); - yCInfo(GAZEBOCONTROLBOARD) << "using eyes_vergence_control"; - } - else if (coupling_bottle->get(0).asString()=="fingers_abduction_control") - { - BaseCouplingHandler* cpl = new FingersAbductionCouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); - m_coupling_handler.push_back(cpl); - yCInfo(GAZEBOCONTROLBOARD) << "using fingers_abduction_control"; - } - else if (coupling_bottle->get(0).asString()=="thumb_control") - { - BaseCouplingHandler* cpl = new ThumbCouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); - m_coupling_handler.push_back(cpl); - yCInfo(GAZEBOCONTROLBOARD) << "using thumb_control"; - } - else if (coupling_bottle->get(0).asString()=="index_control") - { - BaseCouplingHandler* cpl = new IndexCouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); - m_coupling_handler.push_back(cpl); - yCInfo(GAZEBOCONTROLBOARD) << "using index_control"; - } - else if (coupling_bottle->get(0).asString()=="middle_control") + } + + if (coupling_bottle->get(0).asString()=="eyes_vergence_control") + { + BaseCouplingHandler* cpl = new EyesCouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); + m_coupling_handler = cpl; + yCInfo(GAZEBOCONTROLBOARD) << "using eyes_vergence_control"; + } + else if (coupling_bottle->get(0).asString()=="fingers_abduction_control") + { + BaseCouplingHandler* cpl = new FingersAbductionCouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); + m_coupling_handler = cpl; + yCInfo(GAZEBOCONTROLBOARD) << "using fingers_abduction_control"; + } + else if (coupling_bottle->get(0).asString()=="thumb_control") + { + BaseCouplingHandler* cpl = new ThumbCouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); + m_coupling_handler = cpl; + yCInfo(GAZEBOCONTROLBOARD) << "using thumb_control"; + } + else if (coupling_bottle->get(0).asString()=="index_control") + { + BaseCouplingHandler* cpl = new IndexCouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); + m_coupling_handler = cpl; + yCInfo(GAZEBOCONTROLBOARD) << "using index_control"; + } + else if (coupling_bottle->get(0).asString()=="middle_control") + { + BaseCouplingHandler* cpl = new MiddleCouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); + m_coupling_handler = cpl; + yCInfo(GAZEBOCONTROLBOARD) << "using middle_control"; + } + else if (coupling_bottle->get(0).asString()=="pinky_control") + { + BaseCouplingHandler* cpl = new PinkyCouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); + m_coupling_handler = cpl; + yCInfo(GAZEBOCONTROLBOARD) << "using pinky_control"; + } + else if (coupling_bottle->get(0).asString()=="cer_hand") + { + BaseCouplingHandler* cpl = new CerHandCouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); + m_coupling_handler = cpl; + yCInfo(GAZEBOCONTROLBOARD) << "using cer_hand_control"; + } + else if (coupling_bottle->get(0).asString()=="icub_hand_mk3") + { + BaseCouplingHandler* cpl = new HandMk3CouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); + m_coupling_handler = cpl; + yCInfo(GAZEBOCONTROLBOARD) << "using icub_hand_mk3"; + } + else if (coupling_bottle->get(0).asString()=="icub_left_hand_mk4") + { + BaseCouplingHandler* cpl = new HandMk4CouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); + m_coupling_handler = cpl; + yCInfo(GAZEBOCONTROLBOARD) << "using icub_left_hand_mk4"; + } + else if (coupling_bottle->check("device") && coupling_bottle->find("device").asString()=="couplingXCubHandMk5") + { + yarp::os::Property coupling_handler_conf; + + coupling_handler_conf.fromString(m_pluginParameters.toString()); + coupling_handler_conf.put("device", "couplingXCubHandMk5"); + + if(!m_coupling_driver.open(coupling_handler_conf)) { - BaseCouplingHandler* cpl = new MiddleCouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); - m_coupling_handler.push_back(cpl); - yCInfo(GAZEBOCONTROLBOARD) << "using middle_control"; + yCError(GAZEBOCONTROLBOARD) << "Failed to open coupling device"; + yCDebug(GAZEBOCONTROLBOARD) << "CONF:" << coupling_handler_conf.toString(); + return false; } - else if (coupling_bottle->get(0).asString()=="pinky_control") + + if(!m_coupling_driver.view(m_ijointcoupling)) { - BaseCouplingHandler* cpl = new PinkyCouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); - m_coupling_handler.push_back(cpl); - yCInfo(GAZEBOCONTROLBOARD) << "using pinky_control"; + yCError(GAZEBOCONTROLBOARD) << "Failed to view IJointCoupling interface"; + return false; } - else if (coupling_bottle->get(0).asString()=="cer_hand") + // TODO This part should be common between different coupling handlers + size_t nrOfActuatedAxes{0}; + bool ok = m_ijointcoupling->getNrOfActuatedAxes(nrOfActuatedAxes); + if(!ok) { - BaseCouplingHandler* cpl = new CerHandCouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); - m_coupling_handler.push_back(cpl); - yCInfo(GAZEBOCONTROLBOARD) << "using cer_hand_control"; + yCError(GAZEBOCONTROLBOARD) << "Failed to get number of actuated axes"; + return false; } - else if (coupling_bottle->get(0).asString()=="icub_hand_mk3") + m_actuatedAxesPosLimits.resize(nrOfActuatedAxes); + // TODO this should be done in a better way + m_actuatedAxesVelLimits = m_jointVelLimits; + yarp::os::Bottle& actuated_axis_pos_limit_min = coupling_group_bottle.findGroup("actuatedAxesPosMin"); + if (!actuated_axis_pos_limit_min.isNull() && static_cast(actuated_axis_pos_limit_min.size()) == nrOfActuatedAxes+1) { - BaseCouplingHandler* cpl = new HandMk3CouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); - m_coupling_handler.push_back(cpl); - yCInfo(GAZEBOCONTROLBOARD) << "using icub_hand_mk3"; + for(size_t i = 0; i < m_actuatedAxesPosLimits.size(); ++i) + { + m_actuatedAxesPosLimits[i].min = actuated_axis_pos_limit_min.get(i+1).asFloat64(); + } } - else if (coupling_bottle->get(0).asString()=="icub_left_hand_mk4") + else { - BaseCouplingHandler* cpl = new HandMk4CouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); - m_coupling_handler.push_back(cpl); - yCInfo(GAZEBOCONTROLBOARD) << "using icub_left_hand_mk4"; + yCError(GAZEBOCONTROLBOARD) << "Failed to get actuated axes min limits"; + return false; } - else if (coupling_bottle->get(0).asString()=="icub_hand_mk5") + yarp::os::Bottle& actuated_axis_pos_limit_max = coupling_group_bottle.findGroup("actuatedAxesPosMax"); + if (!actuated_axis_pos_limit_max.isNull() && static_cast(actuated_axis_pos_limit_max.size()) == nrOfActuatedAxes+1) { - yarp::os::Bottle& coupling_group_bottle = m_pluginParameters.findGroup("COUPLING_PARAMS"); - - if(coupling_group_bottle.isNull()) + for(size_t i = 0; i < m_actuatedAxesPosLimits.size(); ++i) { - yCError(GAZEBOCONTROLBOARD) << "Missing param in COUPLING_PARAMS section"; - return false; - } - - BaseCouplingHandler* cpl = new HandMk5CouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits); - if (! static_cast(cpl)->parseFingerParameters(coupling_group_bottle)) - { - yCError(GAZEBOCONTROLBOARD) << "Error parsing coupling parameter, wrong size of the finger parameters list"; - return false; + m_actuatedAxesPosLimits[i].max = actuated_axis_pos_limit_max.get(i+1).asFloat64(); } - m_coupling_handler.push_back(cpl); - yCInfo(GAZEBOCONTROLBOARD) << "using icub_hand_mk5"; - } - else if (coupling_bottle->get(0).asString()=="none") - { - yCDebug(GAZEBOCONTROLBOARD) << "Just for test"; } else { - yCError(GAZEBOCONTROLBOARD) << "Unknown coupling type"; + yCError(GAZEBOCONTROLBOARD) << "Failed to get actuated axes min limits"; return false; } - + yCInfo(GAZEBOCONTROLBOARD) << "using coupling_xcub_hand_mk5"; } + else if (coupling_bottle->get(0).asString()=="none") + { + yCDebug(GAZEBOCONTROLBOARD) << "Just for test"; + } + else + { + yCError(GAZEBOCONTROLBOARD) << "Unknown coupling type"; + return false; + } + } if (!setMinMaxVel()) @@ -438,7 +477,7 @@ void GazeboYarpControlBoardDriver::resetPositionsAndTrajectoryGenerators() yarp::sig::Vector initial_config(m_numberOfJoints); unsigned int counter = 1; while (ss >> tmp) { - if(counter > m_numberOfJoints) { + if(counter > m_numberOfJoints || isValidUserDOF(counter-1) == false) { yCError(GAZEBOCONTROLBOARD)<<"Too many element in initial configuration, stopping at element "<Position(0); - initial_positions.push_back(convertGazeboToUser(i, gazeboPos)); + initialPositionPhys.push_back(convertGazeboToUser(i, gazeboPos)); } - 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]->decouplePos(initial_positions); + if(m_ijointcoupling){ + // TODO check if this is correct + size_t nrOfActuatedAxes{0}; + bool ok = m_ijointcoupling->getNrOfActuatedAxes(nrOfActuatedAxes); + initialPositionAct.resize(nrOfActuatedAxes); + ok &= m_ijointcoupling->convertFromPhysicalJointsToActuatedAxesPos(initialPositionPhys, initialPositionAct); + if (!ok) + { + yCError(GAZEBOCONTROLBOARD) << "Failed to convert from physical joints to actuated axes"; + return; + } + } + else { + initialPositionAct = initialPositionPhys; + if (m_coupling_handler) + m_coupling_handler->decouplePos(initialPositionAct); } for (unsigned int i = 0; i < m_numberOfJoints; ++i) { if (isValidUserDOF(i)) { - double limit_min, limit_max; getUserDOFLimit(i, limit_min, limit_max); m_trajectory_generator[i]->setLimits(limit_min, limit_max); - m_trajectory_generator[i]->initTrajectory(initial_positions[i], - initial_positions[i], + m_trajectory_generator[i]->initTrajectory(initialPositionAct[i], + initialPositionAct[i], m_trajectoryGenerationReferenceSpeed[i], m_trajectoryGenerationReferenceAcceleration[i]); } @@ -561,8 +612,8 @@ void GazeboYarpControlBoardDriver::onUpdate(const gazebo::common::UpdateInfo& _i for (size_t jnt_cnt = 0; jnt_cnt < m_jointPointers.size(); jnt_cnt++) { double gazeboPos = m_jointPointers[jnt_cnt]->Position(0); - m_positions[jnt_cnt] = convertGazeboToUser(jnt_cnt, gazeboPos); - m_velocities[jnt_cnt] = convertGazeboToUser(jnt_cnt, m_jointPointers[jnt_cnt]->GetVelocity(0)); + m_motPositions[jnt_cnt] = convertGazeboToUser(jnt_cnt, gazeboPos); + m_motVelocities[jnt_cnt] = convertGazeboToUser(jnt_cnt, m_jointPointers[jnt_cnt]->GetVelocity(0)); if (!m_useVirtualAnalogSensor) { m_torques[jnt_cnt] = m_jointPointers[jnt_cnt]->GetForce(0u); @@ -573,17 +624,26 @@ void GazeboYarpControlBoardDriver::onUpdate(const gazebo::common::UpdateInfo& _i } } - m_motPositions=m_positions; - m_motVelocities=m_velocities; + //measurements decoupling - for (size_t cpl_cnt = 0; cpl_cnt < m_coupling_handler.size(); cpl_cnt++) - { - if (m_coupling_handler[cpl_cnt]) + if(m_ijointcoupling){ + // TODO check if this is correct + size_t nrOfActuatedAxes{0}; + bool ok = m_ijointcoupling->getNrOfActuatedAxes(nrOfActuatedAxes); + m_positions.resize(nrOfActuatedAxes); + m_velocities.resize(nrOfActuatedAxes); + ok &= m_ijointcoupling->convertFromPhysicalJointsToActuatedAxesPos(m_motPositions, m_positions); + ok &= m_ijointcoupling->convertFromPhysicalJointsToActuatedAxesVel(m_motPositions, m_motVelocities, m_velocities); + } + else { + m_positions = m_motPositions; + m_velocities = m_motVelocities; + if (m_coupling_handler) { - m_coupling_handler[cpl_cnt]->decouplePos(m_positions); - m_coupling_handler[cpl_cnt]->decoupleVel(m_velocities); - //m_coupling_handler[cpl_cnt]->decoupleAcc(m_accelerations); //missing - m_coupling_handler[cpl_cnt]->decoupleTrq(m_torques); + m_coupling_handler->decouplePos(m_positions); + m_coupling_handler->decoupleVel(m_velocities); + //m_coupling_handler->decoupleAcc(m_accelerations); //missing + m_coupling_handler->decoupleTrq(m_torques); } } @@ -616,51 +676,63 @@ void GazeboYarpControlBoardDriver::onUpdate(const gazebo::common::UpdateInfo& _i //update Trajectories for (size_t j = 0; j < m_numberOfJoints; ++j) { - if (m_controlMode[j] == VOCAB_CM_POSITION) - { - if (m_clock % _T_controller == 0) + if (isValidUserDOF(j)) { + if (m_controlMode[j] == VOCAB_CM_POSITION) { - m_jntReferencePositions[j] = m_trajectory_generator[j]->computeTrajectory(); - m_isMotionDone[j] = m_trajectory_generator[j]->isMotionDone(); + if (m_clock % _T_controller == 0) + { + m_jntReferencePositions[j] = m_trajectory_generator[j]->computeTrajectory(); + m_isMotionDone[j] = m_trajectory_generator[j]->isMotionDone(); + } } - } - else if (m_controlMode[j] == VOCAB_CM_MIXED) - { - if (m_clock % _T_controller == 0) + else if (m_controlMode[j] == VOCAB_CM_MIXED) { - double computed_ref_speed = m_speed_ramp_handler[j]->getCurrentValue()*stepTime.Double(); //controller period - double computed_ref_pos = m_jntReferencePositions[j] + m_trajectory_generator[j]->computeTrajectoryStep(); - m_jntReferencePositions[j] = computed_ref_pos + computed_ref_speed; - //yCDebug(GAZEBOCONTROLBOARD) << computed_ref_pos << " " << computed_ref_speed; - m_isMotionDone[j] = m_trajectory_generator[j]->isMotionDone(); + if (m_clock % _T_controller == 0) + { + double computed_ref_speed = m_speed_ramp_handler[j]->getCurrentValue()*stepTime.Double(); //controller period + double computed_ref_pos = m_jntReferencePositions[j] + m_trajectory_generator[j]->computeTrajectoryStep(); + m_jntReferencePositions[j] = computed_ref_pos + computed_ref_speed; + //yCDebug(GAZEBOCONTROLBOARD) << computed_ref_pos << " " << computed_ref_speed; + m_isMotionDone[j] = m_trajectory_generator[j]->isMotionDone(); + } } - } - else if (m_controlMode[j] == VOCAB_CM_VELOCITY) - { - if (m_clock % _T_controller == 0) + else if (m_controlMode[j] == VOCAB_CM_VELOCITY) { - if (m_speed_ramp_handler[j]) m_jntReferenceVelocities[j] = m_speed_ramp_handler[j]->getCurrentValue(); - } + if (m_clock % _T_controller == 0) + { + if (m_speed_ramp_handler[j]) m_jntReferenceVelocities[j] = m_speed_ramp_handler[j]->getCurrentValue(); + } - if (m_velocity_control_type == IntegratorAndPositionPID) - { - // All quantities are in degrees for revolute, and meters for linear - m_jntReferencePositions[j] = m_jntReferencePositions[j] + m_speed_ramp_handler[j]->getCurrentValue()*stepTime.Double(); + if (m_velocity_control_type == IntegratorAndPositionPID) + { + // All quantities are in degrees for revolute, and meters for linear + m_jntReferencePositions[j] = m_jntReferencePositions[j] + m_speed_ramp_handler[j]->getCurrentValue()*stepTime.Double(); + } } } } - //references decoupling - m_motReferencePositions=m_jntReferencePositions; - m_motReferenceVelocities=m_jntReferenceVelocities; - m_motReferenceTorques=m_jntReferenceTorques; - for (size_t cpl_cnt = 0; cpl_cnt < m_coupling_handler.size(); cpl_cnt++) - { - if (m_coupling_handler[cpl_cnt]) + + + if(m_ijointcoupling){ + // TODO check if it is correct + size_t nrOfActuatedAxes{0}; + bool ok = m_ijointcoupling->getNrOfActuatedAxes(nrOfActuatedAxes); + m_jntReferencePositions.resize(nrOfActuatedAxes); + m_jntReferenceVelocities.resize(nrOfActuatedAxes); + ok &= m_ijointcoupling->convertFromActuatedAxesToPhysicalJointsPos(m_jntReferencePositions, m_motReferencePositions); + ok &= m_ijointcoupling->convertFromActuatedAxesToPhysicalJointsVel(m_positions, m_jntReferenceVelocities, m_motReferenceVelocities); + } + else { + //references decoupling + m_motReferencePositions=m_jntReferencePositions; + m_motReferenceVelocities=m_jntReferenceVelocities; + m_motReferenceTorques=m_jntReferenceTorques; + if (m_coupling_handler) { - m_motReferencePositions = m_coupling_handler[cpl_cnt]->decoupleRefPos(m_jntReferencePositions); - m_motReferenceVelocities = m_coupling_handler[cpl_cnt]->decoupleRefVel(m_jntReferenceVelocities, m_motPositions); - m_motReferenceTorques = m_coupling_handler[cpl_cnt]->decoupleRefTrq(m_jntReferenceTorques); + m_motReferencePositions = m_coupling_handler->decoupleRefPos(m_jntReferencePositions); + m_motReferenceVelocities = m_coupling_handler->decoupleRefVel(m_jntReferenceVelocities, m_motPositions); + m_motReferenceTorques = m_coupling_handler->decoupleRefTrq(m_jntReferenceTorques); } } diff --git a/plugins/controlboard/src/ControlBoardDriverControlMode.cpp b/plugins/controlboard/src/ControlBoardDriverControlMode.cpp index 149e7a12d..f451f2377 100644 --- a/plugins/controlboard/src/ControlBoardDriverControlMode.cpp +++ b/plugins/controlboard/src/ControlBoardDriverControlMode.cpp @@ -72,18 +72,38 @@ bool GazeboYarpControlBoardDriver::setControlMode(const int j, const int mode) return false; } - for (size_t cpl_i = 0; cpl_i < m_coupling_handler.size(); ++cpl_i) - { - if (m_coupling_handler[cpl_i] && m_coupling_handler[cpl_i]->checkJointIsCoupled(j)) + if (m_ijointcoupling) { + // TODO + yarp::sig::VectorOf coupled_actuated_axes; + bool ok = m_ijointcoupling->getCoupledActuatedAxes(coupled_actuated_axes); + if(!ok) { + yCError(GAZEBOCONTROLBOARD) << "error while getting coupled actuated axes"; + return false; + } + bool joint_is_coupled = find(coupled_actuated_axes.begin(), coupled_actuated_axes.end(), j) != coupled_actuated_axes.end(); + if(joint_is_coupled) { + for (size_t coupled_j = 0; coupled_j < coupled_actuated_axes.size(); ++coupled_j) + { + changeControlMode(coupled_actuated_axes[coupled_j], mode); + } + return true; + } + + } + else{ + if (m_coupling_handler && m_coupling_handler->checkJointIsCoupled(j)) { - yarp::sig::VectorOf coupling_vector = m_coupling_handler[cpl_i]->getCoupledJoints(); + yarp::sig::VectorOf coupling_vector = m_coupling_handler->getCoupledJoints(); for (size_t coupled_j = 0; coupled_j < coupling_vector.size(); ++coupled_j) { changeControlMode(coupling_vector[coupled_j], mode); } return true; } + } + + changeControlMode(j,mode); return true; } diff --git a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp index 0ae704914..c4a06f60c 100644 --- a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp +++ b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp @@ -1129,261 +1129,3 @@ yarp::sig::Vector HandMk4CouplingHandler::decoupleRefTrq (yarp::sig::Vector& trq out[m_coupledJoints[12]] = trq_ref[m_coupledJoints[6]] - out[m_coupledJoints[11]]; return out; } - -//------------------------------------------------------------------------------------------------------------------ -// HandMk5CouplingHandler -//------------------------------------------------------------------------------------------------------------------ - -HandMk5CouplingHandler::HandMk5CouplingHandler(gazebo::physics::Model* model, yarp::sig::VectorOf coupled_joints, std::vector coupled_joint_names, std::vector coupled_joint_limits) -: BaseCouplingHandler(model, coupled_joints,coupled_joint_names, coupled_joint_limits) -{ - m_couplingSize = 12; -} - -bool HandMk5CouplingHandler::parseFingerParameters(yarp::os::Bottle& hand_params) -{ - auto L0x = hand_params.findGroup("L0x"); - auto L0y = hand_params.findGroup("L0y"); - auto q2bias = hand_params.findGroup("q2bias"); - auto q1off = hand_params.findGroup("q1off"); - auto k = hand_params.findGroup("k"); - auto d = hand_params.findGroup("d"); - auto l = hand_params.findGroup("l"); - auto b = hand_params.findGroup("b"); - - constexpr int nFingers = 5; - // All the +1 is because the first element of the bottle is the name of the group - if(L0x.size()!=nFingers+1 || L0y.size()!=nFingers+1 || q2bias.size()!=nFingers+1 || - q1off.size()!=nFingers+1 || k.size()!=nFingers+1 || d.size()!=nFingers+1 || - l.size()!=nFingers+1 || b.size()!=nFingers+1 ) - { - yError()<<"HandMk5CouplingHandler: invalid hand parameters, check your configuration file"; - return false; - } - - - const std::array names = {"thumb", "index", "middle", "ring", "pinky"}; - for (std::size_t i = 0; i < names.size(); i++) - { - mFingerParameters.insert({names.at(i), {L0x.get(i+1).asFloat32(), L0y.get(i+1).asFloat32(), q2bias.get(i+1).asFloat32(), - q1off.get(i+1).asFloat32(), k.get(i+1).asFloat32(), d.get(i+1).asFloat32(), - l.get(i+1).asFloat32(), b.get(i+1).asFloat32()}}); - } - - return true; -} - -bool HandMk5CouplingHandler::decouplePos (yarp::sig::Vector& current_pos) -{ - if (m_coupledJoints.size()!=m_couplingSize) return false; - - const yarp::sig::Vector tmp = current_pos; - - /* thumb_add <-- thumb_add */ - current_pos[m_coupledJoints[0]] = tmp[0]; - /* thumb_oc <-- thumb_prox */ - current_pos[m_coupledJoints[1]] = tmp[1]; - /* index_add <-- index_add */ - current_pos[m_coupledJoints[2]] = tmp[3]; - /* index_oc <-- index_prox */ - current_pos[m_coupledJoints[3]] = tmp[4]; - /* middle_oc <-- middle_prox */ - current_pos[m_coupledJoints[4]] = tmp[6]; - /** - * ring_pinky_oc <-- pinkie_prox - * as, on the real robot, the coupled group composed of ring_prox, ring_dist, pinkie_prox and pinkie_dist - * is controlled using the encoder on the pinkie_prox as feedback - */ - current_pos[m_coupledJoints[5]] = tmp[10]; - - return true; -} - -bool HandMk5CouplingHandler::decoupleVel (yarp::sig::Vector& current_vel) -{ - if (m_coupledJoints.size()!=m_couplingSize) return false; - - const yarp::sig::Vector tmp = current_vel; - - /* thumb_add <-- thumb_add */ - current_vel[m_coupledJoints[0]] = tmp[0]; - /* thumb_oc <-- thumb_prox */ - current_vel[m_coupledJoints[1]] = tmp[1]; - /* index_add <-- index_add */ - current_vel[m_coupledJoints[2]] = tmp[3]; - /* index_oc <-- index_prox */ - current_vel[m_coupledJoints[3]] = tmp[4]; - /* middle_oc <-- middle_prox */ - current_vel[m_coupledJoints[4]] = tmp[6]; - /** - * ring_pinky_oc <-- pinkie_prox - * as, on the real robot, the coupled group composed of ring_prox, ring_dist, pinkie_prox and pinkie_dist - * is controlled using the encoder on the pinkie_prox as feedback - */ - current_vel[m_coupledJoints[5]] = tmp[10]; - - return true; -} - -bool HandMk5CouplingHandler::decoupleAcc (yarp::sig::Vector& current_acc) -{ - /** - * Acceleration control not available for fingers on the real robot. - * Note: this method is never called within the controlboard plugin code. - */ - return false; -} - -bool HandMk5CouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq) -{ - /** - * Torque control not available for fingers on the real robot. - */ - return false; -} - -yarp::sig::Vector HandMk5CouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref) -{ - if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "HandMk5CouplingHandler: Invalid coupling vector"; return pos_ref;} - - yarp::sig::Vector out(pos_ref.size()); - - /* thumb_add <-- thumb_add */ - out[0] = pos_ref[m_coupledJoints[0]]; - /* thumb_prox <-- thumb_oc */ - out[1] = pos_ref[m_coupledJoints[1]]; - /* thumb_dist <-- coupling_law(thumb_prox) */ - out[2] = evaluateCoupledJoint(out[1], "thumb"); - /* index_add <-- index_add */ - out[3] = pos_ref[m_coupledJoints[2]]; - /* index_prox <-- index_oc */ - out[4] = pos_ref[m_coupledJoints[3]]; - /* index_dist <-- coupling_law(index_prox) */ - out[5] = evaluateCoupledJoint(out[4], "index"); - /* middle_prox <-- middle_oc */ - out[6] = pos_ref[m_coupledJoints[4]]; - /* middle_dist <-- coupling_law(middle_prox) */ - out[7] = evaluateCoupledJoint(out[6], "middle"); - /* ring_prox <-- ring_pinky_oc */ - out[8] = pos_ref[m_coupledJoints[5]]; - /* ring_dist <-- coupling_law(ring_prox) */ - out[9] = evaluateCoupledJoint(out[8], "ring"); - /* pinky_prox <-- ring_pinky_oc */ - out[10] = pos_ref[m_coupledJoints[5]]; - /* pinky_dist <-- coupling_law(pinky_prox) */ - out[11] = evaluateCoupledJoint(out[10], "pinky"); - - return out; -} - - -yarp::sig::Vector HandMk5CouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) -{ - if (m_coupledJoints.size()!=m_couplingSize) {yCError(GAZEBOCONTROLBOARD) << "HandMk5CouplingHandler: Invalid coupling vector"; return vel_ref;} - - /** - * Extract the current position of proximal joints from pos_feedback. - */ - double lastThumbProx = pos_feedback[1]; - double lastIndexProx = pos_feedback[4]; - double lastMiddleProx = pos_feedback[6]; - double lastRingProx = pos_feedback[8]; - double lastPinkyProx = pos_feedback[10]; - - /** - * In the following, we use the fact that: - * /dot{distal_joint} = \partial{distal_joint}{proximal_joint} \dot{proximal_joint}. - */ - - yarp::sig::Vector out(vel_ref.size()); - - /* thumb_add <-- thumb_add */ - out[0] = vel_ref[m_coupledJoints[0]]; - /* thumb_prox <-- thumb_oc */ - out[1] = vel_ref[m_coupledJoints[1]]; - /* thumb_dist <-- coupling_law_jacobian(thumb_prox_position) * thumb_prox */ - out[2] = evaluateCoupledJointJacobian(lastThumbProx, "thumb") * out[1]; - /* index_add <-- index_add */ - out[3] = vel_ref[m_coupledJoints[2]]; - /* index_prox <-- index_oc */ - out[4] = vel_ref[m_coupledJoints[3]]; - /* index_dist <-- coupling_law_jacobian(index_prox_position) * index_prox */ - out[5] = evaluateCoupledJointJacobian(lastIndexProx, "index") * out[4]; - /* middle_prox <-- middle_oc */ - out[6] = vel_ref[m_coupledJoints[4]]; - /* middle_dist <-- coupling_law_jacobian(middle_prox_position) * middle_prox */ - out[7] = evaluateCoupledJointJacobian(lastMiddleProx, "middle") * out[6]; - /* ring_prox <-- ring_pinky_oc */ - out[8] = vel_ref[m_coupledJoints[5]]; - /* ring_dist <-- coupling_law_jacobian(ring_prox_position) * ring_prox */ - out[9] = evaluateCoupledJointJacobian(lastRingProx, "ring") * out[8]; - /* pinky_prox <-- ring_pinky_oc */ - out[10] = vel_ref[m_coupledJoints[5]]; - /* pinky_dist <-- coupling_law(pinky_prox) */ - out[11] = evaluateCoupledJointJacobian(lastPinkyProx, "pinky") * out[10]; - - return out; -} - -yarp::sig::Vector HandMk5CouplingHandler::decoupleRefTrq (yarp::sig::Vector& trq_ref) -{ - /** - * Torque control not available for fingers on the real robot. - */ - return trq_ref; -} - -double HandMk5CouplingHandler::evaluateCoupledJoint(const double& q1, const std::string& finger_name) -{ - /** - * Coupling law taken from from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling - */ - auto params = mFingerParameters.at(finger_name); - double q1_rad = q1 * M_PI / 180.0; - double q1off_rad = params.q1off * M_PI / 180.0; - double q2bias_rad = params.q2bias * M_PI / 180.0; - - double P1x_q1 = params.d * cos(q1_rad + q1off_rad); - double P1y_q1 = params.d * sin(q1_rad + q1off_rad); - - double h_sq = std::pow(P1x_q1 - params.L0x, 2) + std::pow(P1y_q1 - params.L0y, 2); - double h = std::sqrt(h_sq); - double l_sq = std::pow(params.l, 2); - double k_sq = std::pow(params.k, 2); - - double q2 = atan2(P1y_q1 - params.L0y, P1x_q1 - params.L0x) + \ - acos((h_sq + l_sq - k_sq) / (2.0 * params.l * h)) + \ - -q2bias_rad - M_PI; - - // The value of q1 is subtracted from the result as the formula provides - // the absolute angle of the coupled distal joint with respect to the palm. - return q2 * 180.0 / M_PI - q1; -} - -double HandMk5CouplingHandler::evaluateCoupledJointJacobian(const double& q1, const std::string& finger_name) -{ - /** - * Coupling law jacobian taken from from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling - */ - auto params = mFingerParameters.at(finger_name); - double q1_rad = q1 * M_PI / 180.0; - double q1off_rad = params.q1off * M_PI / 180.0; - - double P1x_q1 = params.d * cos(q1_rad + q1off_rad); - double P1y_q1 = params.d * sin(q1_rad + q1off_rad); - - double h_sq = std::pow(P1x_q1 - params.L0x, 2) + std::pow(P1y_q1 - params.L0y, 2); - double h = std::sqrt(h_sq); - double l_sq = std::pow(params.l, 2); - double k_sq = std::pow(params.k, 2); - - double dq2_dq1_11 = 1; - double dq2_dq1_21 = 2 - (std::pow(params.d, 2) - std::pow(params.b, 2)) / (std::pow(params.d, 2) - (params.L0x * P1x_q1 + params.L0y * P1y_q1)); - double dq2_dq1_12 = (params.L0x * P1y_q1 - params.L0y * P1x_q1) * (l_sq - k_sq - h_sq); - double dq2_dq1_22 = 2 * params.l * h * h_sq * std::sqrt(1 - std::pow((l_sq - k_sq + h_sq) / (2 * params.l * h), 2)); - double dq2_dq1 = dq2_dq1_11 / dq2_dq1_21 + dq2_dq1_12 / dq2_dq1_22; - - // The value of 1 is subtracted from the result as evaluateCoupledJointJacobian provides - // the jacobian of the absolute angle of the coupled distal joint. - return dq2_dq1 - 1; -} diff --git a/plugins/controlboard/src/ControlBoardDriverDeviceDriver.cpp b/plugins/controlboard/src/ControlBoardDriverDeviceDriver.cpp index 93b750606..cff759c3a 100644 --- a/plugins/controlboard/src/ControlBoardDriverDeviceDriver.cpp +++ b/plugins/controlboard/src/ControlBoardDriverDeviceDriver.cpp @@ -48,13 +48,10 @@ bool GazeboYarpControlBoardDriver::close() } } - for (size_t i = 0; i < m_coupling_handler.size(); i++) + if (m_coupling_handler) { - if (m_coupling_handler[i]) - { - delete m_coupling_handler[i]; - m_coupling_handler[i] = 0; - } + delete m_coupling_handler; + m_coupling_handler = nullptr; } for (size_t i = 0; i < m_speed_ramp_handler.size(); i++) @@ -65,6 +62,11 @@ bool GazeboYarpControlBoardDriver::close() m_speed_ramp_handler[i] = 0; } } - + + if (m_coupling_driver.isValid()) + { + m_coupling_driver.close(); + } + return true; } diff --git a/plugins/controlboard/src/ControlBoardDriverInteractionMode.cpp b/plugins/controlboard/src/ControlBoardDriverInteractionMode.cpp index 356b2ae59..0494f60a5 100644 --- a/plugins/controlboard/src/ControlBoardDriverInteractionMode.cpp +++ b/plugins/controlboard/src/ControlBoardDriverInteractionMode.cpp @@ -100,12 +100,26 @@ bool GazeboYarpControlBoardDriver::setInteractionMode(int j, yarp::dev::Interact { if (j < 0 || static_cast(j) >= m_numberOfJoints) return false; + if (m_ijointcoupling) { + yarp::sig::VectorOf coupled_actuated_axes; + bool ok = m_ijointcoupling->getCoupledActuatedAxes(coupled_actuated_axes); + if(!ok) { + return false; + } + bool joint_is_coupled = find(coupled_actuated_axes.begin(), coupled_actuated_axes.end(), j) != coupled_actuated_axes.end(); + if(joint_is_coupled) { + for (size_t coupled_j = 0; coupled_j < coupled_actuated_axes.size(); ++coupled_j) + { + changeInteractionMode(coupled_actuated_axes[coupled_j], mode); + } + return true; + } - for (size_t cpl_i=0; cpl_i < m_coupling_handler.size(); cpl_i++) - { - if (m_coupling_handler[cpl_i] && m_coupling_handler[cpl_i]->checkJointIsCoupled(j)) + } + else { + if (m_coupling_handler && m_coupling_handler->checkJointIsCoupled(j)) { - yarp::sig::VectorOf coupling_vector = m_coupling_handler[cpl_i]->getCoupledJoints(); + yarp::sig::VectorOf coupling_vector = m_coupling_handler->getCoupledJoints(); for (size_t coupled_j=0; coupled_j < coupling_vector.size(); coupled_j++) { changeInteractionMode(coupling_vector[coupled_j], mode); @@ -113,6 +127,7 @@ bool GazeboYarpControlBoardDriver::setInteractionMode(int j, yarp::dev::Interact return true; } } + changeInteractionMode(j,mode); return true; diff --git a/plugins/controlboard/src/ControlBoardDriverOthers.cpp b/plugins/controlboard/src/ControlBoardDriverOthers.cpp index 782076dad..17a24cb66 100644 --- a/plugins/controlboard/src/ControlBoardDriverOthers.cpp +++ b/plugins/controlboard/src/ControlBoardDriverOthers.cpp @@ -14,16 +14,19 @@ bool GazeboYarpControlBoardDriver::getAxisName(int axis, std::string& name) { if (axis < 0 || static_cast(axis) >= m_numberOfJoints) return false; - for (unsigned int cpl_cnt = 0; cpl_cnt < m_coupling_handler.size(); cpl_cnt++) - { - if (m_coupling_handler[cpl_cnt]) - { - if (m_coupling_handler[cpl_cnt]->checkJointIsCoupled(axis)) + if (m_ijointcoupling) { + bool ok = m_ijointcoupling->getActuatedAxisName(axis, name); + return ok; + } + else { + if (m_coupling_handler) { - name = m_coupling_handler[cpl_cnt]->getCoupledJointName(axis); - return true; + if (m_coupling_handler->checkJointIsCoupled(axis)) + { + name = m_coupling_handler->getCoupledJointName(axis); + return true; + } } - } } name = std::string(controlboard_joint_names.at(axis)); return true; @@ -140,10 +143,15 @@ bool GazeboYarpControlBoardDriver::calibrationDone(int j) // NOT IMPLEMENTED bool GazeboYarpControlBoardDriver::isValidUserDOF(int joint_index) { - if (m_coupling_handler.size() > 0) + if(m_ijointcoupling) { + size_t nrOfActuatedAxes{0}; + m_ijointcoupling->getNrOfActuatedAxes(nrOfActuatedAxes); + return joint_index < nrOfActuatedAxes; + } + else if (m_coupling_handler) { // Only the case of 1 coupling handler is supported - const std::string coupled_joint_name = m_coupling_handler[0]->getCoupledJointName(joint_index); + const std::string coupled_joint_name = m_coupling_handler->getCoupledJointName(joint_index); // The joint *is not* part of the coupled group // hence it is a normal and *valid* jont from the user point of view @@ -161,13 +169,18 @@ bool GazeboYarpControlBoardDriver::isValidUserDOF(int joint_index) void GazeboYarpControlBoardDriver::setUserDOFLimit(int joint_index, const double& min, const double& max) { - if (m_coupling_handler.size() > 0 && m_coupling_handler[0]->checkJointIsCoupled(joint_index)) + if (m_ijointcoupling){ + m_actuatedAxesPosLimits[joint_index].max = max; + m_actuatedAxesPosLimits[joint_index].min = min; + } + else if (m_coupling_handler && m_coupling_handler->checkJointIsCoupled(joint_index)) { // Only the case of 1 coupling handler is supported - m_coupling_handler[0]->setCoupledJointLimit(joint_index, min, max); + m_coupling_handler->setCoupledJointLimit(joint_index, min, max); } else { + m_jointPosLimits[joint_index].max = max; m_jointPosLimits[joint_index].min = min; } @@ -176,9 +189,13 @@ void GazeboYarpControlBoardDriver::setUserDOFLimit(int joint_index, const double void GazeboYarpControlBoardDriver::getUserDOFLimit(int joint_index, double& min, double& max) { // Only the case of 1 coupling handler is supported - if (m_coupling_handler.size() > 0 && m_coupling_handler[0]->checkJointIsCoupled(joint_index)) + if (m_ijointcoupling){ + min = m_actuatedAxesPosLimits[joint_index].min; + max = m_actuatedAxesPosLimits[joint_index].max; + } + else if (m_coupling_handler && m_coupling_handler->checkJointIsCoupled(joint_index)) { - m_coupling_handler[0]->getCoupledJointLimit(joint_index, min, max); + m_coupling_handler->getCoupledJointLimit(joint_index, min, max); } else {