From 62bca49b40709fd36bbf0c551df47bc71cf70cb3 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Wed, 16 Oct 2024 17:52:14 +0200 Subject: [PATCH 01/10] balancing only with legs --- .../ergoCubGazeboV1_1/yarp-robot-logger.xml | 9 +- .../blf-balancing-torque-control-options.ini | 24 +++++ .../contact_wrenches.ini | 22 +++++ .../data_logging.ini | 1 + .../robot_control.ini | 24 +++++ .../sensor_bridge.ini | 7 ++ .../tasks/base_dynamics.ini | 15 +++ .../tasks/joint_dynamics.ini | 14 +++ .../blf_balancing_torque_control/tsid.ini | 95 +++++++++++++++++++ .../robots/ergoCubGazeboV1_1/world/world.sdf | 84 ++++++++++++++++ .../script/blf-balancing-torque-control.py | 8 ++ 11 files changed, 302 insertions(+), 1 deletion(-) create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf-balancing-torque-control-options.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/contact_wrenches.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/data_logging.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/robot_control.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/sensor_bridge.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/tasks/base_dynamics.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/tasks/joint_dynamics.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/tsid.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/world/world.sdf diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml index 6caefa65ff..8f76b43f0d 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml @@ -17,7 +17,7 @@ BSD-3-Clause license. --> - ("Walking", "CurrentControl") + ("Walking", "CurrentControl", "BalancingTSID") () @@ -33,6 +33,13 @@ BSD-3-Clause license. --> "current_control" "udp" + + + "/yarp-robot-logger/exogenous_signals/balancing_tsid" + "/balancing_torque_controller/logger" + "balancing_tsid" + "udp" + diff --git a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf-balancing-torque-control-options.ini b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf-balancing-torque-control-options.ini new file mode 100644 index 0000000000..d662a88fb9 --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf-balancing-torque-control-options.ini @@ -0,0 +1,24 @@ +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 + +; com_knots_delta_x (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # in meter +; com_knots_delta_y (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # in meter +; com_knots_delta_z (0.0, 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"] +[include DATA_LOGGING "./blf_balancing_torque_control/data_logging.ini"] diff --git a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/contact_wrenches.ini b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/contact_wrenches.ini new file mode 100644 index 0000000000..2c47ec5a9d --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/contact_wrenches.ini @@ -0,0 +1,22 @@ +left_contact_wrenches_group ("LEFT_FOOT_FRONT", "LEFT_FOOT_REAR") +right_contact_wrenches_group ("RIGHT_FOOT_FRONT", "RIGHT_FOOT_REAR") + +[LEFT_FOOT_FRONT] +description "left_foot_front" +remote_port_name "/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o" +local_port_name_postfix "/left_foot_front/cartesianEndEffectorWrench:i" + +[LEFT_FOOT_REAR] +description "left_foot_rear" +remote_port_name "/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o" +local_port_name_postfix "/left_foot_rear/cartesianEndEffectorWrench:i" + +[RIGHT_FOOT_FRONT] +description "right_foot_front" +remote_port_name "/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o" +local_port_name_postfix "/right_foot_front/cartesianEndEffectorWrench:i" + +[RIGHT_FOOT_REAR] +description "right_foot_rear" +remote_port_name "/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o" +local_port_name_postfix "/right_foot_rear/cartesianEndEffectorWrench:i" diff --git a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/data_logging.ini b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/data_logging.ini new file mode 100644 index 0000000000..8d88ff6d49 --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/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_1/blf_balancing_torque_control/robot_control.ini b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/robot_control.ini new file mode 100644 index 0000000000..0fb586541c --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/robot_control.ini @@ -0,0 +1,24 @@ +robot_name ergocubSim + +joints_list ("l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", + "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + +remote_control_boards ("left_leg", "right_leg") + +positioning_duration 3.0 #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/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/sensor_bridge.ini b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/sensor_bridge.ini new file mode 100644 index 0000000000..c23838208a --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/sensor_bridge.ini @@ -0,0 +1,7 @@ +check_for_nan false +stream_joint_states true +stream_joint_accelerations false +stream_cartesian_wrenches true + +[CartesianWrenches] +cartesian_wrenches_list ("left_foot_front", "left_foot_rear", "right_foot_front", "right_foot_rear") diff --git a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/tasks/base_dynamics.ini b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/tasks/base_dynamics.ini new file mode 100644 index 0000000000..7c4e1845f2 --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/tasks/base_dynamics.ini @@ -0,0 +1,15 @@ +name "joint_dynamics_task" +type "JointDynamicsTask" +priority 0 + +robot_acceleration_variable_name "robot_acceleration" +joint_torques_variable_name "joint_torques" +max_number_of_contacts 2 + +[CONTACT_0] +variable_name "lf_wrench" +frame_name "l_sole" + +[CONTACT_1] +variable_name "rf_wrench" +frame_name "r_sole" diff --git a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/tasks/joint_dynamics.ini b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/tasks/joint_dynamics.ini new file mode 100644 index 0000000000..99192cb806 --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/tasks/joint_dynamics.ini @@ -0,0 +1,14 @@ +name "base_dynamics_task" +type "BaseDynamicsTask" +priority 0 + +robot_acceleration_variable_name "robot_acceleration" +max_number_of_contacts 2 + +[CONTACT_0] +variable_name "lf_wrench" +frame_name "l_sole" + +[CONTACT_1] +variable_name "rf_wrench" +frame_name "r_sole" diff --git a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/tsid.ini b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/tsid.ini new file mode 100644 index 0000000000..ca5fc5d76c --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/tsid.ini @@ -0,0 +1,95 @@ +tasks ("COM_TASK", "RIGHT_FOOT_TASK", "LEFT_FOOT_TASK", + "JOINT_REGULARIZATION_TASK", "TORSO_TASK", + "JOINT_DYNAMICS_TASK", "BASE_DYNAMICS_TASK", + "LF_WRENCH_TASK", "RF_WRENCH_TASK") + +[VARIABLES] +variables_name ("robot_acceleration", "joint_torques", "lf_wrench", "rf_wrench") +variables_size (18, 12, 6, 6) + +[TSID] +robot_acceleration_variable_name "robot_acceleration" +joint_torques_variable_name "joint_torques" +contact_wrench_variables_name ("lf_wrench", "rf_wrench") + +[COM_TASK] +name com_task +type CoMTask +robot_acceleration_variable_name "robot_acceleration" +kp_linear 100.0 +kd_linear 2.0 +mask (true, true, true) +priority 0 + +[RIGHT_FOOT_TASK] +name right_foot_task +type SE3Task +robot_acceleration_variable_name "robot_acceleration" +frame_name "r_sole" +kp_linear 100.0 +kp_angular 100.0 +kd_linear 2.0 +kd_angular 2.0 +priority 0 + +[LEFT_FOOT_TASK] +name left_foot_task +type SE3Task +robot_acceleration_variable_name "robot_acceleration" +frame_name "l_sole" +kp_linear 100.0 +kp_angular 100.0 +kd_linear 2.0 +kd_angular 2.0 +priority 0 + +[TORSO_TASK] +name torso_task +type SO3Task +robot_acceleration_variable_name "robot_acceleration" +frame_name "chest" +kp_angular 100.0 +kd_angular 2.0 +priority 1 +weight (5.0, 5.0, 5.0) + +[JOINT_REGULARIZATION_TASK] +name joint_regularization_task +type JointTrackingTask +robot_acceleration_variable_name "robot_acceleration" +kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +kd (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +priority 1 +weight (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +[LF_WRENCH_TASK] +name "lf_feasibility_wrench_task" +type "FeasibleContactWrenchTask" +priority 0 + +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) + +[RF_WRENCH_TASK] +name "rf_feasibility_wrench_task" +type "FeasibleContactWrenchTask" +priority 0 + +variable_name "rf_wrench" +frame_name "r_sole" +number_of_slices 2 +static_friction_coefficient 0.3 +foot_limits_x (-0.1, 0.1) +foot_limits_y (-0.05, 0.05) + +[include BASE_DYNAMICS_TASK ./tasks/base_dynamics.ini] +[include JOINT_DYNAMICS_TASK ./tasks/joint_dynamics.ini] diff --git a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/world/world.sdf b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/world/world.sdf new file mode 100644 index 0000000000..f88072e168 --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/world/world.sdf @@ -0,0 +1,84 @@ + + + + + + 0.001 + 0.4 + 400 + + + + + + + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + model://ergoCub/robots/ergoCubGazeboV1_1 + + + + + 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 02539bc9e2..14229a2f63 100755 --- a/utilities/balancing-torque-control/script/blf-balancing-torque-control.py +++ b/utilities/balancing-torque-control/script/blf-balancing-torque-control.py @@ -242,6 +242,9 @@ def __init__(self, config_file: str): self.vectors_collection_server.populate_metadata( "com_from_desired", ["x", "y", "z"] ) + self.vectors_collection_server.populate_metadata( + "com_reference", ["x", "y", "z"] + ) self.vectors_collection_server.populate_metadata( "com_from_measured", ["x", "y", "z"] ) @@ -510,6 +513,11 @@ def advance(self): self.vectors_collection_server.populate_data( "com_from_desired", com_from_desired ) + + self.vectors_collection_server.populate_data( + "com_reference", com_spline_output.position + ) + self.vectors_collection_server.populate_data( "com_from_measured", com_from_measured ) From 9598122976854e06672018f8619629431026ef62 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Wed, 16 Oct 2024 17:56:15 +0200 Subject: [PATCH 02/10] prepare config files for ergocubSN001 --- .../blf-balancing-torque-control-options.ini | 24 +++++ .../contact_wrenches.ini | 22 +++++ .../data_logging.ini | 1 + .../robot_control.ini | 24 +++++ .../sensor_bridge.ini | 7 ++ .../tasks/base_dynamics.ini | 15 +++ .../tasks/joint_dynamics.ini | 14 +++ .../blf_balancing_torque_control/tsid.ini | 95 +++++++++++++++++++ 8 files changed, 202 insertions(+) create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubSN001/blf-balancing-torque-control-options.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/contact_wrenches.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/data_logging.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/sensor_bridge.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tasks/base_dynamics.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tasks/joint_dynamics.ini create mode 100644 utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf-balancing-torque-control-options.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf-balancing-torque-control-options.ini new file mode 100644 index 0000000000..e38e587a68 --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf-balancing-torque-control-options.ini @@ -0,0 +1,24 @@ +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 + +com_knots_delta_x (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # in meter +com_knots_delta_y (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # in meter +com_knots_delta_z (0.0, 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"] +[include DATA_LOGGING "./blf_balancing_torque_control/data_logging.ini"] diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/contact_wrenches.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/contact_wrenches.ini new file mode 100644 index 0000000000..2c47ec5a9d --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/contact_wrenches.ini @@ -0,0 +1,22 @@ +left_contact_wrenches_group ("LEFT_FOOT_FRONT", "LEFT_FOOT_REAR") +right_contact_wrenches_group ("RIGHT_FOOT_FRONT", "RIGHT_FOOT_REAR") + +[LEFT_FOOT_FRONT] +description "left_foot_front" +remote_port_name "/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o" +local_port_name_postfix "/left_foot_front/cartesianEndEffectorWrench:i" + +[LEFT_FOOT_REAR] +description "left_foot_rear" +remote_port_name "/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o" +local_port_name_postfix "/left_foot_rear/cartesianEndEffectorWrench:i" + +[RIGHT_FOOT_FRONT] +description "right_foot_front" +remote_port_name "/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o" +local_port_name_postfix "/right_foot_front/cartesianEndEffectorWrench:i" + +[RIGHT_FOOT_REAR] +description "right_foot_rear" +remote_port_name "/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o" +local_port_name_postfix "/right_foot_rear/cartesianEndEffectorWrench:i" diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/data_logging.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/data_logging.ini new file mode 100644 index 0000000000..8d88ff6d49 --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/data_logging.ini @@ -0,0 +1 @@ +remote "/balancing_torque_controller/logger" diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini new file mode 100644 index 0000000000..a2e740aaf2 --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini @@ -0,0 +1,24 @@ +robot_name ergocub + +joints_list ("l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", + "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + +remote_control_boards ("left_leg", "right_leg") + +positioning_duration 3.0 #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/config/robots/ergoCubSN001/blf_balancing_torque_control/sensor_bridge.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/sensor_bridge.ini new file mode 100644 index 0000000000..c23838208a --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/sensor_bridge.ini @@ -0,0 +1,7 @@ +check_for_nan false +stream_joint_states true +stream_joint_accelerations false +stream_cartesian_wrenches true + +[CartesianWrenches] +cartesian_wrenches_list ("left_foot_front", "left_foot_rear", "right_foot_front", "right_foot_rear") diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tasks/base_dynamics.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tasks/base_dynamics.ini new file mode 100644 index 0000000000..7c4e1845f2 --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tasks/base_dynamics.ini @@ -0,0 +1,15 @@ +name "joint_dynamics_task" +type "JointDynamicsTask" +priority 0 + +robot_acceleration_variable_name "robot_acceleration" +joint_torques_variable_name "joint_torques" +max_number_of_contacts 2 + +[CONTACT_0] +variable_name "lf_wrench" +frame_name "l_sole" + +[CONTACT_1] +variable_name "rf_wrench" +frame_name "r_sole" diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tasks/joint_dynamics.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tasks/joint_dynamics.ini new file mode 100644 index 0000000000..99192cb806 --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tasks/joint_dynamics.ini @@ -0,0 +1,14 @@ +name "base_dynamics_task" +type "BaseDynamicsTask" +priority 0 + +robot_acceleration_variable_name "robot_acceleration" +max_number_of_contacts 2 + +[CONTACT_0] +variable_name "lf_wrench" +frame_name "l_sole" + +[CONTACT_1] +variable_name "rf_wrench" +frame_name "r_sole" diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini new file mode 100644 index 0000000000..ca5fc5d76c --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini @@ -0,0 +1,95 @@ +tasks ("COM_TASK", "RIGHT_FOOT_TASK", "LEFT_FOOT_TASK", + "JOINT_REGULARIZATION_TASK", "TORSO_TASK", + "JOINT_DYNAMICS_TASK", "BASE_DYNAMICS_TASK", + "LF_WRENCH_TASK", "RF_WRENCH_TASK") + +[VARIABLES] +variables_name ("robot_acceleration", "joint_torques", "lf_wrench", "rf_wrench") +variables_size (18, 12, 6, 6) + +[TSID] +robot_acceleration_variable_name "robot_acceleration" +joint_torques_variable_name "joint_torques" +contact_wrench_variables_name ("lf_wrench", "rf_wrench") + +[COM_TASK] +name com_task +type CoMTask +robot_acceleration_variable_name "robot_acceleration" +kp_linear 100.0 +kd_linear 2.0 +mask (true, true, true) +priority 0 + +[RIGHT_FOOT_TASK] +name right_foot_task +type SE3Task +robot_acceleration_variable_name "robot_acceleration" +frame_name "r_sole" +kp_linear 100.0 +kp_angular 100.0 +kd_linear 2.0 +kd_angular 2.0 +priority 0 + +[LEFT_FOOT_TASK] +name left_foot_task +type SE3Task +robot_acceleration_variable_name "robot_acceleration" +frame_name "l_sole" +kp_linear 100.0 +kp_angular 100.0 +kd_linear 2.0 +kd_angular 2.0 +priority 0 + +[TORSO_TASK] +name torso_task +type SO3Task +robot_acceleration_variable_name "robot_acceleration" +frame_name "chest" +kp_angular 100.0 +kd_angular 2.0 +priority 1 +weight (5.0, 5.0, 5.0) + +[JOINT_REGULARIZATION_TASK] +name joint_regularization_task +type JointTrackingTask +robot_acceleration_variable_name "robot_acceleration" +kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +kd (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +priority 1 +weight (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +[LF_WRENCH_TASK] +name "lf_feasibility_wrench_task" +type "FeasibleContactWrenchTask" +priority 0 + +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) + +[RF_WRENCH_TASK] +name "rf_feasibility_wrench_task" +type "FeasibleContactWrenchTask" +priority 0 + +variable_name "rf_wrench" +frame_name "r_sole" +number_of_slices 2 +static_friction_coefficient 0.3 +foot_limits_x (-0.1, 0.1) +foot_limits_y (-0.05, 0.05) + +[include BASE_DYNAMICS_TASK ./tasks/base_dynamics.ini] +[include JOINT_DYNAMICS_TASK ./tasks/joint_dynamics.ini] From 4b0511ae35beebdbaa0b27412000bd08cd7c62d8 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Wed, 16 Oct 2024 18:04:52 +0200 Subject: [PATCH 03/10] update logger of ergoCubSN001 --- .../ft_clients.xml | 4 +- .../imu_clients.xml | 4 +- .../mas-remapper.xml | 20 ++++----- .../wrench_clients.xml | 16 +++---- .../ergoCubSN001/launch-yarp-robot-logger.xml | 2 +- .../robots/ergoCubSN001/yarp-robot-logger.xml | 44 ++++++++++++------- 6 files changed, 52 insertions(+), 38 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/ft_clients.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/ft_clients.xml index 56329018bd..f3b4a0a92b 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/ft_clients.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/ft_clients.xml @@ -4,7 +4,7 @@ - + /ergocub/left_leg/FT diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/imu_clients.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/imu_clients.xml index ea6c1da71c..8ca39c3a66 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/imu_clients.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/imu_clients.xml @@ -21,7 +21,7 @@ - + /ergocub/left_foot_heel_tiptoe/imu diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/mas-remapper.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/mas-remapper.xml index 1bb6cf384b..f6936a3719 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/mas-remapper.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/mas-remapper.xml @@ -6,34 +6,34 @@ 10 - (waist_imu_0, l_arm_ft_imu, r_arm_ft_imu, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu) + (waist_imu_0, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu) - (waist_imu_0, l_arm_ft_imu, r_arm_ft_imu, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu) + (waist_imu_0, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu) - (waist_imu_0, l_arm_ft_imu, r_arm_ft_imu, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu) + (waist_imu_0, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu) - (waist_imu_0, l_arm_ft_imu, r_arm_ft_imu, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu) + (waist_imu_0, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu) - (l_foot_front_ft, l_foot_rear_ft, r_foot_front_ft, r_foot_rear_ft, l_leg_ft, r_leg_ft, r_arm_ft, l_arm_ft) + (l_foot_front_ft, l_foot_rear_ft, r_foot_front_ft, r_foot_rear_ft, l_leg_ft, r_leg_ft) - (l_foot_front_ft, l_foot_rear_ft, r_foot_front_ft, r_foot_rear_ft, l_leg_ft, r_leg_ft, r_arm_ft, l_arm_ft) + (l_foot_front_ft, l_foot_rear_ft, r_foot_front_ft, r_foot_rear_ft, l_leg_ft, r_leg_ft) - left_arm_ft_client - right_arm_ft_client + left_leg_ft_client right_leg_ft_client waist_imu_client - left_arm_imu_client - right_arm_imu_client + left_foot_imu_client right_foot_imu_client left_leg_imu_client diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/wrench_clients.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/wrench_clients.xml index 300d1dfaa2..f44099907b 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/wrench_clients.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/blf-yarp-robot-logger-interfaces/wrench_clients.xml @@ -5,17 +5,17 @@ - + - + /wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o @@ -29,17 +29,17 @@ udp - + - + /wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/launch-yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/launch-yarp-robot-logger.xml index b6c440edad..d5469d0dd1 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/launch-yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/launch-yarp-robot-logger.xml @@ -13,7 +13,7 @@ BSD-3-Clause license. --> - + diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml index 43ea7684ff..9d5dab7b53 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml @@ -5,7 +5,7 @@ BSD-3-Clause license. --> ergocub - 0.01 + 0.001 (20) ("video") @@ -30,7 +30,7 @@ BSD-3-Clause license. --> - ("Walking", "GridTracking","cmw", "Balancing", "CurrentControl") + ("Walking", "GridTracking","cmw", "Balancing", "CurrentControl", "TSID", "BalancingTSID") () @@ -61,6 +61,20 @@ BSD-3-Clause license. --> "udp" + + "/yarp-robot-logger/exogenous_signals/tsid_fixed_cartesian" + "/tsid-fixed-cartesian/logger" + "tsid_fixed_cartesian" + "udp" + + + + "/yarp-robot-logger/exogenous_signals/balancing_tsid" + "/balancing_torque_controller/logger" + "balancing_tsid" + "udp" + + "/yarp-robot-logger/exogenous_signals/rde" "/robot-dynamics-estimator/logger" @@ -77,7 +91,7 @@ BSD-3-Clause license. --> - true + false ("jabra") () @@ -100,22 +114,22 @@ BSD-3-Clause license. --> - (l_foot_front_ft, l_foot_rear_ft, r_foot_front_ft, r_foot_rear_ft, l_leg_ft, r_leg_ft, r_arm_ft, l_arm_ft) + (l_foot_front_ft, l_foot_rear_ft, r_foot_front_ft, r_foot_rear_ft, l_leg_ft, r_leg_ft) - (l_foot_front_ft, l_foot_rear_ft, r_foot_front_ft, r_foot_rear_ft, l_leg_ft, r_leg_ft, r_arm_ft, l_arm_ft) + (l_foot_front_ft, l_foot_rear_ft, r_foot_front_ft, r_foot_rear_ft, l_leg_ft, r_leg_ft) - ("left_front_wrench_client", "left_rear_wrench_client", "right_front_wrench_client", "right_rear_wrench_client", "left_arm_wrench_client", "right_arm_wrench_client", "left_leg_wrench_client", "right_leg_wrench_client") + ("left_front_wrench_client", "left_rear_wrench_client", "right_front_wrench_client", "right_rear_wrench_client") - (waist_imu_0, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu, r_arm_ft_imu, l_arm_ft_imu) - (waist_imu_0, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu, r_arm_ft_imu, l_arm_ft_imu) - (waist_imu_0, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu, r_arm_ft_imu, l_arm_ft_imu) - (waist_imu_0, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu, r_arm_ft_imu, l_arm_ft_imu) + (waist_imu_0, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu) + (waist_imu_0, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu) + (waist_imu_0, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu) + (waist_imu_0, l_foot_front_ft_imu, l_foot_rear_ft_imu, r_foot_front_ft_imu, r_foot_rear_ft_imu, l_leg_ft_imu, r_leg_ft_imu) @@ -128,13 +142,13 @@ BSD-3-Clause license. --> left_rear_wrench_client right_front_wrench_client right_rear_wrench_client - right_leg_wrench_client - left_leg_wrench_client - right_arm_wrench_client - left_arm_wrench_client + + + mas-remapper - jabra + From 65cc719ece708c11e6653a112b75b5c4feb0eb5b Mon Sep 17 00:00:00 2001 From: ergocub Date: Thu, 17 Oct 2024 13:25:08 +0200 Subject: [PATCH 04/10] update tsid gains and joint regularization task setpoint --- .../blf_balancing_torque_control/tsid.ini | 28 +++++++++---------- .../script/blf-balancing-torque-control.py | 4 ++- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini index ca5fc5d76c..589c855ee4 100644 --- a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini @@ -16,8 +16,8 @@ contact_wrench_variables_name ("lf_wrench", "rf_wrench") name com_task type CoMTask robot_acceleration_variable_name "robot_acceleration" -kp_linear 100.0 -kd_linear 2.0 +kp_linear 5.0 +kd_linear 1.0 mask (true, true, true) priority 0 @@ -26,10 +26,10 @@ name right_foot_task type SE3Task robot_acceleration_variable_name "robot_acceleration" frame_name "r_sole" -kp_linear 100.0 -kp_angular 100.0 -kd_linear 2.0 -kd_angular 2.0 +kp_linear 5.0 +kp_angular 5.0 +kd_linear 1.0 +kd_angular 1.0 priority 0 [LEFT_FOOT_TASK] @@ -37,10 +37,10 @@ name left_foot_task type SE3Task robot_acceleration_variable_name "robot_acceleration" frame_name "l_sole" -kp_linear 100.0 -kp_angular 100.0 -kd_linear 2.0 -kd_angular 2.0 +kp_linear 5.0 +kp_angular 5.0 +kd_linear 1.0 +kd_angular 1.0 priority 0 [TORSO_TASK] @@ -48,8 +48,8 @@ name torso_task type SO3Task robot_acceleration_variable_name "robot_acceleration" frame_name "chest" -kp_angular 100.0 -kd_angular 2.0 +kp_angular 5.0 +kd_angular 1.0 priority 1 weight (5.0, 5.0, 5.0) @@ -60,8 +60,8 @@ robot_acceleration_variable_name "robot_acceleration" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) -kd (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, - 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) +; kd (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, +; 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) priority 1 weight (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 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 14229a2f63..c550e7abfe 100755 --- a/utilities/balancing-torque-control/script/blf-balancing-torque-control.py +++ b/utilities/balancing-torque-control/script/blf-balancing-torque-control.py @@ -97,6 +97,8 @@ def __init__(self, config_file: str): are_joint_ok, self.joint_positions, _ = self.sensor_bridge.get_joint_positions() if not are_joint_ok: raise ValueError("Impossible to get the joint position.") + + self.initial_joint_positions = self.joint_positions.copy() base_frame = param_handler.get_parameter_string("base_frame") base_link, self.frame_T_link = self.get_base_frame( @@ -174,7 +176,7 @@ def __init__(self, config_file: str): raise ValueError("Impossible to set the set point for the left foot task.") if not self.tsid.tasks["joint_regularization_task"].set_set_point( - self.joint_positions + self.initial_joint_positions ): raise ValueError( "Impossible to set the set point for the joint regularization task." From b97eba0a99316fdf011211b7bd11824b5cba51d5 Mon Sep 17 00:00:00 2001 From: ergocub Date: Fri, 18 Oct 2024 15:49:30 +0200 Subject: [PATCH 05/10] chage tsid gains --- .../blf-balancing-torque-control-options.ini | 4 ++-- .../blf_balancing_torque_control/tsid.ini | 20 +++++++++---------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf-balancing-torque-control-options.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf-balancing-torque-control-options.ini index e38e587a68..e1489447fa 100644 --- a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf-balancing-torque-control-options.ini +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf-balancing-torque-control-options.ini @@ -10,8 +10,8 @@ com_knots_delta_x (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # in m com_knots_delta_y (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # in meter com_knots_delta_z (0.0, 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) +motion_duration 100.0 # (10 seconds) +motion_timeout 100.0 # (10 seconds) base_frame l_sole left_contact_frame l_sole diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini index 589c855ee4..c0ad0b5b38 100644 --- a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini @@ -16,8 +16,8 @@ contact_wrench_variables_name ("lf_wrench", "rf_wrench") name com_task type CoMTask robot_acceleration_variable_name "robot_acceleration" -kp_linear 5.0 -kd_linear 1.0 +kp_linear 30.0 +kd_linear 0.0 mask (true, true, true) priority 0 @@ -28,8 +28,8 @@ robot_acceleration_variable_name "robot_acceleration" frame_name "r_sole" kp_linear 5.0 kp_angular 5.0 -kd_linear 1.0 -kd_angular 1.0 +kd_linear 0.0 +kd_angular 0.0 priority 0 [LEFT_FOOT_TASK] @@ -39,8 +39,8 @@ robot_acceleration_variable_name "robot_acceleration" frame_name "l_sole" kp_linear 5.0 kp_angular 5.0 -kd_linear 1.0 -kd_angular 1.0 +kd_linear 0.0 +kd_angular 0.0 priority 0 [TORSO_TASK] @@ -48,8 +48,8 @@ name torso_task type SO3Task robot_acceleration_variable_name "robot_acceleration" frame_name "chest" -kp_angular 5.0 -kd_angular 1.0 +kp_angular 20.0 +kd_angular 0.0 priority 1 weight (5.0, 5.0, 5.0) @@ -57,8 +57,8 @@ weight (5.0, 5.0, 5.0) name joint_regularization_task type JointTrackingTask robot_acceleration_variable_name "robot_acceleration" -kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, - 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) +kp (20.0, 20.0, 20.0, 20.0, 20.0, 20.0, + 20.0, 20.0, 20.0, 20.0, 20.0, 20.0) ; kd (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, ; 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) From ee7b6245aea45010714d7c096fcc23edfa8d22a4 Mon Sep 17 00:00:00 2001 From: ergocub Date: Mon, 21 Oct 2024 18:07:27 +0200 Subject: [PATCH 06/10] Tune gains for balancing torque control --- .../ergoCubSN001/blf_balancing_torque_control/tsid.ini | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini index c0ad0b5b38..a5beab9700 100644 --- a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini @@ -51,21 +51,21 @@ frame_name "chest" kp_angular 20.0 kd_angular 0.0 priority 1 -weight (5.0, 5.0, 5.0) +weight (3.0, 3.0, 3.0) [JOINT_REGULARIZATION_TASK] name joint_regularization_task type JointTrackingTask robot_acceleration_variable_name "robot_acceleration" -kp (20.0, 20.0, 20.0, 20.0, 20.0, 20.0, - 20.0, 20.0, 20.0, 20.0, 20.0, 20.0) +kp (100.0, 100.0, 100.0, 100.0, 100.0, 100.0, + 100.0, 100.0, 100.0, 100.0, 100.0, 100.0) ; kd (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, ; 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) priority 1 -weight (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, - 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) +weight (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) [LF_WRENCH_TASK] name "lf_feasibility_wrench_task" From e5d17e67e3618a6ce688b5a445d36511e15e4676 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 22 Oct 2024 10:29:27 +0200 Subject: [PATCH 07/10] add initial position of fixed joints --- .../blf_balancing_torque_control/robot_control.ini | 9 +++++++-- .../blf_balancing_torque_control/robot_control.ini | 8 ++++++-- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/robot_control.ini b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/robot_control.ini index 0fb586541c..3dadb8f14e 100644 --- a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/robot_control.ini +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/robot_control.ini @@ -20,5 +20,10 @@ position_direct_max_admissible_error 0.1 #in radians # 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 () +fixed_joint_list_names ("torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow") +fixed_joint_list_values (7.955, 0.051, -0.008, + -3.778, 19.433, -12.942, 40.664, + -3.780, 19.428, -12.942, 40.665) + diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini index a2e740aaf2..25849d8410 100644 --- a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini @@ -20,5 +20,9 @@ position_direct_max_admissible_error 0.1 #in radians # 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 () +fixed_joint_list_names ("torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow") +fixed_joint_list_values (7.955, 0.051, -0.008, + -3.778, 19.433, -12.942, 40.664, + -3.780, 19.428, -12.942, 40.665) From ba4c488792736f000d9ea06ca3ca4fef29013ed8 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 22 Oct 2024 14:33:37 +0200 Subject: [PATCH 08/10] log desired contact wrenches computed by TSID --- .../script/blf-balancing-torque-control.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) 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 c550e7abfe..e7d7de2357 100755 --- a/utilities/balancing-torque-control/script/blf-balancing-torque-control.py +++ b/utilities/balancing-torque-control/script/blf-balancing-torque-control.py @@ -253,6 +253,12 @@ def __init__(self, config_file: str): self.vectors_collection_server.populate_metadata( "desired_torque", self.robot_control.get_joint_list() ) + self.vectors_collection_server.populate_metadata( + "desired_left_contact_wrench", ["Fx", "Fy", "Fz", "Tx", "Ty", "Tz"] + ) + self.vectors_collection_server.populate_metadata( + "desired_right_contact_wrench", ["Fx", "Fy", "Fz", "Tx", "Ty", "Tz"] + ) self.vectors_collection_server.finalize_metadata() def build_remote_control_board_driver( @@ -523,6 +529,12 @@ def advance(self): self.vectors_collection_server.populate_data( "com_from_measured", com_from_measured ) + self.vectors_collection_server.populate_data( + "desired_left_contact_wrench", self.tsid.solver.get_output().contact_wrenches["lf_wrench"].wrench + ) + self.vectors_collection_server.populate_data( + "desired_right_contact_wrench", self.tsid.solver.get_output().contact_wrenches["rf_wrench"].wrench + ) self.vectors_collection_server.populate_data( "desired_torque", self.tsid.solver.get_output().joint_torques ) From c52212898eeb5c7bfa9a16bb39b936588ba20388 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 22 Oct 2024 16:33:29 +0200 Subject: [PATCH 09/10] clip tsid computed torques --- .../robot_control.ini | 7 ++++++- .../robot_control.ini | 6 ++++++ .../script/blf-balancing-torque-control.py | 21 +++++++++++++++++-- 3 files changed, 31 insertions(+), 3 deletions(-) diff --git a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/robot_control.ini b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/robot_control.ini index 3dadb8f14e..b870dad5e2 100644 --- a/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/robot_control.ini +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/robot_control.ini @@ -3,6 +3,12 @@ robot_name ergocubSim joints_list ("l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +joint_torque_lower_limits (-10.0, -15.0, -3.0, -5.0, -15.0, -15.0, + -10.0, -15.0, -3.0, -5.0, -15.0, -15.0) + +joint_torque_upper_limits ( 30.0, 15.0, 3.0, 30.0, 15.0, 15.0, + 30.0, 15.0, 3.0, 30.0, 15.0, 15.0) + remote_control_boards ("left_leg", "right_leg") positioning_duration 3.0 #in seconds @@ -26,4 +32,3 @@ fixed_joint_list_names ("torso_pitch", "torso_roll", "torso_yaw", fixed_joint_list_values (7.955, 0.051, -0.008, -3.778, 19.433, -12.942, 40.664, -3.780, 19.428, -12.942, 40.665) - diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini index 25849d8410..387a09b319 100644 --- a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini @@ -3,6 +3,12 @@ robot_name ergocub joints_list ("l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +joint_torque_lower_limits (-10.0, -15.0, -3.0, -5.0, -15.0, -15.0, + -10.0, -15.0, -3.0, -5.0, -15.0, -15.0) + +joint_torque_upper_limits ( 30.0, 15.0, 3.0, 30.0, 15.0, 15.0, + 30.0, 15.0, 3.0, 30.0, 15.0, 15.0) + remote_control_boards ("left_leg", "right_leg") positioning_duration 3.0 #in seconds 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 e7d7de2357..d5e98a3c09 100755 --- a/utilities/balancing-torque-control/script/blf-balancing-torque-control.py +++ b/utilities/balancing-torque-control/script/blf-balancing-torque-control.py @@ -81,6 +81,14 @@ def __init__(self, config_file: str): self.poly_drivers["REMOTE_CONTROL_BOARD"].poly ): raise ValueError("Impossible to set the polydriver in the robot control.") + + self.joint_torque_lower_limits = param_handler.get_group("ROBOT_CONTROL").get_parameter_vector_float( + "joint_torque_lower_limits" + ) + + self.joint_torque_upper_limits = param_handler.get_group("ROBOT_CONTROL").get_parameter_vector_float( + "joint_torque_upper_limits" + ) # Create the sensor bridge self.sensor_bridge = blf.robot_interface.YarpSensorBridge() @@ -453,9 +461,18 @@ def advance(self): self.first_iteration = False + tsid_torques = self.tsid.solver.get_output().joint_torques + + # thresholding the torques + tsid_torques = np.clip( + tsid_torques, + self.joint_torque_lower_limits, + self.joint_torque_upper_limits, + ) + # send the joint torques if not self.robot_control.set_references( - self.tsid.solver.get_output().joint_torques, + tsid_torques, blf.robot_interface.YarpRobotControl.Torque, ): blf.log().error("Impossible to set the joint torques.") @@ -536,7 +553,7 @@ def advance(self): "desired_right_contact_wrench", self.tsid.solver.get_output().contact_wrenches["rf_wrench"].wrench ) self.vectors_collection_server.populate_data( - "desired_torque", self.tsid.solver.get_output().joint_torques + "desired_torque", tsid_torques ) self.vectors_collection_server.send_data() From 4cdfc3890bd0a6c75030675d35a7dad71b1254d1 Mon Sep 17 00:00:00 2001 From: ergocub Date: Wed, 23 Oct 2024 18:23:44 +0200 Subject: [PATCH 10/10] Add torso torque controlled. Remove torso task. --- .../blf-balancing-torque-control-options.ini | 2 +- .../robot_control.ini | 18 +++++----- .../blf_balancing_torque_control/tsid.ini | 34 ++++++++++--------- .../script/blf-balancing-torque-control.py | 14 +++++--- 4 files changed, 39 insertions(+), 29 deletions(-) diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf-balancing-torque-control-options.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf-balancing-torque-control-options.ini index e1489447fa..9e9dceed69 100644 --- a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf-balancing-torque-control-options.ini +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf-balancing-torque-control-options.ini @@ -3,7 +3,7 @@ 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_y (0.0, -0.04, -0.04, 0.04, 0.04, -0.04, -0.04, 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 com_knots_delta_x (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # in meter diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini index 387a09b319..73f117b421 100644 --- a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini @@ -1,15 +1,18 @@ robot_name ergocub -joints_list ("l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", +joints_list ("torso_pitch", "torso_roll", "torso_yaw", + "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") -joint_torque_lower_limits (-10.0, -15.0, -3.0, -5.0, -15.0, -15.0, +joint_torque_lower_limits (-50.0, -50.0, -50.0, + -10.0, -15.0, -3.0, -5.0, -15.0, -15.0, -10.0, -15.0, -3.0, -5.0, -15.0, -15.0) -joint_torque_upper_limits ( 30.0, 15.0, 3.0, 30.0, 15.0, 15.0, +joint_torque_upper_limits ( 50.0, 50.0, 50.0, + 30.0, 15.0, 3.0, 30.0, 15.0, 15.0, 30.0, 15.0, 3.0, 30.0, 15.0, 15.0) -remote_control_boards ("left_leg", "right_leg") +remote_control_boards ("torso", "left_leg", "right_leg") positioning_duration 3.0 #in seconds positioning_tolerance 0.05 #in radians @@ -26,9 +29,8 @@ position_direct_max_admissible_error 0.1 #in radians # 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 ("torso_pitch", "torso_roll", "torso_yaw", - "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", +fixed_joint_list_names ("l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow") -fixed_joint_list_values (7.955, 0.051, -0.008, - -3.778, 19.433, -12.942, 40.664, + +fixed_joint_list_values (-3.778, 19.433, -12.942, 40.664, -3.780, 19.428, -12.942, 40.665) diff --git a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini index a5beab9700..d48fbd1fd6 100644 --- a/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini @@ -1,11 +1,11 @@ tasks ("COM_TASK", "RIGHT_FOOT_TASK", "LEFT_FOOT_TASK", - "JOINT_REGULARIZATION_TASK", "TORSO_TASK", + "JOINT_REGULARIZATION_TASK", "JOINT_DYNAMICS_TASK", "BASE_DYNAMICS_TASK", "LF_WRENCH_TASK", "RF_WRENCH_TASK") [VARIABLES] variables_name ("robot_acceleration", "joint_torques", "lf_wrench", "rf_wrench") -variables_size (18, 12, 6, 6) +variables_size (21, 15, 6, 6) [TSID] robot_acceleration_variable_name "robot_acceleration" @@ -16,8 +16,8 @@ contact_wrench_variables_name ("lf_wrench", "rf_wrench") name com_task type CoMTask robot_acceleration_variable_name "robot_acceleration" -kp_linear 30.0 -kd_linear 0.0 +kp_linear 50.0 +kd_linear 1.0 mask (true, true, true) priority 0 @@ -28,8 +28,8 @@ robot_acceleration_variable_name "robot_acceleration" frame_name "r_sole" kp_linear 5.0 kp_angular 5.0 -kd_linear 0.0 -kd_angular 0.0 +kd_linear 1.0 +kd_angular 1.0 priority 0 [LEFT_FOOT_TASK] @@ -39,8 +39,8 @@ robot_acceleration_variable_name "robot_acceleration" frame_name "l_sole" kp_linear 5.0 kp_angular 5.0 -kd_linear 0.0 -kd_angular 0.0 +kd_linear 1.0 +kd_angular 1.0 priority 0 [TORSO_TASK] @@ -51,21 +51,23 @@ frame_name "chest" kp_angular 20.0 kd_angular 0.0 priority 1 -weight (3.0, 3.0, 3.0) +weight (5.0, 5.0, 5.0) [JOINT_REGULARIZATION_TASK] name joint_regularization_task type JointTrackingTask robot_acceleration_variable_name "robot_acceleration" -kp (100.0, 100.0, 100.0, 100.0, 100.0, 100.0, - 100.0, 100.0, 100.0, 100.0, 100.0, 100.0) - -; kd (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, -; 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) +kp (70.0, 70.0, 70.0, + 120.0, 120.0, 120.0, 120.0, 120.0, 120.0, + 120.0, 120.0, 120.0, 120.0, 120.0, 120.0) +kd (1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) priority 1 -weight (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, - 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) +weight (2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 2.0, 2.0, 2.0) [LF_WRENCH_TASK] name "lf_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 d5e98a3c09..c15c881adb 100755 --- a/utilities/balancing-torque-control/script/blf-balancing-torque-control.py +++ b/utilities/balancing-torque-control/script/blf-balancing-torque-control.py @@ -190,10 +190,16 @@ def __init__(self, config_file: str): "Impossible to set the set point for the joint regularization task." ) - if not self.tsid.tasks["torso_task"].set_set_point( - manif.SO3.Identity(), manif.SO3Tangent.Zero(), manif.SO3Tangent.Zero() - ): - raise ValueError("Impossible to set the set point for the torso task.") + # torso_initial_orientation = self.kindyn_with_measured.getWorldTransform("chest") + + # if not self.tsid.tasks["torso_task"].set_set_point( + # blf.conversions.to_manif_rot( + # torso_initial_orientation.getRotation().toNumPy() + # ), + # manif.SO3Tangent.Zero(), + # manif.SO3Tangent.Zero() + # ): + # raise ValueError("Impossible to set the set point for the torso task.") self.desired_joint_positions = self.joint_positions.copy() self.desired_joint_velocities = self.joint_velocities.copy()