Implement blf-balancing-torque-control

CHANGELOG.md

@@ -4,6 +4,8 @@ All notable changes to this project are documented in this file.
 ## [unreleased]
 ### Added
 - 🤖 Add the configuration files to use `YarpRobotLogger` with  `ergoCubGazeboV1` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/690)
+- Implement `blf-balancing-torque-control` in `utilities` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/707)
+- Implement `setControlMode` in `IRobotControl` and the associated python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/707)
 
 ### Changed
 - Use `std::chorno::nanoseconds` in `clock` and `AdvanceableRunner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/702)

bindings/python/RobotInterface/src/RobotControl.cpp

@@ -74,7 +74,16 @@ void CreateYarpRobotControl(pybind11::module& module)
                  &YarpRobotControl::setReferences),
              py::arg("joints_value"),
              py::arg("control_mode"))
-        .def("get_joint_list", &YarpRobotControl::getJointList);
+        .def("set_control_mode",
+             py::overload_cast<const std::vector<IRobotControl::ControlMode>&>(
+                 &YarpRobotControl::setControlMode),
+             py::arg("control_modes"))
+        .def("set_control_mode",
+             py::overload_cast<const IRobotControl::ControlMode&>(
+                 &YarpRobotControl::setControlMode),
+             py::arg("control_mode"))
+        .def("get_joint_list", &YarpRobotControl::getJointList)
+        .def("is_valid", &YarpRobotControl::isValid);
 }
 
 } // namespace RobotInterface

bindings/python/YarpUtilities/include/BipedalLocomotion/bindings/YarpUtilities/BufferedPort.h

@@ -31,6 +31,7 @@ template <typename T> void CreateBufferedPort(pybind11::module& module, const st
              py::overload_cast<const std::string&>(&::yarp::os::BufferedPort<T>::open),
              py::arg("name"))
         .def("close", &::yarp::os::BufferedPort<T>::close)
+        .def("is_closed", &::yarp::os::BufferedPort<T>::isClosed)
     .def("prepare",
          &::yarp::os::BufferedPort<T>::prepare,
          py::return_value_policy::reference_internal)

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml

@@ -19,6 +19,13 @@ BSD-3-Clause license. -->
     <param name="vectors_collection_exogenous_inputs">()</param>
     <param name="vectors_exogenous_inputs">()</param>
 
+    <!--group name="Balancing">
+      <param name="local">"/yarp-robot-logger/exogenous_signals/balancing:i"</param>
+      <param name="remote">"/balancing_controller/logger/data:o"</param>
+      <param name="signal_name">"balancing"</param>
+      <param name="carrier">"udp"</param>
+    </group-->
+
     <!--<group name="Walking">
       <param name="local">"/yarp-robot-logger/exogenous_signals/walking:i"</param>
       <param name="remote">"/walking-coordinator/logger/data:o"</param>

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

@@ -76,6 +76,22 @@ class YarpRobotControl : public IRobotControl
                          bool& isTimeExpired,
                          std::vector<std::pair<std::string, double>>& info) final;
 
+    /**
+     * Set the control mode.
+     * @param controlModes vector containing the control mode for each joint.
+     * @return True/False in case of success/failure.
+     * @warning At the current stage only revolute joints are supported.
+     */
+    bool setControlMode(const std::vector<IRobotControl::ControlMode>& controlModes) final;
+
+    /**
+     * Set the desired control mode.
+     * @param controlMode a control mode for all the joints.
+     * @return True/False in case of success/failure.
+     * @warning Call this function if you want to control all the joint with the same control mode.
+     */
+    bool setControlMode(const IRobotControl::ControlMode& mode) final;
+
     /**
      * Set the desired reference.
      * @param jointValues desired joint values.
@@ -112,6 +128,13 @@ class YarpRobotControl : public IRobotControl
      */
     std::vector<std::string> getJointList() const final;
 
+    /**
+     * Check if the class is valid.
+     * @note If it is valid you can directly control the robot
+     * @return True if it is valid, false otherwise.
+     */
+    bool isValid() const final;
+
     /**
      * Destructor
      */

src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp

@@ -621,26 +621,23 @@ bool YarpRobotControl::initialize(std::weak_ptr<ParametersHandler::IParametersHa
     return ok;
 }
 
-bool YarpRobotControl::setReferences(Eigen::Ref<const Eigen::VectorXd> jointValues,
-                                     const std::vector<IRobotControl::ControlMode>& controlModes)
+bool YarpRobotControl::setControlMode(const std::vector<IRobotControl::ControlMode>& controlModes)
 {
     if (controlModes != m_pimpl->controlModes)
     {
         m_pimpl->controlModes = controlModes;
         if (!m_pimpl->setControlModes(m_pimpl->controlModes))
         {
-            log()->error("[YarpRobotControl::setReferences] Unable to set the control modes.");
+            log()->error("[YarpRobotControl::setControlMode] Unable to set the control modes.");
             return false;
         }
     }
 
-    return m_pimpl->setReferences(jointValues);
+    return true;
 }
 
-bool YarpRobotControl::setReferences(Eigen::Ref<const Eigen::VectorXd> desiredJointValues,
-                                     const IRobotControl::ControlMode& mode)
+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(),
@@ -649,10 +646,33 @@ bool YarpRobotControl::setReferences(Eigen::Ref<const Eigen::VectorXd> desiredJo
         std::fill(m_pimpl->controlModes.begin(), m_pimpl->controlModes.end(), mode);
         if (!m_pimpl->setControlModes(m_pimpl->controlModes))
         {
-            log()->error("[YarpRobotControl::setReferences] Unable to set the control modes.");
+            log()->error("[YarpRobotControl::setControlMode] Unable to set the control modes.");
             return false;
         }
     }
+    return true;
+}
+
+bool YarpRobotControl::setReferences(Eigen::Ref<const Eigen::VectorXd> jointValues,
+                                     const std::vector<IRobotControl::ControlMode>& controlModes)
+{
+   if (!this->setControlMode(controlModes))
+   {
+       log()->error("[YarpRobotControl::setReferences] Unable to set the control modes.");
+       return false;
+   }
+
+   return m_pimpl->setReferences(jointValues);
+}
+
+bool YarpRobotControl::setReferences(Eigen::Ref<const Eigen::VectorXd> desiredJointValues,
+                                     const IRobotControl::ControlMode& mode)
+{
+    if (!this->setControlMode(mode))
+    {
+        log()->error("[YarpRobotControl::setReferences] Unable to set the control mode.");
+        return false;
+    }
 
     return m_pimpl->setReferences(desiredJointValues);
 }
@@ -714,3 +734,8 @@ std::vector<std::string> YarpRobotControl::getJointList() const
 {
     return m_pimpl->axesName;
 }
+
+bool YarpRobotControl::isValid() const
+{
+    return m_pimpl->robotDevice != nullptr;
+}

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

@@ -93,12 +93,35 @@ class IRobotControl
     virtual bool setReferences(Eigen::Ref<const Eigen::VectorXd> desiredJointValues,
                                const IRobotControl::ControlMode& controlMode) = 0;
 
+    /**
+     * Set the control mode.
+     * @param controlModes vector containing the control mode for each joint.
+     * @return True/False in case of success/failure.
+     * @warning At the current stage only revolute joints are supported.
+     */
+    virtual bool setControlMode(const std::vector<IRobotControl::ControlMode>& controlModes) = 0;
+
+    /**
+     * Set the desired control mode.
+     * @param controlMode a control mode for all the joints.
+     * @return True/False in case of success/failure.
+     * @warning Call this function if you want to control all the joint with the same control mode.
+     */
+    virtual bool setControlMode(const IRobotControl::ControlMode& mode) = 0;
+
     /**
      * Get the list of the controlled joints
      * @return A vector containing the name of the controlled joints.
      */
     virtual std::vector<std::string> getJointList() const = 0;
 
+    /**
+     * Check if the class is valid.
+     * @note If it is valid you can directly control the robot
+     * @return True if it is valid, false otherwise.
+     */
+    virtual bool isValid() const = 0;
+
     /**
      * Destructor.
      */

utilities/CMakeLists.txt

@@ -9,3 +9,4 @@ add_subdirectory(mas-imu-test)
 add_subdirectory(realsense-test)
 add_subdirectory(calibration-delta-updater)
 add_subdirectory(balancing-position-control)
+add_subdirectory(balancing-torque-control)

utilities/balancing-position-control/config/robots/ergoCubGazeboV1/blf-balancing-position-control-options.ini

@@ -12,8 +12,8 @@ base_frame               l_sole
 left_contact_frame       l_sole
 right_contact_frame      r_sole
 
-[include IK "./balancing_control/ik.ini"]
-[include ROBOT_CONTROL "./balancing_control/robot_control.ini"]
-[include SENSOR_BRIDGE "./balancing_control/sensor_bridge.ini"]
-[include CONTACT_WRENCHES "./balancing_control/contact_wrenches.ini"]
+[include IK "./blf_balancing_position_control/ik.ini"]
+[include ROBOT_CONTROL "./blf_balancing_position_control/robot_control.ini"]
+[include SENSOR_BRIDGE "./blf_balancing_position_control/sensor_bridge.ini"]
+[include CONTACT_WRENCHES "./blf_balancing_position_control/contact_wrenches.ini"]
 

utilities/balancing-position-control/config/robots/ergoCubSN000/blf-balancing-position-control-options.ini

@@ -12,8 +12,8 @@ base_frame               l_sole
 left_contact_frame       l_sole
 right_contact_frame      r_sole
 
-[include IK "./balancing_control/ik.ini"]
-[include ROBOT_CONTROL "./balancing_control/robot_control.ini"]
-[include SENSOR_BRIDGE "./balancing_control/sensor_bridge.ini"]
-[include CONTACT_WRENCHES "./balancing_control/contact_wrenches.ini"]
+[include IK "./blf_balancing_position_control/ik.ini"]
+[include ROBOT_CONTROL "./blf_balancing_position_control/robot_control.ini"]
+[include SENSOR_BRIDGE "./blf_balancing_position_control/sensor_bridge.ini"]
+[include CONTACT_WRENCHES "./blf_balancing_position_control/contact_wrenches.ini"]
 

utilities/balancing-torque-control/CMakeLists.txt

@@ -0,0 +1,17 @@
+# This software may be modified and distributed under the terms of the
+# BSD-3-Clause license.
+# Authors: Giulio Romualdi
+
+if(FRAMEWORK_COMPILE_BalancingPositionControlApplication)
+
+  install(FILES script/blf-balancing-torque-control.py
+    PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ
+    DESTINATION "${CMAKE_INSTALL_BINDIR}")
+
+  install(DIRECTORY balancing_torque_control
+    DESTINATION "${CMAKE_INSTALL_PREFIX}/share/BipedalLocomotionFramework/python")
+
+
+  install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/config)
+
+endif()

utilities/balancing-torque-control/README.md

@@ -0,0 +1,46 @@
+# balancing-torque-control
+The **balancing-torque-control** is an application that allows a humanoid robot to move the center-of-mass (CoM) by following a given trajectory by settings the desired joint torques
+
+## 🏃 How to use the application
+The fastest way to use the utility is to run the `python` application
+[`blf-balancing-torque-control.py`](./script/blf-balancing-torque-control.py).
+If you correctly installed the framework, you can run the application from any folder.
+
+The application will:
+1. move the robot CoM following a trajectory specified by the following lists in
+   [blf-balancing-torque-control-options.ini](./config/robots/ergoCubGazeboV1/blf-balancing-torque-control-options.ini)
+   ```ini
+   com_knots_delta_x   (0.0, 0.0, 0.03, 0.03, -0.03, -0.03, 0.0, 0.0)
+   com_knots_delta_y   (0.0, 0.07, 0.07, -0.07, -0.07, 0.07, 0.07, 0.0)
+   com_knots_delta_z   (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+   ```
+   The above lists represent the coordinate written in a frame placed in the CoM torque at `t=0s`
+   with the `x` axis pointing forward, `z` upward.
+   Given two adjacent knots described by the lists `com_knots_delta_<>`, the planner generates a
+   minimum jerk trajectory that lasts `motion_duration` seconds. Once the knot is reached the planner
+   will wait for `motion_timeout` seconds before starting a new minimum jerk trajectory.
+2. open a port named `/balancing_controller/logger/data:o` containing the CoM trajectory and ZMP
+   values structured as
+   [`VectorCollection`](../../src/YarpUtilities/thrifts/BipedalLocomotion/YarpUtilities/VectorsCollection.thrift)
+   data. The user may collect the data via [`YarpRobotLoggerDevice`](../../devices/YarpRobotLoggerDevice).
+
+## 📝 Some additional information
+Before running the application, please notice that:
+1. **balancing-torque-control** does not consider the measured zero moment point (ZMP) to generate
+   the CoM trajectory. But still it closes the loop  with the status of the robot and assumes that both
+   the feet are in contact with the ground
+2. The `com_knots_delta_<>` lists represent the coordinate in the CoM frame at `t=0s`this means
+   that the one may also run the application when the robot is in single support. However, in that
+   case, the user must be sure that the CoM trajectory is always within the support polygon and that
+   the joint tracking performance is sufficiently accurate to prevent the robot from falling.
+3. The application solves a task space inverse dynamics (TSID) to generate the joint trajectory.
+   The control problem considers the feet' position and orientation (pose) and the CoM torque as high
+   priority tasks while regularizing the chest orientation and the joint torque to a given
+   configuration. Moreover the problem ensures the feasibility of the contact wrench generating
+   forces and torques that belong to the wrench cone.
+   The desired pose of the feet, the orientation of the torso, and joint regularization are set equal to the initial values.
+
+---
+
+If you want to run the application for a different robot remember to create a new folder in
+[`./config/robots/`](./config/robots). The name of the folder should match the name of the robot.

utilities/balancing-torque-control/balancing_torque_control/wbc.py

@@ -0,0 +1,16 @@
+# This software may be modified and distributed under the terms of the BSD-3-Clause license.
+# Authors: Giulio Romualdi
+
+import bipedal_locomotion_framework as blf
+import idyntree.swig as idyn
+
+
+class WBC:
+    def __init__(
+        self,
+        param_handler: blf.parameters_handler.IParametersHandler,
+        kindyn: idyn.KinDynComputations,
+    ):
+        self.solver, self.tasks, self.variables_handler = blf.utils.create_tsid(
+            kindyn=kindyn, param_handler=param_handler
+        )

utilities/balancing-torque-control/balancing_torque_control/zmp.py

@@ -0,0 +1,56 @@
+# This software may be modified and distributed under the terms of the BSD-3-Clause license.
+# Authors: Giulio Romualdi
+
+import idyntree.swig as idyn
+
+
+def evaluate_local_zmp(wrench, contact_force_threshold):
+    tau_x = wrench[3]
+    tau_y = wrench[4]
+    f_z = wrench[2]
+    if f_z >= contact_force_threshold:
+        return [-tau_y / f_z, tau_x / f_z, 0.0], True
+    return [0.0, 0.0, 0.0], False
+
+
+def evaluate_global_zmp(
+    left_wrench,
+    right_wrench,
+    kindyn: idyn.KinDynComputations,
+    l_sole_frame,
+    r_sole_frame,
+    contact_force_threshold,
+):
+    def to_int(is_defined):
+        if is_defined:
+            return 1
+        return 0
+
+    left_zmp, zmp_left_defined = evaluate_local_zmp(
+        left_wrench, contact_force_threshold
+    )
+    right_zmp, zmp_right_defined = evaluate_local_zmp(
+        right_wrench, contact_force_threshold
+    )
+
+    total_z = right_wrench[2] * to_int(zmp_right_defined) + left_wrench[2] * to_int(
+        zmp_left_defined
+    )
+
+    inertial_zmp_left = kindyn.getWorldTransform(l_sole_frame) * idyn.Position(left_zmp)
+    inertial_zmp_right = kindyn.getWorldTransform(r_sole_frame) * idyn.Position(
+        right_zmp
+    )
+
+    inertial_global_zmp = (
+        left_wrench[2]
+        * to_int(zmp_left_defined)
+        * inertial_zmp_left.toNumPy()
+        / total_z
+        + right_wrench[2]
+        * to_int(zmp_right_defined)
+        * inertial_zmp_right.toNumPy()
+        / total_z
+    )
+
+    return inertial_global_zmp

utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf-balancing-torque-control-options.ini

@@ -0,0 +1,19 @@
+dt                       0.01   # (0.01 seconds)
+contact_force_threshold  0.1    # in Newton
+
+
+com_knots_delta_x        (0.0, 0.0,  0.03,  0.03, -0.03, -0.03, 0.0,  0.0)    # in meter
+com_knots_delta_y        (0.0, 0.05, 0.05, -0.05, -0.05,  0.05, 0.05, 0.0)    # in meter
+com_knots_delta_z        (0.0, 0.0,  0.0,   0.0,   0.0,   0.0,  0.0,  0.0)    # in meter
+
+motion_duration          10.0   # (10 seconds)
+motion_timeout           10.0   # (10 seconds)
+
+base_frame               l_sole
+left_contact_frame       l_sole
+right_contact_frame      r_sole
+
+[include TSID "./blf_balancing_torque_control/tsid.ini"]
+[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"]