Add the possibility to set the fixed joint configuration in balancing-torque-control app (#880)

Author: GiulioRomualdi

.github/workflows/ci.yml

@@ -14,7 +14,7 @@ env:
   vcpkg_robotology_TAG: v0.11.0
   YCM_TAG: v0.15.3
   YARP_TAG: v3.8.0
-  iDynTree_TAG: 42f2874b729348575aeee723c1775c3425735ef9
+  iDynTree_TAG: v12.2.1
   CasADi_TAG: 3.5.5.2
   manif_TAG: 0.0.5
   matioCpp_TAG: v0.2.0

.github/workflows/conda-forge-ci.yml

@@ -37,7 +37,7 @@ jobs:
         conda config --remove channels defaults
         # Dependencies
         conda install cmake compilers make ninja pkg-config \
-                      "idyntree>=8.0.0" "yarp>=3.5.0" libmatio libmatio-cpp librobometry \
+                      "idyntree>=12.2.1" "yarp>=3.5.0" libmatio libmatio-cpp librobometry \
                       liblie-group-controllers eigen qhull "casadi>=3.5.5" cppad spdlog \
                       nlohmann_json manif manifpy pybind11 numpy pytest scipy opencv pcl \
                       tomlplusplus libunicycle-footstep-planner "icub-models>=1.23.4" \

CHANGELOG.md

@@ -28,6 +28,7 @@ All notable changes to this project are documented in this file.
 - Add the possibility to disable streaming of joint encoder acceleration measurements (https://github.com/ami-iit/bipedal-locomotion-framework/pull/876)
 - Implement joint torque control device and friction estimation through PINN (https://github.com/ami-iit/bipedal-locomotion-framework/pull/866)
 - Add a task in the IK to set the joint velocity limits (https://github.com/ami-iit/bipedal-locomotion-framework/pull/879)
+- Add the possibility to set the fixed joint configuration in `balancing-torque-control` app (https://github.com/ami-iit/bipedal-locomotion-framework/pull/880)
 
 ### Changed
 - 🤖 [ergoCubSN001] Add logging of the wrist and fix the name of the waist imu (https://github.com/ami-iit/bipedal-locomotion-framework/pull/810)

cmake/BipedalLocomotionFrameworkDependencies.cmake

@@ -17,8 +17,8 @@ endif()
 ################################################################################
 ########################## Mandatory dependencies ##############################
 
-find_package(iDynTree 10.0.0 REQUIRED)
-dependency_classifier(iDynTree MINIMUM_VERSION 10.0.0 IS_USED TRUE PUBLIC)
+find_package(iDynTree 12.2.1 REQUIRED)
+dependency_classifier(iDynTree MINIMUM_VERSION 12.2.1 IS_USED TRUE PUBLIC)
 
 find_package(Eigen3 3.2.92 REQUIRED)
 dependency_classifier(Eigen3 MINIMUM_VERSION 3.2.92 IS_USED TRUE PUBLIC)

utilities/balancing-torque-control/README.md

@@ -1,46 +1,71 @@
 # 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 **balancing-torque-control** application allows a humanoid robot to move its center-of-mass
+(CoM) by following a given trajectory by setting 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.
+[`blf-balancing-torque-control.py`](./script/blf-balancing-torque-control.py). If the framework is
+correctly installed, 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)
+
+1. Move the robot's CoM by following a trajectory specified in the lists found 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
+   These lists represent coordinates in a frame located at the CoM at `t=0s`, with the `x` axis
+   pointing forward and the `z` axis upward. Between adjacent knots described by the
+   `com_knots_delta_<>` lists, the planner generates a minimum jerk trajectory that lasts
+   `motion_duration` seconds. Once the knot is reached, the planner waits 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).
+   data. The user may collect this data via
+   [`YarpRobotLoggerDevice`](../../devices/YarpRobotLoggerDevice).
+
+
+## 📝 Additional Information
+
+Before running the application, please note:
 
-## 📝 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.
+   the CoM trajectory. However, it still closes the loop using the robot's status and assumes both
+   feet are in contact with the ground.
+
+2. The `com_knots_delta_<>` lists represent the coordinates in the CoM frame at `t=0s`. This means
+   that the application can also be run when the robot is in single support. However, in that case,
+   the user must ensure that the CoM trajectory remains within the support polygon and that joint
+   tracking performance is sufficiently accurate to prevent the robot from falling.
+
+3. The application solves a task-space inverse dynamics (TSID) problem to generate the joint
+   trajectory. The control problem prioritizes the feet's position and orientation (pose) and the
+   CoM torque while regularizing the chest orientation and joint torque to a desired
+   configuration. The problem also ensures that the contact wrenches are feasible, generating forces
+   and torques within the wrench cone.
+   The desired pose of the feet, torso orientation, and joint regularization are set to the initial
+   values.
+
+4. The list of controlled joints can be found in the configuration file
+   [`robot_control.ini`](./config/robots/ergoCubGazeboV1/blf_balancing_torque_control/robot_control.ini).
+   Removing a joint from the `joints_list` will exclude it from the control problem. The user must
+   also adjust the `kp`, `kd`, and `weight` values in the `[JOINT_REGULARIZATION_TASK]` section to
+   match the new joint list in
+   [`tsid.ini`](./config/robots/ergoCubGazeboV1/blf_balancing_torque_control/tsid.ini). By default,
+   if a joint is not in the `joints_list`, its position is considered to be zero. This behavior can
+   be changed by setting the `fixed_joint_list_names` and `fixed_joint_list_values` in the
+   [`robot_control.ini`](./config/robots/ergoCubGazeboV1/blf_balancing_torque_control/robot_control.ini)
+   file. Specifically, `fixed_joint_list_names` is a list of joint names not in the `joints_list`,
+   and `fixed_joint_list_values` contains the corresponding joint positions (in degrees).
 
 ---
 
-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.
+If you want to run the application for a different robot, remember to create a new folder in
+[`./config/robots/`](./config/robots). The folder name should match the name of the robot.

utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf_balancing_torque_control/robot_control.ini

@@ -7,6 +7,21 @@ joints_list                             ("torso_pitch", "torso_roll", "torso_yaw
                                          "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")
 
 remote_control_boards                   ("torso", "left_arm", "right_arm", "left_leg", "right_leg")
+
 positioning_duration                    3.0  #in seconds
-positioning_tolerance                   0.05 #in seconds
-position_direct_max_admissible_error    0.1  #in seconds
+positioning_tolerance                   0.05 #in radians
+position_direct_max_admissible_error    0.1  #in radians
+
+# List of the names of the fixed joints. Specify here the joints that you will consider as fixed.
+# The joints in this list cannot be part of the controlled joints list (joints_list).
+# If a joint is present in both lists, an error will occur. The purpose of this list is to define a
+# specific configuration for the fixed joints that differs from the default value (0.0 deg).
+# If a joint is not included in this list or the controlled joints list, it will be considered fixed with a default value of 0.0 deg.
+#
+# Below is an example of how to specify a fixed joint with a value other than 0.0 deg:
+#
+# fixed_joint_list_names                ("torso_pitch")  # List of the fixed joint names
+# fixed_joint_list_values               (10.0)           # List of the fixed joint values (in degrees)
+
+fixed_joint_list_names                 ()
+fixed_joint_list_values                ()

utilities/balancing-torque-control/script/blf-balancing-torque-control.py

@@ -313,8 +313,33 @@ def build_kin_dyn(self, param_handler):
         joint_list = param_handler.get_group(
             "ROBOT_CONTROL"
         ).get_parameter_vector_string("joints_list")
+
+        try:
+            fixed_joints_list = param_handler.get_group(
+                "ROBOT_CONTROL"
+            ).get_parameter_vector_string("fixed_joint_list_names")
+
+            fixed_joints_values = param_handler.get_group(
+                "ROBOT_CONTROL"
+            ).get_parameter_vector_float("fixed_joint_list_values")
+        except:
+            blf.log().warning("No fixed joints are provided.")
+            fixed_joints_list = []
+            fixed_joints_values = []
+
+        # check if the length of the fixed joints is correct
+        if len(fixed_joints_list) != len(fixed_joints_values):
+            raise ValueError(
+                "The length of the fixed joints list and values is different."
+            )
+
+        # create a dictionary with the fixed joints
+        fixed_joints = dict()
+        for joint_name, joint_value in zip(fixed_joints_list, fixed_joints_values):
+            fixed_joints[joint_name] = np.deg2rad(joint_value)
+
         ml = idyn.ModelLoader()
-        ml.loadReducedModelFromFile(robot_model_path, joint_list)
+        ml.loadReducedModelFromFile(robot_model_path, joint_list, fixed_joints)
 
         kindyn = idyn.KinDynComputations()
         kindyn.loadRobotModel(ml.model())