From 44e23c829a15edca4c09dc992b1f79d4e1d91b6a Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 5 Apr 2024 14:45:22 +0200 Subject: [PATCH 1/7] Refactor YarpRobotControl::setReferences function to include optional current joint values and avoid to switch control mode in YarpRobotControl::setReferences --- .../RobotInterface/src/RobotControl.cpp | 19 +- .../RobotInterface/YarpRobotControl.h | 13 +- .../src/YarpRobotControl.cpp | 208 +++++++++++------- .../RobotInterface/IRobotControl.h | 22 +- 4 files changed, 161 insertions(+), 101 deletions(-) diff --git a/bindings/python/RobotInterface/src/RobotControl.cpp b/bindings/python/RobotInterface/src/RobotControl.cpp index 2bb4673377..4220051475 100644 --- a/bindings/python/RobotInterface/src/RobotControl.cpp +++ b/bindings/python/RobotInterface/src/RobotControl.cpp @@ -5,6 +5,8 @@ * distributed under the terms of the BSD-3-Clause license. */ +#include + #include #include #include @@ -65,15 +67,20 @@ void CreateYarpRobotControl(pybind11::module& module) }) .def("set_references", py::overload_cast, - const std::vector&>( + const std::vector&, + std::optional>>( &YarpRobotControl::setReferences), - py::arg("joints_value"), - py::arg("control_modes")) + py::arg("desired_joints_value"), + py::arg("control_modes"), + py::arg("current_joints_value") = std::nullopt) .def("set_references", - py::overload_cast, const IRobotControl::ControlMode&>( + py::overload_cast, + const IRobotControl::ControlMode&, + std::optional>>( &YarpRobotControl::setReferences), - py::arg("joints_value"), - py::arg("control_mode")) + py::arg("desired_joints_value"), + py::arg("control_mode"), + py::arg("current_joints_value") = std::nullopt) .def("set_control_mode", py::overload_cast&>( &YarpRobotControl::setControlMode), diff --git a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpRobotControl.h b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpRobotControl.h index 330caa6c0b..f72bed2702 100644 --- a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpRobotControl.h +++ b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpRobotControl.h @@ -10,6 +10,7 @@ // std #include +#include // Eigen #include @@ -94,7 +95,7 @@ class YarpRobotControl : public IRobotControl /** * Set the desired reference. - * @param jointValues desired joint values. + * @param desiredJointValues desired joint values. * @param controlModes vector containing the control mode for each joint. * @return True/False in case of success/failure. * @note In case of position control the values has to be expressed in rad, in case of velocity @@ -103,12 +104,13 @@ class YarpRobotControl : public IRobotControl * between -100 and 100. * @warning At the current stage only revolute joints are supported. */ - bool setReferences(Eigen::Ref jointValues, - const std::vector& controlModes) final; + bool setReferences(Eigen::Ref desiredJointValues, + const std::vector& controlModes, + std::optional> currentJointValues = {}) final; /** * Set the desired reference. - * @param jointValues desired joint values. + * @param desiredJointValues desired joint values. * @param controlMode a control mode for all the joints. * @return True/False in case of success/failure. * @note In case of position control the values has to be expressed in rad, in case of velocity @@ -120,7 +122,8 @@ class YarpRobotControl : public IRobotControl * std::vector&). */ bool setReferences(Eigen::Ref desiredJointValues, - const IRobotControl::ControlMode& mode) final; + const IRobotControl::ControlMode& mode, + std::optional> currentJointValues = {}) final; /** * Get the list of the controlled joints diff --git a/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp index 53ee07b922..80886b3ba5 100644 --- a/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp +++ b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp @@ -19,9 +19,9 @@ #include #include #include -#include #include +#include #include using namespace BipedalLocomotion::RobotInterface; @@ -63,7 +63,8 @@ struct YarpRobotControl::Impl std::vector controlModes; /**< Vector containing the map between the joint and the current control mode */ std::vector controlModesYarp; /**< Vector containing the map between the - joint and the current yarp control mode */ + joint and the current yarp control mode + */ std::vector axesName; /**< List containing the mapping between the joints index and the their name */ @@ -74,10 +75,12 @@ struct YarpRobotControl::Impl std::vector positionControlRefSpeeds; /**< Vector containing the ref speed in deg/seconds for the position control joints. */ - double positioningDuration{0.0}; /**< Duration of the trajectory generated when the joint is - controlled in position mode */ - double startPositionControlInstant{0.0}; /**< Initial time instant of the trajectory generated - when the joint is controlled in position mode */ + std::chrono::nanoseconds positioningDuration{0}; /**< Duration of the trajectory generated when + the joint is controlled in position mode */ + std::chrono::nanoseconds startPositionControlInstant{0}; /**< Initial time instant of the + trajectory generated when the + joint is controlled in position + mode */ double positioningTolerance{0.0}; /**< Max Admissible error for position control joint [rad] */ double positionDirectMaxAdmissibleError{0.0}; /**< Max admissible error for position direct control joint [rad] */ @@ -217,9 +220,9 @@ struct YarpRobotControl::Impl } // resize the desired joint value vector associated to each control mode - for (const auto& [mode, indeces] : this->desiredJointValuesAndMode.index) + for (const auto& [mode, indices] : this->desiredJointValuesAndMode.index) { - this->desiredJointValuesAndMode.value[mode].resize(indeces.size()); + this->desiredJointValuesAndMode.value[mode].resize(indices.size()); } // resize the position control reference speed vector @@ -370,9 +373,9 @@ struct YarpRobotControl::Impl } // resize the desired joint value vector associated to each control mode - for (const auto& [mode, indeces] : this->desiredJointValuesAndMode.index) + for (const auto& [mode, indices] : this->desiredJointValuesAndMode.index) { - this->desiredJointValuesAndMode.value[mode].resize(indeces.size()); + this->desiredJointValuesAndMode.value[mode].resize(indices.size()); } // resize the reference speed for the position control mode @@ -387,8 +390,9 @@ struct YarpRobotControl::Impl * Return the the worst position error for the joint controlled in position direct. * The first value is the index while the second is the error in radians. */ - std::pair getWorstPositionDirectError(Eigen::Ref desiredJointValues, - Eigen::Ref jointPositions) const + std::pair + getWorstPositionDirectError(Eigen::Ref desiredJointValues, + Eigen::Ref jointPositions) const { // clear the std::pair std::pair worstError{0, 0.0}; @@ -461,44 +465,65 @@ struct YarpRobotControl::Impl return nullptr; } - bool setReferences(Eigen::Ref jointValues) + bool setReferences(Eigen::Ref jointValues, + std::optional> currentJointValues) { constexpr auto errorPrefix = "[YarpRobotControl::Impl::setReferences]"; - if(!this->getJointPos()) + // the following checks are performed only if the robot is controlled in position direct or + // in position mode + if (!this->desiredJointValuesAndMode.index[IRobotControl::ControlMode::Position].empty() + || !this->desiredJointValuesAndMode.index[IRobotControl::ControlMode::PositionDirect] + .empty()) { - log()->error("{} Unable to get the joint position.", errorPrefix); - return false; - } + if (currentJointValues.has_value()) + { + this->positionFeedback = currentJointValues.value(); + } else + { + if (!this->getJointPos()) + { + log()->error("{} Unable to get the joint position.", errorPrefix); + return false; + } + } - const auto worstError = this->getWorstPositionDirectError(jointValues, - this->positionFeedback); + const auto worstError = this->getWorstPositionDirectError(jointValues, // + this->positionFeedback); - if (worstError.second > this->positionDirectMaxAdmissibleError) - { - log()->error("{} The worst error between the current and the desired position of the " - "joint named '{}' is greater than {} deg. Error = {} deg.", - errorPrefix, - this->axesName[worstError.first], - 180 / M_PI * this->positionDirectMaxAdmissibleError, - 180 / M_PI * worstError.second); - return false; + if (worstError.second > this->positionDirectMaxAdmissibleError) + { + log()->error("{} The worst error between the current and the desired position of " + "the " + "joint named '{}' is greater than {} deg. Error = {} deg.", + errorPrefix, + this->axesName[worstError.first], + 180 / M_PI * this->positionDirectMaxAdmissibleError, + 180 / M_PI * worstError.second); + return false; + } } - for (const auto& [mode, indeces] : this->desiredJointValuesAndMode.index) + for (const auto& [mode, indices] : this->desiredJointValuesAndMode.index) { - // if indeces vector is empty no joint is controlled with this specific control mode - if (indeces.empty()) + // if indices vector is empty no joint is controlled with this specific control mode + if (indices.empty()) + { continue; + } if (mode == IRobotControl::ControlMode::Idle) + { continue; + } else if (mode == IRobotControl::ControlMode::Unknown) { std::string joints = ""; - for (const auto& index : indeces) + for (const auto& index : indices) + { joints += " '" + this->axesName[index] + "'"; + } log()->error("{} The following joints does not have a specified control " "mode:{}. Please set a feasible control mode.", @@ -508,20 +533,22 @@ struct YarpRobotControl::Impl } else if (mode == IRobotControl::ControlMode::Position) { - for (int i = 0; i < indeces.size(); i++) + const double positioningDurationSeconds + = std::chrono::duration(this->positioningDuration).count(); + for (int i = 0; i < indices.size(); i++) { - const auto jointError = std::abs(jointValues[indeces[i]] - - this->positionFeedback[indeces[i]]); + const auto jointError + = std::abs(jointValues[indices[i]] - this->positionFeedback[indices[i]]); constexpr double scaling = 180 / M_PI; constexpr double minVelocityInDegPerSeconds = 3.0; this->positionControlRefSpeeds[i] = std::max(minVelocityInDegPerSeconds, - scaling * (jointError / this->positioningDuration)); + scaling * (jointError / positioningDurationSeconds)); } - if (!this->positionInterface->setRefSpeeds(indeces.size(), - indeces.data(), + if (!this->positionInterface->setRefSpeeds(indices.size(), + indices.data(), this->positionControlRefSpeeds.data())) { log()->error("{} Unable to set the reference speed for the position control " @@ -530,22 +557,27 @@ struct YarpRobotControl::Impl return false; } - this->startPositionControlInstant = yarp::os::Time::now(); + this->startPositionControlInstant = BipedalLocomotion::clock().now(); } // Yarp wants the quantities in degrees double scaling = 180 / M_PI; - // if the control mode is torque or PWM it is not required to change the unit of + // if the control mode is torque or PWM current it is not required to change the unit of // measurement - if (mode == ControlMode::Torque || mode == ControlMode::PWM + if (mode == ControlMode::Torque // + || mode == ControlMode::PWM // || mode == ControlMode::Current) + { scaling = 1; + } - for (int i = 0; i < indeces.size(); i++) - this->desiredJointValuesAndMode.value[mode][i] = scaling * jointValues[indeces[i]]; + for (int i = 0; i < indices.size(); i++) + { + this->desiredJointValuesAndMode.value[mode][i] = scaling * jointValues[indices[i]]; + } - if (!this->control(mode)(indeces.size(), - indeces.data(), + if (!this->control(mode)(indices.size(), + indices.data(), this->desiredJointValuesAndMode.value[mode].data())) { @@ -555,6 +587,18 @@ struct YarpRobotControl::Impl } return true; } + + bool checkControlMode(const std::vector& controlModes) const + { + return controlModes == this->controlModes; + } + + bool checkControlMode(const IRobotControl::ControlMode& mode) const + { + return std::all_of(this->controlModes.begin(), + this->controlModes.end(), + [&mode](const auto& m) { return m == mode; }); + } }; YarpRobotControl::YarpRobotControl() @@ -610,12 +654,14 @@ bool YarpRobotControl::initialize(std::weak_ptrgetParameter("positioning_duration", m_pimpl->positioningDuration) - && (m_pimpl->positioningDuration > 0); + && (m_pimpl->positioningDuration > 0s); ok = ok && ptr->getParameter("positioning_tolerance", m_pimpl->positioningTolerance) && (m_pimpl->positioningTolerance > 0); - ok = ok && ptr->getParameter("position_direct_max_admissible_error", - m_pimpl->positionDirectMaxAdmissibleError) + ok = ok + && ptr->getParameter("position_direct_max_admissible_error", + m_pimpl->positionDirectMaxAdmissibleError) && (m_pimpl->positionDirectMaxAdmissibleError > 0); return ok; @@ -623,58 +669,53 @@ bool YarpRobotControl::initialize(std::weak_ptr& controlModes) { - if (controlModes != m_pimpl->controlModes) + if (!m_pimpl->setControlModes(controlModes)) { - m_pimpl->controlModes = controlModes; - if (!m_pimpl->setControlModes(m_pimpl->controlModes)) - { - log()->error("[YarpRobotControl::setControlMode] Unable to set the control modes."); - return false; - } + log()->error("[YarpRobotControl::setControlMode] Unable to set the control modes."); + return false; } + // if the control mode is set for all the joints, we can store it + m_pimpl->controlModes = controlModes; + return true; } bool YarpRobotControl::setControlMode(const IRobotControl::ControlMode& mode) { - // check if all the joints are controlled in the desired control mode - if (!std::all_of(m_pimpl->controlModes.begin(), - m_pimpl->controlModes.end(), - [&mode](const auto& m) { return m == mode; })) - { - std::fill(m_pimpl->controlModes.begin(), m_pimpl->controlModes.end(), mode); - if (!m_pimpl->setControlModes(m_pimpl->controlModes)) - { - log()->error("[YarpRobotControl::setControlMode] Unable to set the control modes."); - return false; - } - } - return true; + // create a vector containing the same control mode for all the joints + const std::vector controlModes(m_pimpl->actuatedDOFs, mode); + return this->setControlMode(controlModes); } -bool YarpRobotControl::setReferences(Eigen::Ref jointValues, - const std::vector& controlModes) +bool YarpRobotControl::setReferences( + Eigen::Ref desiredJointValues, + const std::vector& controlModes, + std::optional> currentJointValues) { - if (!this->setControlMode(controlModes)) - { - log()->error("[YarpRobotControl::setReferences] Unable to set the control modes."); - return false; - } + if (!m_pimpl->checkControlMode(controlModes)) + { + log()->error("[YarpRobotControl::setReferences] Control modes are not the expected one. " + "Please call setControlMode before calling this function."); + return false; + } - return m_pimpl->setReferences(jointValues); + return m_pimpl->setReferences(desiredJointValues, currentJointValues); } -bool YarpRobotControl::setReferences(Eigen::Ref desiredJointValues, - const IRobotControl::ControlMode& mode) +bool YarpRobotControl::setReferences( + Eigen::Ref desiredJointValues, + const IRobotControl::ControlMode& mode, + std::optional> currentJointValues) { - if (!this->setControlMode(mode)) + if (!m_pimpl->checkControlMode(mode)) { - log()->error("[YarpRobotControl::setReferences] Unable to set the control mode."); + log()->error("[YarpRobotControl::setReferences] Control mode is not the expected one. " + "Please call setControlMode before calling this function."); return false; } - return m_pimpl->setReferences(desiredJointValues); + return m_pimpl->setReferences(desiredJointValues, currentJointValues); } bool YarpRobotControl::checkMotionDone(bool& motionDone, @@ -716,8 +757,9 @@ bool YarpRobotControl::checkMotionDone(bool& motionDone, } } - const double now = yarp::os::Time::now(); - constexpr double timeTolerance = 1.0; + using namespace std::chrono_literals; + const std::chrono::nanoseconds now = BipedalLocomotion::clock().now(); + constexpr std::chrono::nanoseconds timeTolerance{1s}; if (now - m_pimpl->startPositionControlInstant > m_pimpl->positioningDuration + timeTolerance) { isTimeExpired = true; diff --git a/src/RobotInterface/include/BipedalLocomotion/RobotInterface/IRobotControl.h b/src/RobotInterface/include/BipedalLocomotion/RobotInterface/IRobotControl.h index e183e415c2..60c776ee48 100644 --- a/src/RobotInterface/include/BipedalLocomotion/RobotInterface/IRobotControl.h +++ b/src/RobotInterface/include/BipedalLocomotion/RobotInterface/IRobotControl.h @@ -9,6 +9,8 @@ #define BIPEDAL_LOCOMOTION_ROBOT_INTERFACE_IROBOT_CONTROL_H #include +#include +#include #include @@ -64,24 +66,29 @@ class IRobotControl */ virtual bool checkMotionDone(bool& motionDone, bool& isTimerExpired, - std::vector>& info) = 0; + std::vector>& info) + = 0; /** * Set the desired reference. - * @param jointValues desired joint values. + * @param desiredJointValues desired joint values. * @param controlModes vector containing the control mode for each joint. + * @param currentJointValues current joint values. * @return True/False in case of success/failure. * @note In case of position control the values has to be expressed in rad, in case of velocity * control in rad/s. If the robot is controlled in torques, the desired joint values are * expressed in Nm. */ - virtual bool setReferences(Eigen::Ref jointValues, - const std::vector& controlModes) = 0; + virtual bool setReferences(Eigen::Ref desiredJointValues, + const std::vector& controlModes, + std::optional> currentJointValues = {}) + = 0; /** * Set the desired reference. - * @param jointValues desired joint values. + * @param desiredJointValues desired joint values. * @param controlMode a control mode for all the joints. + * @param currentJointValues current joint values. * @return True/False in case of success/failure. * @note In case of position control the values has to be expressed in rad, in case of velocity * control in rad/s. If the robot is controlled in torques, the desired joint values are @@ -91,7 +98,9 @@ class IRobotControl * std::vector&). */ virtual bool setReferences(Eigen::Ref desiredJointValues, - const IRobotControl::ControlMode& controlMode) = 0; + const IRobotControl::ControlMode& controlMode, + std::optional> currentJointValues = {}) + = 0; /** * Set the control mode. @@ -126,7 +135,6 @@ class IRobotControl * Destructor. */ virtual ~IRobotControl() = default; - }; } // namespace RobotInterface } // namespace BipedalLocomotion From 583f3231660e31f4ec85f67c530d8e5814420ab3 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 5 Apr 2024 15:41:17 +0200 Subject: [PATCH 2/7] Fix the joint blf-balancing-position-control --- .../script/blf-balancing-position-control.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/utilities/balancing-position-control/script/blf-balancing-position-control.py b/utilities/balancing-position-control/script/blf-balancing-position-control.py index f685951f85..835db22972 100644 --- a/utilities/balancing-position-control/script/blf-balancing-position-control.py +++ b/utilities/balancing-position-control/script/blf-balancing-position-control.py @@ -475,7 +475,9 @@ def main(): # send the joint pose if not robot_control.set_references( - desired_joint_positions, blf.robot_interface.YarpRobotControl.PositionDirect + desired_joint_positions, + blf.robot_interface.YarpRobotControl.PositionDirect, + joint_positions, ): raise RuntimeError("Unable to set the references") @@ -487,9 +489,7 @@ def main(): com_from_desired = kindyn.getCenterOfMassPosition().toNumPy() com_from_measured = kindyn_with_measured.getCenterOfMassPosition().toNumPy() - if not vectors_collection_server.prepare_data(): - raise RuntimeError("Unable to prepare the data") - + vectors_collection_server.prepare_data() vectors_collection_server.clear_data() vectors_collection_server.populate_data("zmp::desired_planner", desired_zmp) From ff90d2dd19588969384d12ebd3e0981cf3db3d5b Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 5 Apr 2024 15:42:06 +0200 Subject: [PATCH 3/7] Fix the script blf-balancing-torque-control --- .../blf-balancing-torque-control-options.ini | 1 + .../data_logging.ini | 1 + .../blf_balancing_torque_control/tsid.ini | 9 +-- .../script/blf-balancing-torque-control.py | 78 ++++++++++++++----- 4 files changed, 65 insertions(+), 24 deletions(-) create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf_balancing_torque_control/data_logging.ini diff --git a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf-balancing-torque-control-options.ini b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf-balancing-torque-control-options.ini index 47ed66b709..faa87a668c 100644 --- a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf-balancing-torque-control-options.ini +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf-balancing-torque-control-options.ini @@ -17,3 +17,4 @@ right_contact_frame r_sole [include ROBOT_CONTROL "./blf_balancing_torque_control/robot_control.ini"] [include SENSOR_BRIDGE "./blf_balancing_torque_control/sensor_bridge.ini"] [include CONTACT_WRENCHES "./blf_balancing_torque_control/contact_wrenches.ini"] +[include DATA_LOGGING "./blf_balancing_torque_control/data_logging.ini"] diff --git a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf_balancing_torque_control/data_logging.ini b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf_balancing_torque_control/data_logging.ini new file mode 100644 index 0000000000..8d88ff6d49 --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf_balancing_torque_control/data_logging.ini @@ -0,0 +1 @@ +remote "/balancing_torque_controller/logger" diff --git a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf_balancing_torque_control/tsid.ini b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf_balancing_torque_control/tsid.ini index 432ed0b455..d4f40fecb1 100644 --- a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf_balancing_torque_control/tsid.ini +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf_balancing_torque_control/tsid.ini @@ -50,9 +50,8 @@ robot_acceleration_variable_name "robot_acceleration" frame_name "chest" kp_angular 5.0 kd_angular 2.0 - -priority 1 -weight (5.0, 5.0, 5.0) +priority 1 +weight (5.0, 5.0, 5.0) [JOINT_REGULARIZATION_TASK] name joint_regularization_task @@ -86,8 +85,8 @@ variable_name "lf_wrench" frame_name "l_sole" number_of_slices 2 static_friction_coefficient 0.3 -foot_limits_x (-0.1, 0.1) -foot_limits_y (-0.05, 0.05) +foot_limits_x (-0.1, 0.1) +foot_limits_y (-0.05, 0.05) [RF_WRENCH_TASK] name "rf_feasibility_wrench_task" diff --git a/utilities/balancing-torque-control/script/blf-balancing-torque-control.py b/utilities/balancing-torque-control/script/blf-balancing-torque-control.py index f3098f2d40..2d49bf46c4 100755 --- a/utilities/balancing-torque-control/script/blf-balancing-torque-control.py +++ b/utilities/balancing-torque-control/script/blf-balancing-torque-control.py @@ -36,6 +36,8 @@ class Application: def __init__(self, config_file: str): + self.first_iteration = True + param_handler = blf.parameters_handler.YarpParametersHandler() if not param_handler.set_from_filename(config_file): raise ValueError("Unable to set the parameter handler from the file.") @@ -141,7 +143,10 @@ def __init__(self, config_file: str): param_handler=param_handler.get_group("TSID"), kindyn=self.kindyn_with_measured, ) + + # print the solver to check if it is correct print(self.tsid.solver) + I_H_r_sole = self.kindyn_with_measured.getWorldTransform( self.right_contact_frame ) @@ -221,9 +226,29 @@ def __init__(self, config_file: str): self.index = 0 self.knot_index = 1 - self.port = blf.yarp_utilities.BufferedPortVectorsCollection() - if not self.port.open("/balancing_controller/logger/data:o"): - raise ValueError("Impossible to open the port.") + self.vectors_collection_server = blf.yarp_utilities.VectorsCollectionServer() + if not self.vectors_collection_server.initialize( + param_handler.get_group("DATA_LOGGING") + ): + raise RuntimeError("Unable to initialize the vectors collection server") + + # populate the metadata + self.vectors_collection_server.populate_metadata("global_zmp", ["x", "y"]) + self.vectors_collection_server.populate_metadata( + "global_zmp_from_measured", ["x", "y"] + ) + self.vectors_collection_server.populate_metadata("local_zmp_left", ["x", "y"]) + self.vectors_collection_server.populate_metadata("local_zmp_right", ["x", "y"]) + self.vectors_collection_server.populate_metadata( + "com_from_desired", ["x", "y", "z"] + ) + self.vectors_collection_server.populate_metadata( + "com_from_measured", ["x", "y", "z"] + ) + self.vectors_collection_server.populate_metadata( + "desired_torque", self.robot_control.get_joint_list() + ) + self.vectors_collection_server.finalize_metadata() def build_remote_control_board_driver( self, @@ -382,6 +407,16 @@ def advance(self): self.tsid.solver.get_output().joint_accelerations * self.dt.total_seconds() ) + # if is the first iteration we switch the control mode to torque control + if self.first_iteration: + if not self.robot_control.set_control_mode( + blf.robot_interface.YarpRobotControl.Torque + ): + blf.log().error("Impossible to set the control mode.") + return False + + self.first_iteration = False + # send the joint torques if not self.robot_control.set_references( self.tsid.solver.get_output().joint_torques, @@ -438,17 +473,26 @@ def advance(self): self.kindyn_with_measured.getCenterOfMassPosition().toNumPy() ) - data = self.port.prepare() - data.vectors = { - "global_zmp": global_zmp, - "global_zmp_from_measured": global_zmp_from_measured, - "local_zmp_left": local_zmp_left, - "local_zmp_right": local_zmp_right, - "com_from_desired": com_from_desired, - "com_from_measured": com_from_measured, - "desired_torque": self.tsid.solver.get_output().joint_torques, - } - self.port.write() + self.vectors_collection_server.prepare_data() + self.vectors_collection_server.clear_data() + + self.vectors_collection_server.populate_data("global_zmp", global_zmp) + self.vectors_collection_server.populate_data( + "global_zmp_from_measured", global_zmp_from_measured + ) + self.vectors_collection_server.populate_data("local_zmp_left", local_zmp_left) + self.vectors_collection_server.populate_data("local_zmp_right", local_zmp_right) + self.vectors_collection_server.populate_data( + "com_from_desired", com_from_desired + ) + self.vectors_collection_server.populate_data( + "com_from_measured", com_from_measured + ) + self.vectors_collection_server.populate_data( + "desired_torque", self.tsid.solver.get_output().joint_torques + ) + + self.vectors_collection_server.send_data() if self.index * self.dt >= self.motion_duration + self.motion_timeout: if self.knot_index + 1 >= len(self.com_knots_delta_x): @@ -486,17 +530,13 @@ def advance(self): return True def __del__(self): - print("chiudo tutto e devo mori") - # switch the control mode to position control if self.robot_control.is_valid(): self.robot_control.set_control_mode( blf.robot_interface.YarpRobotControl.Position ) - # close the port - if not self.port.is_closed(): - self.port.close() + blf.log().info("Closing the application.") def main(): From 46dd488bdd06fb79d6862ce604264d6541180c23 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 5 Apr 2024 15:59:56 +0200 Subject: [PATCH 4/7] Update joint position tracking and joint trajectory player --- .../joint-position-tracking/src/Module.cpp | 105 +++++++++++----- .../joint-trajectory-player/src/Module.cpp | 114 +++++++++++------- 2 files changed, 142 insertions(+), 77 deletions(-) diff --git a/utilities/joint-position-tracking/src/Module.cpp b/utilities/joint-position-tracking/src/Module.cpp index bf9cb39353..814d7dce94 100644 --- a/utilities/joint-position-tracking/src/Module.cpp +++ b/utilities/joint-position-tracking/src/Module.cpp @@ -10,9 +10,9 @@ #include #include +#include #include #include -#include #include @@ -32,18 +32,18 @@ double Module::getPeriod() bool Module::createPolydriver(std::shared_ptr handler) { + constexpr auto logPrefix = "[Module::createPolydriver]"; auto ptr = handler->getGroup("ROBOT_INTERFACE").lock(); if (ptr == nullptr) { - std::cerr << "[Module::createPolydriver] Robot interface options is empty." << std::endl; + log()->error("{} Unable to find the group ROBOT_INTERFACE.", logPrefix); return false; } ptr->setParameter("local_prefix", this->getName()); m_controlBoard = RobotInterface::constructRemoteControlBoardRemapper(ptr); if (!m_controlBoard.isValid()) { - std::cerr << "[Module::createPolydriver] the robot polydriver has not been constructed." - << std::endl; + log()->error("{} Unable to create the polydriver.", logPrefix); return false; } @@ -52,14 +52,16 @@ bool Module::createPolydriver(std::shared_ptrview(axis); if (axis != nullptr) + { axis->getAxes(&controlBoardDOFs); + } if (controlBoardDOFs != 1) { - std::cerr << "[Module::createPolydriver] The current implementation can be used to control " - "only one joint. Please be sure that the size of the joint_list and " - "remote_control_boards is equal to one." - << std::endl; + log()->error("{} The current implementation can be used to control only one joint. " + "Please be sure that the size of the joint_list and remote_control_boards is " + "equal to one.", + logPrefix); return false; } @@ -70,16 +72,12 @@ bool Module::initializeRobotControl(std::shared_ptrgetGroup("ROBOT_CONTROL"))) { - std::cerr << "[Module::initializeRobotControl] Unable to initialize the " - "control board" - << std::endl; + log()->error("[Module::initializeRobotControl] Unable to initialize the control board"); return false; } if (!m_robotControl.setDriver(m_controlBoard.poly)) { - std::cerr << "[Module::initializeRobotControl] Unable to initialize the " - "control board" - << std::endl; + log()->error("[Module::initializeRobotControl] Unable to set the driver"); return false; } @@ -90,8 +88,7 @@ bool Module::instantiateSensorBridge(std::shared_ptrgetGroup("SENSOR_BRIDGE"))) { - std::cerr << "[Module::initializeSensorBridge] Unable to initialize the sensor bridge" - << std::endl; + log()->error("[Module::initializeSensorBridge] Unable to initialize the sensor bridge"); return false; } @@ -99,7 +96,7 @@ bool Module::instantiateSensorBridge(std::shared_ptrerror("[Module::initializeSensorBridge] Unable to set the driver list"); return false; } @@ -113,28 +110,43 @@ bool Module::configure(yarp::os::ResourceFinder& rf) auto parametersHandler = std::make_shared(rf); std::string name; - if(!parametersHandler->getParameter("name", name)) + if (!parametersHandler->getParameter("name", name)) + { + log()->error("[Module::configure] Unable to find the parameter 'name'."); return false; + } this->setName(name.c_str()); - if(!parametersHandler->getParameter("sampling_time", m_dT)) + if (!parametersHandler->getParameter("sampling_time", m_dT)) + { + log()->error("[Module::configure] Unable to find the parameter 'sampling_time'."); return false; + } double maxValue = 0; - if(!parametersHandler->getParameter("max_angle_deg", maxValue)) + if (!parametersHandler->getParameter("max_angle_deg", maxValue)) + { + log()->error("[Module::configure] Unable to find the parameter 'max_angle_deg'."); return false; + } maxValue *= M_PI / 180; double minValue = 0; - if(!parametersHandler->getParameter("min_angle_deg", minValue)) + if (!parametersHandler->getParameter("min_angle_deg", minValue)) + { + log()->error("[Module::configure] Unable to find the parameter 'min_angle_deg'."); return false; + } minValue *= M_PI / 180; double trajectoryDuration = 5; - if(!parametersHandler->getParameter("trajectory_duration", trajectoryDuration)) + if (!parametersHandler->getParameter("trajectory_duration", trajectoryDuration)) + { + log()->error("[Module::configure] Unable to find the parameter 'trajectory_duration'."); return false; + } this->createPolydriver(parametersHandler); this->initializeRobotControl(parametersHandler); @@ -156,7 +168,7 @@ bool Module::configure(yarp::os::ResourceFinder& rf) if (!m_sensorBridge.advance()) { - std::cerr << "[Module::updateModule] Unable to read the sensor." << std::endl; + log()->error("[Module::configure] Unable to read the sensor."); return false; } m_sensorBridge.getJointPositions(m_currentJointPos); @@ -172,7 +184,14 @@ bool Module::configure(yarp::os::ResourceFinder& rf) m_initTrajectoryTime = yarp::os::Time::now(); - std::cout << "[Module::configure] Starting the experiment." << std::endl; + // switch in position direct control + if (!m_robotControl.setControlMode(RobotInterface::IRobotControl::ControlMode::PositionDirect)) + { + log()->error("[Module::configure] Unable to switch in position direct control."); + return false; + } + + log()->info("[Module::configure] Module configured."); return true; } @@ -183,7 +202,9 @@ bool Module::generateNewTrajectory() // the trajectory is ended if (std::next(m_currentSetPoint, 1) == m_setPoints.end()) + { return false; + } std::advance(m_currentSetPoint, 1); double endTrajectory = *m_currentSetPoint; @@ -200,23 +221,38 @@ bool Module::generateNewTrajectory() bool Module::updateModule() { + constexpr auto logPrefix = "[Module::updateModule]"; + if (!m_sensorBridge.advance()) { - std::cerr << "[Module::updateModule] Unable to read the sensor." << std::endl; + log()->error("{} Unable to read the sensor.", logPrefix); return false; } - m_sensorBridge.getJointPositions(m_currentJointPos); + if (!m_sensorBridge.getJointPositions(m_currentJointPos)) + { + log()->error("{} Unable to get the joint position.", logPrefix); + return false; + } // set the reference - m_robotControl.setReferences(m_spline.getOutput().position, - RobotInterface::IRobotControl::ControlMode::PositionDirect); + if (!m_robotControl.setReferences(m_spline.getOutput().position, + RobotInterface::IRobotControl::ControlMode::PositionDirect, + m_currentJointPos)) + { + log()->error("{} Unable to set the reference.", logPrefix); + return false; + } m_logJointPos.push_back(m_currentJointPos[0]); m_logDesiredJointPos.push_back(m_spline.getOutput().position[0]); // advance the spline - m_spline.advance(); + if (!m_spline.advance()) + { + log()->error("{} Unable to advance the spline.", logPrefix); + return false; + } const double now = yarp::os::Time::now(); if (now - m_initTrajectoryTime > std::chrono::duration(m_timeKnots.back()).count() + 2) @@ -235,7 +271,7 @@ bool Module::updateModule() bool Module::close() { - std::cout << "[Module::close] I'm storing the dataset." << std::endl; + log()->info("[Module::close] Storing the dataset."); // set the file name std::time_t t = std::time(nullptr); @@ -256,11 +292,14 @@ bool Module::close() stream.close(); - std::cout << "[Module::close] Dataset stored. Closing." << std::endl; + log()->info("[Module::close] Dataset stored in {}", fileName.str()); // switch back in position control - m_robotControl.setReferences(m_spline.getOutput().position, - RobotInterface::IRobotControl::ControlMode::Position); + if (!m_robotControl.setControlMode(RobotInterface::IRobotControl::ControlMode::Position)) + { + log()->error("[Module::close] Unable to switch back in position control."); + return false; + } return true; } diff --git a/utilities/joint-trajectory-player/src/Module.cpp b/utilities/joint-trajectory-player/src/Module.cpp index 0c752740ac..24df7c59b3 100644 --- a/utilities/joint-trajectory-player/src/Module.cpp +++ b/utilities/joint-trajectory-player/src/Module.cpp @@ -14,7 +14,6 @@ #include - #include #include @@ -31,18 +30,18 @@ double Module::getPeriod() bool Module::createPolydriver(std::shared_ptr handler) { + constexpr auto logPrefix = "[Module::createPolydriver]"; auto ptr = handler->getGroup("ROBOT_INTERFACE").lock(); if (ptr == nullptr) { - std::cerr << "[Module::createPolydriver] Robot interface options is empty." << std::endl; + log()->error("{} Unable to find the group ROBOT_INTERFACE.", logPrefix); return false; } ptr->setParameter("local_prefix", this->getName()); m_controlBoard = RobotInterface::constructRemoteControlBoardRemapper(ptr); if (!m_controlBoard.isValid()) { - std::cerr << "[Module::createPolydriver] the robot polydriver has not been constructed." - << std::endl; + log()->error("{} Unable to create the polydriver.", logPrefix); return false; } @@ -51,18 +50,15 @@ bool Module::createPolydriver(std::shared_ptr handler) { + constexpr auto logPrefix = "[Module::initializeRobotControl]"; if (!m_robotControl.initialize(handler->getGroup("ROBOT_CONTROL"))) { - std::cerr << "[Module::initializeRobotControl] Unable to initialize the " - "control board" - << std::endl; + log()->error("{} Unable to initialize the robot control.", logPrefix); return false; } if (!m_robotControl.setDriver(m_controlBoard.poly)) { - std::cerr << "[Module::initializeRobotControl] Unable to initialize the " - "control board" - << std::endl; + log()->error("{} Unable to set the driver.", logPrefix); return false; } @@ -71,10 +67,10 @@ bool Module::initializeRobotControl(std::shared_ptr handler) { + constexpr auto logPrefix = "[Module::instantiateSensorBridge]"; if (!m_sensorBridge.initialize(handler->getGroup("SENSOR_BRIDGE"))) { - std::cerr << "[Module::initializeSensorBridge] Unable to initialize the sensor bridge" - << std::endl; + log()->error("{} Unable to initialize the sensor bridge.", logPrefix); return false; } @@ -82,7 +78,7 @@ bool Module::instantiateSensorBridge(std::shared_ptrerror("{} Unable to set the drivers list.", logPrefix); return false; } @@ -91,13 +87,14 @@ bool Module::instantiateSensorBridge(std::shared_ptr data; matioCpp::File input(filename); if (!input.isOpen()) { - std::cout << "[Module::readStateFromFile] Failed to open " << filename << "." << std::endl; + log()->error("{} Unable to open the file {}.", logPrefix, filename); return false; } else { @@ -105,8 +102,7 @@ bool Module::readStateFromFile(const std::string& filename, const std::size_t nu // array named "traj" if (!m_traj.isValid()) { - std::cerr << "[Module::readStateFromFile] Error reading input file: " << filename << "." - << std::endl; + log()->error("{} Unable to read the trajectory from the file.", logPrefix); return false; } @@ -116,43 +112,50 @@ bool Module::readStateFromFile(const std::string& filename, const std::size_t nu bool Module::configure(yarp::os::ResourceFinder& rf) { + constexpr auto logPrefix = "[Module::configure]"; auto parametersHandler = std::make_shared(rf); std::string name; if (!parametersHandler->getParameter("name", name)) + { + log()->error("{} Unable to find the parameter 'name'.", logPrefix); return false; + } this->setName(name.c_str()); if (!parametersHandler->getParameter("sampling_time", m_dT)) + { + log()->error("{} Unable to find the parameter 'sampling_time'.", logPrefix); return false; + } std::string trajectoryFile; if (!parametersHandler->getParameter("trajectory_file", trajectoryFile)) { - std::cerr << "[Module::configure] Trajectory_file parameter not specified." << std::endl; + log()->error("{} Unable to find the parameter 'trajectory_file'.", logPrefix); return false; } if (!this->createPolydriver(parametersHandler)) { - std::cerr << "[Module::configure] Unable to create the polydriver." << std::endl; + log()->error("{} Unable to create the polydriver.", logPrefix); return false; } if (!this->initializeRobotControl(parametersHandler)) { - std::cerr << "[Module::configure] Unable to initialize the robotControl interface." << std::endl; + log()->error("{} Unable to initialize the robot control.", logPrefix); return false; } if (!this->instantiateSensorBridge(parametersHandler)) { - std::cerr << "[Module::configure] Unable toinitialize the sensorBridge interface." << std::endl; + log()->error("{} Unable to instantiate the sensor bridge.", logPrefix); return false; } m_numOfJoints = m_robotControl.getJointList().size(); if (m_numOfJoints == 0) { - std::cerr << "[Module::configure] No joints to control." << std::endl; + log()->error("{} Are you trying to control a robot without joints?", logPrefix); return false; } @@ -164,14 +167,26 @@ bool Module::configure(yarp::os::ResourceFinder& rf) if (!readStateFromFile(trajectoryFile, m_numOfJoints)) { + log()->error("{} Unable to read the trajectory from the file.", logPrefix); return false; } - std::cout << "[Module::configure] Starting the experiment." << std::endl; + log()->info("{} Module configured.", logPrefix); + + // switch to position control + if (!m_robotControl.setControlMode(RobotInterface::IRobotControl::ControlMode::Position)) + { + log()->error("{} Unable to set the control mode to position.", logPrefix); + return false; + } // Reach the first position of the desired trajectory in position control - m_robotControl.setReferences(Conversions::toEigen(m_traj).row(m_idxTraj), - RobotInterface::IRobotControl::ControlMode::Position); + if (!m_robotControl.setReferences(Conversions::toEigen(m_traj).row(m_idxTraj), + RobotInterface::IRobotControl::ControlMode::Position)) + { + log()->error("{} Error while setting the reference position.", logPrefix); + return false; + } m_state = State::positioning; @@ -180,6 +195,7 @@ bool Module::configure(yarp::os::ResourceFinder& rf) bool Module::updateModule() { + constexpr auto logPrefix = "[Module::updateModule]"; bool isMotionDone; bool isTimeExpired; std::vector> jointlist; @@ -189,23 +205,33 @@ bool Module::updateModule() case State::positioning: if (!m_robotControl.checkMotionDone(isMotionDone, isTimeExpired, jointlist)) { - std::cerr << "[Module::updateModule] Impossible to check if the motion is done." - << std::endl; + log()->error("{} Unable to check if the motion is done.", logPrefix); return false; } if (isTimeExpired) { - std::cerr << "[Module::updateModule] List of joints not finishing in time: " - << std::endl; + log()->error("{} The timer expired.", logPrefix); for (int i = 0; i < jointlist.size(); i++) { - std::cerr << "Joint " << jointlist[i].first << "--> Error " << jointlist[i].second - << " rad" << std::endl; + log()->error("{} Joint {} --> Error {} rad", + logPrefix, + jointlist[i].first, + jointlist[i].second); } return false; } if (isMotionDone) { + log()->info("{} Positioning done.", logPrefix); + + // switch in position direct control + if (!m_robotControl.setControlMode( + RobotInterface::IRobotControl::ControlMode::PositionDirect)) + { + log()->error("{} Unable to switch in position direct control.", logPrefix); + return false; + } + m_state = State::running; } break; @@ -213,25 +239,25 @@ bool Module::updateModule() case State::running: if (!m_sensorBridge.advance()) { - std::cerr << "[Module::updateModule] Unable to read the sensor." << std::endl; + log()->error("{} Unable to read the sensor.", logPrefix); return false; } if (!m_sensorBridge.getJointPositions(m_currentJointPos)) { - std::cerr << "[Module::updateModule] Error in reading current position." << std::endl; + log()->error("{} Unable to get the joint position.", logPrefix); return false; } if (!m_sensorBridge.getJointVelocities(m_currentJointVel)) { - std::cerr << "[Module::updateModule] Error in reading current velocity." << std::endl; + log()->error("{} Unable to get the joint velocity.", logPrefix); return false; } if (!m_sensorBridge.getMotorCurrents(m_currentMotorCurr)) { - std::cerr << "[Module::updateModule] Error in reading motor currents." << std::endl; + log()->error("{} Unable to get the motor currents.", logPrefix); return false; } @@ -256,8 +282,7 @@ bool Module::updateModule() m_idxTraj++; if (m_idxTraj == Conversions::toEigen(m_traj).rows()) { - std::cout << "[Module::updateModule] Experiment finished." << std::endl; - + log()->info("{} Trajectory ended.", logPrefix); return false; } @@ -266,16 +291,14 @@ bool Module::updateModule() .setReferences(Conversions::toEigen(m_traj).row(m_idxTraj), RobotInterface::IRobotControl::ControlMode::PositionDirect)) { - std::cerr << "[Module::updateModule] Error while setting the reference position to " - "iCub." - << std::endl; + log()->error("{} Unable to set the reference.", logPrefix); return false; } break; default: - std::cerr << "[Module::updateModule] The program is in an unknown state. Cannot proceed."; + log()->error("{} Unknown state.", logPrefix); return false; } @@ -284,7 +307,7 @@ bool Module::updateModule() bool Module::close() { - std::cout << "[Module::close] I'm storing the dataset." << std::endl; + log()->info("[Module::close] Storing the dataset."); // set the file name std::time_t t = std::time(nullptr); @@ -304,11 +327,14 @@ bool Module::close() file.write(out); } - std::cout << "[Module::close] Dataset stored. Closing." << std::endl; + log()->info("[Module::close] Dataset stored in {}.", fileName.str()); // switch back in position control - m_robotControl.setReferences(m_currentJointPos, - RobotInterface::IRobotControl::ControlMode::Position); + if (!m_robotControl.setControlMode(RobotInterface::IRobotControl::ControlMode::Position)) + { + log()->error("[Module::close] Unable to switch back in position control."); + return false; + } return true; } From 16091ec6ab8bb493d1a4e0823bade5790f7ae7af Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 5 Apr 2024 16:01:20 +0200 Subject: [PATCH 5/7] Update the CHANGELOG --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index fe8ccf0caa..02c217e107 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,7 @@ All notable changes to this project are documented in this file. - 🤖 [ergoCubSN001] Add logging of the wrist and fix the name of the waist imu (https://github.com/ami-iit/bipedal-locomotion-framework/pull/810) - Export the CoM velocity and the angular momentum trajectory in the `CentroidalMPC` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/818) - Require `iDynTree v10.0.0` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/832) +- Refactor `YarpRobotControl::setReferences` function to include optional current joint values and avoid to switch control mode in `YarpRobotControl::setReferences` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/833) ### Fixed - Fix the barrier logic for threads synchronization (https://github.com/ami-iit/bipedal-locomotion-framework/pull/811) From 2141adc4cdb4a282a3bfb0b8899bad976e53ad94 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 5 Apr 2024 16:32:44 +0200 Subject: [PATCH 6/7] Update bindings/python/RobotInterface/src/RobotControl.cpp Co-authored-by: Stefano Dafarra --- bindings/python/RobotInterface/src/RobotControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bindings/python/RobotInterface/src/RobotControl.cpp b/bindings/python/RobotInterface/src/RobotControl.cpp index 4220051475..9c148c891f 100644 --- a/bindings/python/RobotInterface/src/RobotControl.cpp +++ b/bindings/python/RobotInterface/src/RobotControl.cpp @@ -72,7 +72,7 @@ void CreateYarpRobotControl(pybind11::module& module) &YarpRobotControl::setReferences), py::arg("desired_joints_value"), py::arg("control_modes"), - py::arg("current_joints_value") = std::nullopt) + py::arg("current_joint_values") = std::nullopt) .def("set_references", py::overload_cast, const IRobotControl::ControlMode&, From 7b8f89d233a00b2bd70a1ba3abf6768b79132ea8 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 5 Apr 2024 16:32:51 +0200 Subject: [PATCH 7/7] Update bindings/python/RobotInterface/src/RobotControl.cpp Co-authored-by: Stefano Dafarra --- bindings/python/RobotInterface/src/RobotControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bindings/python/RobotInterface/src/RobotControl.cpp b/bindings/python/RobotInterface/src/RobotControl.cpp index 9c148c891f..13281eb65b 100644 --- a/bindings/python/RobotInterface/src/RobotControl.cpp +++ b/bindings/python/RobotInterface/src/RobotControl.cpp @@ -80,7 +80,7 @@ void CreateYarpRobotControl(pybind11::module& module) &YarpRobotControl::setReferences), py::arg("desired_joints_value"), py::arg("control_mode"), - py::arg("current_joints_value") = std::nullopt) + py::arg("current_joint_values") = std::nullopt) .def("set_control_mode", py::overload_cast&>( &YarpRobotControl::setControlMode),