diff --git a/CHANGELOG.md b/CHANGELOG.md index 9f9d39ef15..374ce85ff2 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/bindings/python/RobotInterface/src/RobotControl.cpp b/bindings/python/RobotInterface/src/RobotControl.cpp index 13281eb65b..0fa66b31ac 100644 --- a/bindings/python/RobotInterface/src/RobotControl.cpp +++ b/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 diff --git a/devices/YarpRobotLoggerDevice/scripts/blf-logger-with-audio.sh b/devices/YarpRobotLoggerDevice/scripts/blf-logger-with-audio.sh index c7e636f13d..25be31cbff 100755 --- a/devices/YarpRobotLoggerDevice/scripts/blf-logger-with-audio.sh +++ b/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 diff --git a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpRobotControl.h b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpRobotControl.h index eba2251433..42375ee67d 100644 --- a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpRobotControl.h +++ b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpRobotControl.h @@ -158,6 +158,14 @@ class YarpRobotControl : public IRobotControl */ std::vector 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 lowerLimits, Eigen::Ref upperLimits) const final; + /** * Check if the class is valid. * @note If it is valid you can directly control the robot diff --git a/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp index 1344fdea14..07fadd7b43 100644 --- a/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp +++ b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp @@ -11,6 +11,7 @@ #include #include +#include #include #include #include @@ -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 lowerLimits, + Eigen::Ref 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; +} diff --git a/src/RobotInterface/include/BipedalLocomotion/RobotInterface/IRobotControl.h b/src/RobotInterface/include/BipedalLocomotion/RobotInterface/IRobotControl.h index cd90c8e458..3a6668d8ea 100644 --- a/src/RobotInterface/include/BipedalLocomotion/RobotInterface/IRobotControl.h +++ b/src/RobotInterface/include/BipedalLocomotion/RobotInterface/IRobotControl.h @@ -146,6 +146,16 @@ class IRobotControl */ virtual std::vector 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 lowerLimits, + Eigen::Ref upperLimits) const + = 0; + /** * Check if the class is valid. * @note If it is valid you can directly control the robot