Merge branch 'master' into friction_estimation

CHANGELOG.md

@@ -23,6 +23,8 @@ All notable changes to this project are documented in this file.
 - Create python bindings of `VectorsCollection` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/854)
 - 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)
 - Implement joint torque control device and friction estimation through PINN (https://github.com/ami-iit/bipedal-locomotion-framework/pull/866)
 
 ### Changed

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

devices/YarpRobotLoggerDevice/scripts/blf-logger-with-audio.sh

@@ -1,6 +1,38 @@
 #!/usr/bin/env bash
 
-input_port_audio=$1
+# Parse command line arguments
+while [[ $# -gt 0 ]]
+do
+    key="$1"
+
+    case $key in
+        # Parse the input port for the audio device
+        -i|--input-port-audio)
+        input_port_audio="$2"
+        shift
+        shift
+        ;;
+        # Parse the launch file for the logger device
+        -l|--launch-file)
+        launch_file="$2"
+        shift
+        shift
+        ;;
+        -h|--help|*)
+        # Display help text
+        echo "Usage: $0 [OPTIONS]"
+        echo "Options:"
+        echo "  -i, --input-port-audio    Specify the input port for the audio device (default: /icub/microphone/audio:o)"
+        echo "  -l, --launch-file         Specify the launch file for the logger device (default: launch-yarp-robot-logger.xml)"
+        echo "  -h, --help                Display this help message"
+        exit 0
+        ;;
+    esac
+done
+
+# Set default values if not provided
+input_port_audio=${input_port_audio:-/icub/microphone/audio:o}
+launch_file=${launch_file:-launch-yarp-robot-logger.xml}
 
 # Function to handle SIGINT signal
 function handle_sigint {
@@ -35,7 +67,7 @@ yarp wait /YarpRobotLogger/audio/audio:i
 yarp connect $input_port_audio /YarpRobotLogger/audio/audio:i fast_tcp
 
 # launch the logger device
-yarprobotinterface --config launch-yarp-robot-logger.xml &
+yarprobotinterface --config $launch_file &
 bg_pid2=$!
 
 # register the signal handlers

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