From 01c686c00343ce6a90fcb21d9a11fe8aaead37e4 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 6 Sep 2024 12:26:44 +0200 Subject: [PATCH] Add the possibility to set the fixed joint configuration in balancing-torque-control app (#880) --- .github/workflows/ci.yml | 2 +- .github/workflows/conda-forge-ci.yml | 2 +- CHANGELOG.md | 1 + ...pedalLocomotionFrameworkDependencies.cmake | 4 +- utilities/balancing-torque-control/README.md | 83 ++++++++++++------- .../robot_control.ini | 19 ++++- .../script/blf-balancing-torque-control.py | 27 +++++- 7 files changed, 102 insertions(+), 36 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 0881305edd..8964e4c9ac 100644 --- a/.github/workflows/ci.yml +++ b/.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 diff --git a/.github/workflows/conda-forge-ci.yml b/.github/workflows/conda-forge-ci.yml index 71b4687de3..d76615ae02 100644 --- a/.github/workflows/conda-forge-ci.yml +++ b/.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" \ diff --git a/CHANGELOG.md b/CHANGELOG.md index fc0f80bdad..95278b7aa5 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/cmake/BipedalLocomotionFrameworkDependencies.cmake b/cmake/BipedalLocomotionFrameworkDependencies.cmake index dc59b95afc..4490e1b724 100644 --- a/cmake/BipedalLocomotionFrameworkDependencies.cmake +++ b/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) diff --git a/utilities/balancing-torque-control/README.md b/utilities/balancing-torque-control/README.md index 66b6406b70..6199fadb33 100644 --- a/utilities/balancing-torque-control/README.md +++ b/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. diff --git a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf_balancing_torque_control/robot_control.ini b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf_balancing_torque_control/robot_control.ini index d2ac34a2c7..09a8f605a2 100644 --- a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1/blf_balancing_torque_control/robot_control.ini +++ b/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 () 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 2d49bf46c4..02539bc9e2 100755 --- a/utilities/balancing-torque-control/script/blf-balancing-torque-control.py +++ b/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())