Refactor YarpRobotControl::setReferences function to include optional current joint values and avoid to switch control mode in YarpRobotControl::setReferences

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)

bindings/python/RobotInterface/src/RobotControl.cpp

@@ -5,6 +5,8 @@
  * distributed under the terms of the BSD-3-Clause license.
  */
 
+#include <optional>
+
 #include <pybind11/eigen.h>
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
@@ -65,15 +67,20 @@ void CreateYarpRobotControl(pybind11::module& module)
  })
  .def("set_references",
  py::overload_cast<Eigen::Ref<const Eigen::VectorXd>,
- const std::vector<IRobotControl::ControlMode>&>(
+ const std::vector<IRobotControl::ControlMode>&,
+ std::optional<Eigen::Ref<const Eigen::VectorXd>>>(
  &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<Eigen::Ref<const Eigen::VectorXd>, const IRobotControl::ControlMode&>(
+ py::overload_cast<Eigen::Ref<const Eigen::VectorXd>,
+ const IRobotControl::ControlMode&,
+ std::optional<Eigen::Ref<const Eigen::VectorXd>>>(
  &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<const std::vector<IRobotControl::ControlMode>&>(
  &YarpRobotControl::setControlMode),

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

@@ -10,6 +10,7 @@
 
 // std
 #include <memory>
+#include <optional>
 
 // Eigen
 #include <Eigen/Dense>
@@ -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<const Eigen::VectorXd> jointValues,
- const std::vector<IRobotControl::ControlMode>& controlModes) final;
+ bool setReferences(Eigen::Ref<const Eigen::VectorXd> desiredJointValues,
+ const std::vector<IRobotControl::ControlMode>& controlModes,
+ std::optional<Eigen::Ref<const Eigen::VectorXd>> 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<IRobotControl::ControlMode>&).
  */
  bool setReferences(Eigen::Ref<const Eigen::VectorXd> desiredJointValues,
- const IRobotControl::ControlMode& mode) final;
+ const IRobotControl::ControlMode& mode,
+ std::optional<Eigen::Ref<const Eigen::VectorXd>> currentJointValues = {}) final;
 
  /**
  * Get the list of the controlled joints