Make joint encoder acceleration reading optional (#876)

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)

devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h

@@ -9,10 +9,10 @@
 #include <atomic>
 #include <memory>
 #include <mutex>
-#include <vector>
 #include <string>
 #include <unordered_map>
 #include <unordered_set>
+#include <vector>
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/videoio.hpp>
@@ -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<std::string>& metadata = {});
+ bool addChannel(const std::string& nameKey,
+ std::size_t vectorSize,
+ const std::vector<std::string>& metadata = {});
 
  bool hasSubstring(const std::string& str, const std::vector<std::string>& 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<std::string> 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<std::string> 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

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))
  {

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<SensorBridgeMetaData>
 {

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<const BipedalLocomotion::ParametersHandler::IParametersHandler>,
@@ -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 "

src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp

@@ -38,6 +38,16 @@ bool YarpSensorBridge::initialize(std::weak_ptr<const IParametersHandler> 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<Eigen::VectorXd> 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))