Add method to get joint limits from control board (#868)

CHANGELOG.md

@@ -24,6 +24,7 @@ All notable changes to this project are documented in this file.
 - Added a simple motor control example (https://github.com/ami-iit/bipedal-locomotion-framework/pull/855)
 - 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)
 
 ### 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)

bindings/python/RobotInterface/src/RobotControl.cpp

@@ -90,7 +90,14 @@ void CreateYarpRobotControl(pybind11::module& module)
  &YarpRobotControl::setControlMode),
  py::arg("control_mode"))
  .def("get_joint_list", &YarpRobotControl::getJointList)
- .def("is_valid", &YarpRobotControl::isValid);
+ .def("is_valid", &YarpRobotControl::isValid)
+ .def("get_joint_limits", [](YarpRobotControl& impl) {
+ Eigen::VectorXd lowerLimits, upperLimits;
+ lowerLimits.resize(impl.getJointList().size());
+ upperLimits.resize(impl.getJointList().size());
+ bool ok = impl.getJointLimits(lowerLimits, upperLimits);
+ return std::make_tuple(ok, lowerLimits, upperLimits);
+ });
 }
 
 } // namespace RobotInterface

src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpRobotControl.h

@@ -158,6 +158,14 @@ class YarpRobotControl : public IRobotControl
  */
  std::vector<std::string> getJointList() const final;
 
+ /**
+ * Get the actuated joints limits.
+ * @param lowerLimits vector to be filled with the lower limits of the actuated joints.
+ * @param upperLimits vector to be filled with the upper limits of the actuated joints.
+ * @return True/False in case of success/failure.
+ */
+ bool getJointLimits(Eigen::Ref<Eigen::VectorXd> lowerLimits, Eigen::Ref<Eigen::VectorXd> upperLimits) const final;
+
  /**
  * Check if the class is valid.
  * @note If it is valid you can directly control the robot

src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp

@@ -11,6 +11,7 @@
 #include <unordered_map>
 
 #include <yarp/dev/IAxisInfo.h>
+#include <yarp/dev/IControlLimits.h>
 #include <yarp/dev/IControlMode.h>
 #include <yarp/dev/ICurrentControl.h>
 #include <yarp/dev/IEncodersTimed.h>
@@ -56,6 +57,7 @@ struct YarpRobotControl::Impl
  yarp::dev::ICurrentControl* currentInterface{nullptr}; /**< Current control interface. */
  yarp::dev::IControlMode* controlModeInterface{nullptr}; /**< Control mode interface. */
  yarp::dev::IAxisInfo* axisInfoInterface{nullptr}; /**< Axis info interface. */
+ yarp::dev::IControlLimits* controlLimitsInterface{nullptr}; /**< Control limits interface. */
 
  std::size_t actuatedDOFs; /**< Number of the actuated DoFs. */
 
@@ -329,6 +331,12 @@ struct YarpRobotControl::Impl
  return false;
  }
 
+ if (!robotDevice->view(controlLimitsInterface) || controlLimitsInterface == nullptr)
+ {
+ log()->error("{} Cannot load the IControlMode interface.", errorPrefix);
+ return false;
+ }
+
  // get the number of degree of freedom
  int dofs = 0;
  if (!encodersInterface->getAxes(&dofs))
@@ -820,3 +828,36 @@ bool YarpRobotControl::isValid() const
 {
  return m_pimpl->robotDevice != nullptr;
 }
+
+bool YarpRobotControl::getJointLimits(Eigen::Ref<Eigen::VectorXd> lowerLimits,
+ Eigen::Ref<Eigen::VectorXd> upperLimits) const
+{
+
+ constexpr auto errorPrefix = "[YarpRobotControl::getJointLimits]";
+
+ if (lowerLimits.size() != m_pimpl->actuatedDOFs)
+ {
+ log()->error("{} The size of the first input vector is not "
+ "correct. Expected size: {}. Received size: {}.",
+ errorPrefix,
+ m_pimpl->actuatedDOFs,
+ lowerLimits.size());
+ return false;
+ }
+
+ if (upperLimits.size() != m_pimpl->actuatedDOFs)
+ {
+ log()->error("{} The size of the second input vector is not "
+ "correct. Expected size: {}. Received size: {}.",
+ errorPrefix,
+ m_pimpl->actuatedDOFs,
+ upperLimits.size());
+ return false;
+ }
+
+ for (int i = 0; i < m_pimpl->actuatedDOFs; i++)
+ {
+ m_pimpl->controlLimitsInterface->getLimits(i, &lowerLimits[i], &upperLimits[i]);
+ }
+ return true;
+}

src/RobotInterface/include/BipedalLocomotion/RobotInterface/IRobotControl.h

@@ -146,6 +146,16 @@ class IRobotControl
  */
  virtual std::vector<std::string> getJointList() const = 0;
 
+ /**
+ * Get the actuated joints limits.
+ * @param lowerLimits vector to be filled with the lower limits of the actuated joints.
+ * @param upperLimits vector to be filled with the upper limits of the actuated joints.
+ * @return True/False in case of success/failure.
+ */
+ virtual bool getJointLimits(Eigen::Ref<Eigen::VectorXd> lowerLimits,
+ Eigen::Ref<Eigen::VectorXd> upperLimits) const
+ = 0;
+
  /**
  * Check if the class is valid.
  * @note If it is valid you can directly control the robot