From 75386bb0e778674a9c3471d683e1be44ea43090a Mon Sep 17 00:00:00 2001 From: Lorenzo Moretti <107630048+LoreMoretti@users.noreply.github.com> Date: Mon, 19 Aug 2024 08:53:55 +0200 Subject: [PATCH] Make joint encoder acceleration reading optional (#876) --- CHANGELOG.md | 1 + .../BipedalLocomotion/YarpRobotLoggerDevice.h | 19 +++++----- .../src/YarpRobotLoggerDevice.cpp | 23 ++++++++++-- .../RobotInterface/YarpSensorBridge.h | 4 ++- .../RobotInterface/YarpSensorBridgeImpl.h | 36 ++++++++++++------- .../src/YarpSensorBridge.cpp | 27 +++++++++++++- 6 files changed, 84 insertions(+), 26 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c3c64fdf84..a1476c7ca7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -25,6 +25,7 @@ All notable changes to this project are documented in this file. - Add `setControlModeAsync` function to set motor control mode in an asynchronous process (https://github.com/ami-iit/bipedal-locomotion-framework/pull/860) - Add launch parameter to `blf-logger-with-audio.sh` script to set logger launch file (https://github.com/ami-iit/bipedal-locomotion-framework/pull/867) - Add `getJointLimits` function to set get actuated joints position limits (https://github.com/ami-iit/bipedal-locomotion-framework/pull/868) +- Add the possibility to disable streaming of joint encoder acceleration measurements (https://github.com/ami-iit/bipedal-locomotion-framework/pull/876) ### Changed - 🤖 [ergoCubSN001] Add logging of the wrist and fix the name of the waist imu (https://github.com/ami-iit/bipedal-locomotion-framework/pull/810) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 8ffefbf967..80b5f4595d 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -9,10 +9,10 @@ #include #include #include -#include #include #include #include +#include #include #include @@ -164,6 +164,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, bool m_streamMotorStates{false}; bool m_streamJointStates{false}; + bool m_streamJointAccelerations{true}; bool m_streamMotorPWM{false}; bool m_streamPIDs{false}; bool m_streamInertials{false}; @@ -180,7 +181,9 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, void lookForNewLogs(); void lookForExogenousSignals(); - bool addChannel(const std::string& nameKey, std::size_t vectorSize, const std::vector& metadata = {}); + bool addChannel(const std::string& nameKey, + std::size_t vectorSize, + const std::vector& metadata = {}); bool hasSubstring(const std::string& str, const std::vector& substrings) const; void recordVideo(const std::string& cameraName, VideoWriter& writer); @@ -238,11 +241,11 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, const std::string cartesianWrenchesName = "cartesian_wrenches"; const std::vector cartesianWrenchNames = {ftElementNames[0], - ftElementNames[1], - ftElementNames[2], - ftElementNames[3], - ftElementNames[4], - ftElementNames[5]}; + ftElementNames[1], + ftElementNames[2], + ftElementNames[3], + ftElementNames[4], + ftElementNames[5]}; const std::string temperatureName = "temperatures"; const std::vector temperatureNames = {"temperature"}; @@ -252,10 +255,8 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, const std::string robotDescriptionList = "description_list"; const std::string timestampsName = "timestamps"; - }; } // namespace BipedalLocomotion - #endif // BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_H diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index d274661592..b89cf13ed1 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -559,6 +559,17 @@ bool YarpRobotLoggerDevice::setupRobotSensorBridge( logPrefix); } + if (!ptr->getParameter("stream_joint_accelerations", m_streamJointAccelerations)) + { + log()->info("{} The 'stream_joint_accelerations' parameter is not found. Set to true by " + "default", + logPrefix); + } + if (!m_streamJointAccelerations) + { + log()->info("{} The joint accelerations is not logged", logPrefix); + } + if (!ptr->getParameter("stream_motor_states", m_streamMotorStates)) { log()->info("{} The 'stream_motor_states' parameter is not found. The motor states is not " @@ -750,7 +761,10 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { ok = ok && addChannel(jointStatePositionsName, joints.size(), joints); ok = ok && addChannel(jointStateVelocitiesName, joints.size(), joints); - ok = ok && addChannel(jointStateAccelerationsName, joints.size(), joints); + if (m_streamJointAccelerations) + { + ok = ok && addChannel(jointStateAccelerationsName, joints.size(), joints); + } ok = ok && addChannel(jointStateTorquesName, joints.size(), joints); } if (m_streamMotorStates) @@ -1462,9 +1476,12 @@ void YarpRobotLoggerDevice::run() { logData(jointStateVelocitiesName, m_jointSensorBuffer, time); } - if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) + if (m_streamJointAccelerations) { - logData(jointStateAccelerationsName, m_jointSensorBuffer, time); + if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) + { + logData(jointStateAccelerationsName, m_jointSensorBuffer, time); + } } if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) { diff --git a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h index 4ddf26ea56..0aea843e34 100644 --- a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h +++ b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h @@ -24,7 +24,7 @@ namespace BipedalLocomotion namespace RobotInterface { - +// clang-format off /** * YarpSensorBridge Yarp implementation of the ISensorBridge interface * Currently available interfaces @@ -75,6 +75,8 @@ namespace RobotInterface * |TemperatureSensors | | |Expects the temperature sensors to be opened with TemperatureSensors interface remapped through multiple analog sensors remapper| * | |temperature_sensors_list | vector of strings |list containing the devices opened with TemperatureSensors interface | */ +// clang-format on + class YarpSensorBridge : public ISensorBridge, public BipedalLocomotion::System::Source { diff --git a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h index 6dd353ac3c..990af5d515 100644 --- a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h +++ b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h @@ -181,6 +181,8 @@ struct YarpSensorBridge::Impl required device drivers */ bool checkForNAN{false}; /**< flag to enable search for NANs in the incoming measurement buffers */ + bool streamJointAccelerations{true}; /**< flag to enable reading joint accelerations from + encoders */ using SubConfigLoader = bool (YarpSensorBridge::Impl::*)( std::weak_ptr, @@ -1500,7 +1502,7 @@ struct YarpSensorBridge::Impl bool ok{true}; - ok = ok && readAllJointSensors(checkForNan); + ok = ok && readAllJointSensors(checkForNan, streamJointAccelerations); ok = ok && readAllMotorSensors(checkForNan); ok = ok && readAllPIDs(checkForNan); ok = ok && readAllMotorPWMs(checkForNan); @@ -1519,7 +1521,7 @@ struct YarpSensorBridge::Impl /** * Read joint sensors measurements */ - bool readAllJointSensors(bool checkForNan = false) + bool readAllJointSensors(bool checkForNan = false, bool streamJointAccelerations = true) { constexpr auto logPrefix = "[YarpSensorBridge::Impl::readAllJointSensors]"; @@ -1533,9 +1535,13 @@ struct YarpSensorBridge::Impl ok = ok && controlBoardRemapperInterfaces.encoders->getEncoderSpeeds( controlBoardRemapperMeasures.jointVelocitiesUnordered.data()); - ok = ok - && controlBoardRemapperInterfaces.encoders->getEncoderAccelerations( - controlBoardRemapperMeasures.jointAccelerationsUnordered.data()); + + if (streamJointAccelerations) + { + ok = ok + && controlBoardRemapperInterfaces.encoders->getEncoderAccelerations( + controlBoardRemapperMeasures.jointAccelerationsUnordered.data()); + } if (ok) { @@ -1555,11 +1561,14 @@ struct YarpSensorBridge::Impl return false; } - if (nanExistsInVec(controlBoardRemapperMeasures.jointAccelerationsUnordered, - logPrefix, - "encoder accelerations")) + if (streamJointAccelerations) { - return false; + if (nanExistsInVec(controlBoardRemapperMeasures.jointAccelerationsUnordered, + logPrefix, + "encoder accelerations")) + { + return false; + } } } @@ -1573,9 +1582,12 @@ struct YarpSensorBridge::Impl = controlBoardRemapperMeasures.remappedJointPermutationMatrix * controlBoardRemapperMeasures.jointVelocitiesUnordered * M_PI / 180; - controlBoardRemapperMeasures.jointAccelerations.noalias() - = controlBoardRemapperMeasures.remappedJointPermutationMatrix - * controlBoardRemapperMeasures.jointAccelerationsUnordered * M_PI / 180; + if (streamJointAccelerations) + { + controlBoardRemapperMeasures.jointAccelerations.noalias() + = controlBoardRemapperMeasures.remappedJointPermutationMatrix + * controlBoardRemapperMeasures.jointAccelerationsUnordered * M_PI / 180; + } } else { log()->error("{} Unable to read from IEncodersTimed interface, use previous " diff --git a/src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp b/src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp index e665767bec..fe17720ead 100644 --- a/src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp +++ b/src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp @@ -38,6 +38,16 @@ bool YarpSensorBridge::initialize(std::weak_ptr handle return false; } + if (!ptr->getParameter("stream_joint_accelerations", m_pimpl->streamJointAccelerations)) + { + log()->info("{} Unable to get stream_joint_accelerations. Set to true by default", + logPrefix); + } + if (!m_pimpl->streamJointAccelerations) + { + log()->info("{} Joint accelerations will not be streamed.", logPrefix); + } + bool ret{true}; ret = m_pimpl->subConfigLoader("stream_joint_states", "RemoteControlBoardRemapper", @@ -443,6 +453,13 @@ bool YarpSensorBridge::getJointAcceleration(const std::string& jointName, OptionalDoubleRef receiveTimeInSeconds) { constexpr auto logPrefix = "[YarpSensorBridge::getJointAcceleration]"; + + if (!m_pimpl->streamJointAccelerations) + { + log()->error("{} Joint acceleration is not streamed.", logPrefix); + return false; + } + int idx; if (!m_pimpl->getIndexFromVector(m_pimpl->metaData.sensorsList.jointsList, jointName, idx)) { @@ -466,7 +483,15 @@ bool YarpSensorBridge::getJointAcceleration(const std::string& jointName, bool YarpSensorBridge::getJointAccelerations(Eigen::Ref jointAccelerations, OptionalDoubleRef receiveTimeInSeconds) { - if (!m_pimpl->checkControlBoardSensor("[YarpSensorBridge::getJointAccelerations]", + constexpr auto logPrefix = "[YarpSensorBridge::getJointAcceleration]"; + + if (!m_pimpl->streamJointAccelerations) + { + log()->error("{} Joint acceleration is not streamed.", logPrefix); + return false; + } + + if (!m_pimpl->checkControlBoardSensor(logPrefix, m_pimpl->controlBoardRemapperInterfaces.encoders, m_pimpl->metaData.bridgeOptions.isJointSensorsEnabled, m_pimpl->controlBoardRemapperMeasures.jointAccelerations))