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/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 + 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..b870dad5e2 --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubGazeboV1_1/blf_balancing_torque_control/robot_control.ini @@ -0,0 +1,34 @@ +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 +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 ("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/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/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..9e9dceed69 --- /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.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 +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 100.0 # (10 seconds) +motion_timeout 100.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..73f117b421 --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini @@ -0,0 +1,36 @@ +robot_name ergocub + +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 (-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 ( 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 ("torso", "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 ("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 (-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/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..d48fbd1fd6 --- /dev/null +++ b/utilities/balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini @@ -0,0 +1,97 @@ +tasks ("COM_TASK", "RIGHT_FOOT_TASK", "LEFT_FOOT_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 (21, 15, 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 50.0 +kd_linear 1.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 5.0 +kp_angular 5.0 +kd_linear 1.0 +kd_angular 1.0 +priority 0 + +[LEFT_FOOT_TASK] +name left_foot_task +type SE3Task +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 +priority 0 + +[TORSO_TASK] +name torso_task +type SO3Task +robot_acceleration_variable_name "robot_acceleration" +frame_name "chest" +kp_angular 20.0 +kd_angular 0.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 (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 (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" +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/script/blf-balancing-torque-control.py b/utilities/balancing-torque-control/script/blf-balancing-torque-control.py index 02539bc9e2..c15c881adb 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() @@ -97,6 +105,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,16 +184,22 @@ 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." ) - 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() @@ -242,12 +258,21 @@ 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"] ) 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( @@ -442,9 +467,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.") @@ -510,11 +544,22 @@ 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 ) self.vectors_collection_server.populate_data( - "desired_torque", self.tsid.solver.get_output().joint_torques + "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", tsid_torques ) self.vectors_collection_server.send_data()