diff --git a/cmake/BipedalLocomotionFrameworkDependencies.cmake b/cmake/BipedalLocomotionFrameworkDependencies.cmake index f5b90e2fdc..a1b6acbb42 100644 --- a/cmake/BipedalLocomotionFrameworkDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkDependencies.cmake @@ -232,6 +232,10 @@ framework_dependent_option(FRAMEWORK_COMPILE_CalibrationDeltaUpdaterApplication "Compile calibration-delta-updater application?" ON "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_PYTHON_BINDINGS;FRAMEWORK_COMPILE_RobotInterface" OFF) +framework_dependent_option(FRAMEWORK_COMPILE_BalancingPositionControlApplication + "Compile balancing-position-control application?" ON + "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_PYTHON_BINDINGS;FRAMEWORK_COMPILE_RobotInterface;FRAMEWORK_COMPILE_IK" OFF) + framework_dependent_option(FRAMEWORK_COMPILE_YarpRobotLoggerDevice "Do you want to generate and compile the YarpRobotLoggerDevice?" ON "FRAMEWORK_COMPILE_RobotInterface;FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_Perception;FRAMEWORK_COMPILE_YarpUtilities;FRAMEWORK_USE_robometry" OFF) diff --git a/utilities/CMakeLists.txt b/utilities/CMakeLists.txt index 91be8e5c31..a93a3f9446 100644 --- a/utilities/CMakeLists.txt +++ b/utilities/CMakeLists.txt @@ -8,3 +8,4 @@ add_subdirectory(joint-trajectory-player) add_subdirectory(mas-imu-test) add_subdirectory(realsense-test) add_subdirectory(calibration-delta-updater) +add_subdirectory(balancing-position-control) diff --git a/utilities/balancing-position-control/CMakeLists.txt b/utilities/balancing-position-control/CMakeLists.txt new file mode 100644 index 0000000000..6a63686521 --- /dev/null +++ b/utilities/balancing-position-control/CMakeLists.txt @@ -0,0 +1,17 @@ +# This software may be modified and distributed under the terms of the +# BSD-3-Clause license. +# Authors: Giulio Romualdi + +if(FRAMEWORK_COMPILE_BalancingPositionControlApplication) + + install(FILES script/blf-balancing-position-control.py + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ + DESTINATION "${CMAKE_INSTALL_BINDIR}") + + install(DIRECTORY balancing_position_control + DESTINATION "${CMAKE_INSTALL_PREFIX}/share/BipedalLocomotionFramework/python") + + + install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/config) + +endif() diff --git a/utilities/balancing-position-control/README.md b/utilities/balancing-position-control/README.md new file mode 100644 index 0000000000..9db67463fa --- /dev/null +++ b/utilities/balancing-position-control/README.md @@ -0,0 +1,26 @@ +# balancing-position-control +The **balancing-position-control** is an application that allows a humanoid robot to move the center-of-mass (CoM) by +following a given trajectory. + +## 🏃 How to use the application +The fastest way to use the utility is to run the `python` application +[`blf-balancing-position-control.py`](./script/blf-balancing-position-control.py). +If you correctly installed the framework, you can run the application from any folder. + +The application will: +- move the robot CoM following a trajectory specified by the following lists in + [blf-balancing-position-control-options.ini](./config/robots/ergoCubGazeboV1/blf-balancing-position-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 `com_knots_delta_` represents the delta vertices of the polygon representing the desired CoM trajectory. + Given two vertices the application will generate a minimum jerk trajectory with given `motion_duration`. + Moreover, the controller will stop for `motion_timeout` once the vertex is reached. +- 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). + +Please 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. diff --git a/utilities/balancing-position-control/balancing_position_control/__init__.py b/utilities/balancing-position-control/balancing_position_control/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/utilities/balancing-position-control/balancing_position_control/wbc.py b/utilities/balancing-position-control/balancing_position_control/wbc.py new file mode 100644 index 0000000000..164f150434 --- /dev/null +++ b/utilities/balancing-position-control/balancing_position_control/wbc.py @@ -0,0 +1,11 @@ +# This software may be modified and distributed under the terms of the BSD-3-Clause license. +# Authors: Giulio Romualdi + +import bipedal_locomotion_framework as blf +import idyntree.swig as idyn + + +class WBC: + def __init__(self, param_handler: blf.parameters_handler.IParametersHandler, kindyn: idyn.KinDynComputations): + self.solver, self.tasks, self.variables_handler = blf.utils.create_ik(kindyn=kindyn, + param_handler=param_handler) diff --git a/utilities/balancing-position-control/balancing_position_control/zmp.py b/utilities/balancing-position-control/balancing_position_control/zmp.py new file mode 100644 index 0000000000..14811b75b8 --- /dev/null +++ b/utilities/balancing-position-control/balancing_position_control/zmp.py @@ -0,0 +1,46 @@ +# This software may be modified and distributed under the terms of the BSD-3-Clause license. +# Authors: Giulio Romualdi + +import idyntree.swig as idyn +import numpy as np + + +def evaluate_local_zmp(wrench): + tau_x = wrench[3] + tau_y = wrench[4] + f_z = wrench[2] + if f_z > 0.1: + return [-tau_y / f_z, tau_x / f_z, 0.0] + return [0.0, 0.0, 0.0] + + +def evaluate_global_zmp( + left_wrench, + right_wrench, + kindyn: idyn.KinDynComputations, + l_sole_frame, + r_sole_frame, +): + left_zmp = idyn.Position(np.array([0.0, 0.0, 0.0])) + zmp_left_defined = 0 + if left_wrench[2] > 0.1: + left_zmp = idyn.Position(evaluate_local_zmp(left_wrench)) + zmp_left_defined = 1 + + right_zmp = idyn.Position(np.array([0.0, 0.0, 0.0])) + zmp_right_defined = 0 + if right_wrench[2] > 0.1: + right_zmp = idyn.Position(evaluate_local_zmp(right_wrench)) + zmp_right_defined = 1 + + total_z = right_wrench[2] * zmp_right_defined + left_wrench[2] * zmp_left_defined + + inertial_zmp_left = kindyn.getWorldTransform(l_sole_frame) * left_zmp + inertial_zmp_right = kindyn.getWorldTransform(r_sole_frame) * right_zmp + + inertial_global_zmp = ( + left_wrench[2] * zmp_left_defined * inertial_zmp_left.toNumPy() / total_z + + right_wrench[2] * zmp_right_defined * inertial_zmp_right.toNumPy() / total_z + ) + + return inertial_global_zmp diff --git a/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_control/contact_wrenches.ini b/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_control/contact_wrenches.ini new file mode 100644 index 0000000000..2c47ec5a9d --- /dev/null +++ b/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_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-position-control/config/robots/ergoCubGazeboV1/balancing_control/ik.ini b/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_control/ik.ini new file mode 100644 index 0000000000..bd10ba0671 --- /dev/null +++ b/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_control/ik.ini @@ -0,0 +1,66 @@ +tasks ("COM_TASK", "RIGHT_FOOT_TASK", "LEFT_FOOT_TASK", "TORSO_TASK", "JOINT_REGULARIZATION_TASK") + +[VARIABLES] +variables_name ("robot_velocity") +variables_size (29) + + +[IK] +robot_velocity_variable_name "robot_velocity" + +[COM_TASK] +name com_task +type CoMTask +robot_velocity_variable_name "robot_velocity" +kp_linear 2.0 +mask (true, true, true) +priority 0 + +[RIGHT_FOOT_TASK] +name right_foot_task +type SE3Task +robot_velocity_variable_name "robot_velocity" +frame_name "r_sole" +kp_linear 7.0 +kp_angular 5.0 +priority 0 + +[LEFT_FOOT_TASK] +name left_foot_task +type SE3Task +robot_velocity_variable_name "robot_velocity" +frame_name "l_sole" +kp_linear 7.0 +kp_angular 5.0 +priority 0 + +[TORSO_TASK] +name torso_task + +robot_velocity_variable_name "robot_velocity" +frame_name "chest" +kp_angular 5.0 + +type SO3Task + +priority 1 +weight (5.0, 5.0, 5.0) + +[JOINT_REGULARIZATION_TASK] +name joint_regularization_task + +robot_velocity_variable_name "robot_velocity" +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, 5.0, 5.0, 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +type JointTrackingTask + +priority 1 +weight (1.0, 1.0, 1.0, + 2.0, 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 2.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) diff --git a/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_control/robot_control.ini b/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_control/robot_control.ini new file mode 100644 index 0000000000..73d225c69f --- /dev/null +++ b/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_control/robot_control.ini @@ -0,0 +1,12 @@ +robot_name ergocubSim + +joints_list ("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", + "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 ("torso", "left_arm", "right_arm", "left_leg", "right_leg") +positioning_duration 3.0 +positioning_tolerance 0.05 +position_direct_max_admissible_error 0.1 diff --git a/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_control/sensor_bridge.ini b/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_control/sensor_bridge.ini new file mode 100644 index 0000000000..0616af981f --- /dev/null +++ b/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/balancing_control/sensor_bridge.ini @@ -0,0 +1,6 @@ +check_for_nan false +stream_joint_states true +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-position-control/config/robots/ergoCubGazeboV1/blf-balancing-position-control-options.ini b/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/blf-balancing-position-control-options.ini new file mode 100644 index 0000000000..ef8d814852 --- /dev/null +++ b/utilities/balancing-position-control/config/robots/ergoCubGazeboV1/blf-balancing-position-control-options.ini @@ -0,0 +1,16 @@ +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) +motion_duration 10.0 +motion_timeout 10.0 +base_frame l_sole +dt 0.01 +left_contact_frame l_sole +right_contact_frame r_sole + + +[include IK "./balancing_control/ik.ini"] +[include ROBOT_CONTROL "./balancing_control/robot_control.ini"] +[include SENSOR_BRIDGE "./balancing_control/sensor_bridge.ini"] +[include CONTACT_WRENCHES "./balancing_control/contact_wrenches.ini"] + diff --git a/utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_control/contact_wrenches.ini b/utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_control/contact_wrenches.ini new file mode 100644 index 0000000000..2c47ec5a9d --- /dev/null +++ b/utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_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-position-control/config/robots/ergoCubSN000/balancing_control/ik.ini b/utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_control/ik.ini new file mode 100644 index 0000000000..bd10ba0671 --- /dev/null +++ b/utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_control/ik.ini @@ -0,0 +1,66 @@ +tasks ("COM_TASK", "RIGHT_FOOT_TASK", "LEFT_FOOT_TASK", "TORSO_TASK", "JOINT_REGULARIZATION_TASK") + +[VARIABLES] +variables_name ("robot_velocity") +variables_size (29) + + +[IK] +robot_velocity_variable_name "robot_velocity" + +[COM_TASK] +name com_task +type CoMTask +robot_velocity_variable_name "robot_velocity" +kp_linear 2.0 +mask (true, true, true) +priority 0 + +[RIGHT_FOOT_TASK] +name right_foot_task +type SE3Task +robot_velocity_variable_name "robot_velocity" +frame_name "r_sole" +kp_linear 7.0 +kp_angular 5.0 +priority 0 + +[LEFT_FOOT_TASK] +name left_foot_task +type SE3Task +robot_velocity_variable_name "robot_velocity" +frame_name "l_sole" +kp_linear 7.0 +kp_angular 5.0 +priority 0 + +[TORSO_TASK] +name torso_task + +robot_velocity_variable_name "robot_velocity" +frame_name "chest" +kp_angular 5.0 + +type SO3Task + +priority 1 +weight (5.0, 5.0, 5.0) + +[JOINT_REGULARIZATION_TASK] +name joint_regularization_task + +robot_velocity_variable_name "robot_velocity" +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, 5.0, 5.0, 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +type JointTrackingTask + +priority 1 +weight (1.0, 1.0, 1.0, + 2.0, 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 2.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) diff --git a/utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_control/robot_control.ini b/utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_control/robot_control.ini new file mode 100644 index 0000000000..8d698f2044 --- /dev/null +++ b/utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_control/robot_control.ini @@ -0,0 +1,12 @@ +robot_name ergocub + +joints_list ("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", + "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 ("torso", "left_arm", "right_arm", "left_leg", "right_leg") +positioning_duration 3.0 +positioning_tolerance 0.05 +position_direct_max_admissible_error 0.1 diff --git a/utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_control/sensor_bridge.ini b/utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_control/sensor_bridge.ini new file mode 100644 index 0000000000..0616af981f --- /dev/null +++ b/utilities/balancing-position-control/config/robots/ergoCubSN000/balancing_control/sensor_bridge.ini @@ -0,0 +1,6 @@ +check_for_nan false +stream_joint_states true +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-position-control/config/robots/ergoCubSN000/blf-balancing-position-control-options.ini b/utilities/balancing-position-control/config/robots/ergoCubSN000/blf-balancing-position-control-options.ini new file mode 100644 index 0000000000..ef8d814852 --- /dev/null +++ b/utilities/balancing-position-control/config/robots/ergoCubSN000/blf-balancing-position-control-options.ini @@ -0,0 +1,16 @@ +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) +motion_duration 10.0 +motion_timeout 10.0 +base_frame l_sole +dt 0.01 +left_contact_frame l_sole +right_contact_frame r_sole + + +[include IK "./balancing_control/ik.ini"] +[include ROBOT_CONTROL "./balancing_control/robot_control.ini"] +[include SENSOR_BRIDGE "./balancing_control/sensor_bridge.ini"] +[include CONTACT_WRENCHES "./balancing_control/contact_wrenches.ini"] + diff --git a/utilities/balancing-position-control/script/blf-balancing-position-control.py b/utilities/balancing-position-control/script/blf-balancing-position-control.py new file mode 100644 index 0000000000..bb37a54a94 --- /dev/null +++ b/utilities/balancing-position-control/script/blf-balancing-position-control.py @@ -0,0 +1,262 @@ +#!/usr/bin/env python3 + +# This software may be modified and distributed under the terms of the BSD-3-Clause license. + +import time +import numpy as np +import datetime + +import bipedal_locomotion_framework.bindings as blf +import yarp +import idyntree.swig as idyn + +import manifpy as manif + +## extend the python path +from pathlib import Path +import sys + +sys.path.extend([str(Path(__file__).parent.resolve() / ".." / "share" / "BipedalLocomotionFramework" / "python")]) + +from balancing_position_control.wbc import WBC +from balancing_position_control.zmp import evaluate_local_zmp, evaluate_global_zmp + + +def build_remote_control_board_driver(param_handler: blf.parameters_handler.IParametersHandler, local_prefix: str): + param_handler.set_parameter_string("local_prefix", local_prefix) + return blf.robot_interface.construct_remote_control_board_remapper(param_handler) + + +def build_contact_wrench_driver(param_handler: blf.parameters_handler.IParametersHandler, local_prefix: str): + param_handler.set_parameter_string("local_prefix", local_prefix) + return blf.robot_interface.construct_generic_sensor_client(param_handler) + + +def build_contact_wrenches_driver(params_contact_wrenches: blf.parameters_handler.IParametersHandler, + local_prefix: str): + # build contact wrenches polydrivers + contact_wrenches_drivers = dict() + contact_wrenches_names = dict() + contact_wrenches_names["left_foot"] = [] + for wrench_name in params_contact_wrenches.get_parameter_vector_string("left_contact_wrenches_group"): + contact_wrenches_drivers[wrench_name] = build_contact_wrench_driver( + params_contact_wrenches.get_group(wrench_name), local_prefix) + assert contact_wrenches_drivers[wrench_name].is_valid() + contact_wrenches_names["left_foot"].append( + params_contact_wrenches.get_group(wrench_name).get_parameter_string("description")) + + for wrench_name in params_contact_wrenches.get_parameter_vector_string("right_contact_wrenches_group"): + contact_wrenches_drivers[wrench_name] = build_contact_wrench_driver( + params_contact_wrenches.get_group(wrench_name), local_prefix) + assert contact_wrenches_drivers[wrench_name].is_valid() + contact_wrenches_names["right_foot"].append( + params_contact_wrenches.get_group(wrench_name).get_parameter_string("description")) + + return contact_wrenches_drivers, contact_wrenches_names + + +def build_kin_dyn(param_handler): + rf = yarp.ResourceFinder() + robot_model_path = rf.findFile("model.urdf") + joint_list = param_handler.get_group("ROBOT_CONTROL").get_parameter_vector_string("joints_list") + ml = idyn.ModelLoader() + ml.loadReducedModelFromFile(robot_model_path, joint_list) + + kindyn = idyn.KinDynComputations() + kindyn.loadRobotModel(ml.model()) + return kindyn + + +def get_base_frame(base_frame: str, kindyn: idyn.KinDynComputations): + frame_base_index = kindyn.model().getFrameIndex(base_frame) + link_base_index = kindyn.model().getFrameLink(frame_base_index) + base_frame_name = kindyn.model().getLinkName(link_base_index) + return base_frame_name, kindyn.getRelativeTransform(frame_base_index, link_base_index) + + +def create_new_spline(knots_positions, motion_duration, dt): + com_spline = blf.planners.QuinticSpline() + com_spline.set_initial_conditions([0, 0, 0], [0, 0, 0]) + com_spline.set_final_conditions([0, 0, 0], [0, 0, 0]) + com_spline.set_advance_time_step(dt) + com_spline.set_knots(knots_positions, [0, motion_duration], ) + return com_spline + + +def main(): + # Before everything let use the YarpSink for the logger and the YarpClock as clock. These are functionalities + # exposed by blf. + assert blf.text_logging.LoggerBuilder.set_factory(blf.text_logging.YarpLoggerFactory('balancing-position-control')) + assert blf.system.ClockBuilder.set_factory(blf.system.YarpClockFactory()) + + param_handler = blf.parameters_handler.YarpParametersHandler() + assert param_handler.set_from_filename("blf-balancing-position-control-options.ini") + + dt = param_handler.get_parameter_float("dt") + # convert the period in milliseconds + period = datetime.timedelta(milliseconds=dt * 1000) + + kindyn = build_kin_dyn(param_handler=param_handler) + kindyn_with_measured = build_kin_dyn(param_handler=param_handler) + + # Create the polydrivers + poly_drivers, contact_wrenches_names = build_contact_wrenches_driver( + params_contact_wrenches=param_handler.get_group("CONTACT_WRENCHES"), + local_prefix="balancing_controller") + + poly_drivers["REMOTE_CONTROL_BOARD"] = build_remote_control_board_driver( + param_handler=param_handler.get_group("ROBOT_CONTROL"), + local_prefix="balancing_controller") + assert poly_drivers["REMOTE_CONTROL_BOARD"].is_valid() + + # just to wait that everything is in place + blf.log().info("Sleep for two seconds. Just to be sure the interfaces are on.") + blf.clock().sleep_for(datetime.timedelta(seconds=2)) + + robot_control = blf.robot_interface.YarpRobotControl() + assert robot_control.initialize(param_handler.get_group("ROBOT_CONTROL")) + assert robot_control.set_driver(poly_drivers["REMOTE_CONTROL_BOARD"].poly) + + # Create the sensor bridge + sensor_bridge = blf.robot_interface.YarpSensorBridge() + assert sensor_bridge.initialize(param_handler.get_group("SENSOR_BRIDGE")) + assert sensor_bridge.set_drivers_list(list(poly_drivers.values())) + + # set the kindyn computations with the robot state + assert sensor_bridge.advance() + are_joint_ok, joint_positions, _ = sensor_bridge.get_joint_positions() + assert are_joint_ok + + base_frame = param_handler.get_parameter_string("base_frame") + base_link, frame_T_link = get_base_frame(base_frame, kindyn=kindyn) + + right_contact_frame = param_handler.get_parameter_string("right_contact_frame") + left_contact_frame = param_handler.get_parameter_string("left_contact_frame") + + assert kindyn.setFloatingBase(base_link) + gravity = [0., 0., -blf.math.StandardAccelerationOfGravitation] + base_velocity = idyn.Twist() + base_velocity.zero() + joint_velocity = joint_positions * 0 + assert kindyn.setRobotState(frame_T_link, joint_positions, base_velocity, joint_velocity, gravity) + initial_com_position = kindyn.getCenterOfMassPosition().toNumPy() + + assert kindyn_with_measured.setFloatingBase(base_link) + assert kindyn_with_measured.setRobotState(frame_T_link, joint_positions, base_velocity, joint_velocity, gravity) + + # create and initialize the IK + ik = WBC(param_handler=param_handler.get_group("IK"), kindyn=kindyn) + I_H_r_sole = kindyn.getWorldTransform(right_contact_frame) + I_H_l_sole = kindyn.getWorldTransform(left_contact_frame) + assert ik.tasks["right_foot_task"].set_set_point( + blf.conversions.to_manif_pose(I_H_r_sole.getRotation().toNumPy(), I_H_r_sole.getPosition().toNumPy())) + assert ik.tasks["left_foot_task"].set_set_point( + blf.conversions.to_manif_pose(I_H_l_sole.getRotation().toNumPy(), I_H_l_sole.getPosition().toNumPy())) + assert ik.tasks["joint_regularization_task"].set_set_point(joint_positions) + assert ik.tasks["torso_task"].set_set_point(manif.SO3.Identity()) + + desired_joint_positions = joint_positions.copy() + + com_knots_delta_x = param_handler.get_parameter_vector_float("com_knots_delta_x") + com_knots_delta_y = param_handler.get_parameter_vector_float("com_knots_delta_y") + com_knots_delta_z = param_handler.get_parameter_vector_float("com_knots_delta_z") + motion_duration = param_handler.get_parameter_float("motion_duration") + motion_timeout = param_handler.get_parameter_float("motion_timeout") + + spline = create_new_spline( + [initial_com_position + np.array([com_knots_delta_x[0], com_knots_delta_y[0], com_knots_delta_z[0]]), + initial_com_position + np.array([com_knots_delta_x[1], com_knots_delta_y[1], com_knots_delta_z[1]])], + motion_duration, dt) + + index = 0 + knot_index = 1 + + port = blf.yarp_utilities.BufferedPortVectorsCollection() + port.open("/balancing_controller/logger/data:o") + + while True: + + tic = blf.clock().now() + + # get the feedback + assert sensor_bridge.advance() + are_joint_ok, joint_positions, _ = sensor_bridge.get_joint_positions() + assert are_joint_ok + assert kindyn.setRobotState(frame_T_link, desired_joint_positions, base_velocity, joint_velocity, gravity) + assert kindyn_with_measured.setRobotState(frame_T_link, joint_positions, base_velocity, joint_velocity, gravity) + + # solve the IK + com_spline_output = spline.get_output() + assert ik.tasks["com_task"].set_set_point(com_spline_output.position, com_spline_output.velocity) + assert ik.solver.advance() + assert ik.solver.is_output_valid() + + # integrate the system + desired_joint_positions += ik.solver.get_output().joint_velocity * dt + + # send the joint pose + assert robot_control.set_references(desired_joint_positions, + blf.robot_interface.YarpRobotControl.PositionDirect) + + # send the data + left_wrench = np.zeros(6) + for cartesian_wrench_name in contact_wrenches_names["left_foot"]: + is_ok, wrench, _ = sensor_bridge.get_cartesian_wrench(cartesian_wrench_name) + assert is_ok + left_wrench += wrench + + right_wrench = np.zeros(6) + for cartesian_wrench_name in contact_wrenches_names["right_foot"]: + is_ok, wrench, _ = sensor_bridge.get_cartesian_wrench(cartesian_wrench_name) + assert is_ok + right_wrench += wrench + + global_zmp = evaluate_global_zmp(left_wrench=left_wrench, right_wrench=right_wrench, + l_sole_frame=left_contact_frame, + r_sole_frame=right_contact_frame, + kindyn=kindyn) + global_zmp_from_measured = evaluate_global_zmp(left_wrench=left_wrench, right_wrench=right_wrench, + l_sole_frame=left_contact_frame, + r_sole_frame=right_contact_frame, + kindyn=kindyn_with_measured) + local_zmp_left = evaluate_local_zmp(left_wrench) + local_zmp_right = evaluate_local_zmp(right_wrench) + com_from_desired = kindyn.getCenterOfMassPosition().toNumPy() + com_from_measured = kindyn_with_measured.getCenterOfMassPosition().toNumPy() + + data = port.prepare() + data.vectors = {"global_zmp": global_zmp, "global_zmp_from_measured": global_zmp_from_measured, + "local_zmp_left": local_zmp_left, "local_zmp_right": local_zmp_right, + "com_from_desired": com_from_desired, "com_from_measured": com_from_measured} + port.write() + + # Final steps + assert spline.advance() + + if index * dt >= motion_duration + motion_timeout: + if knot_index + 1 >= len(com_knots_delta_x): + blf.log().info("Motion completed. Closing.") + break + + spline = create_new_spline( + [initial_com_position + np.array( + [com_knots_delta_x[knot_index], com_knots_delta_y[knot_index], com_knots_delta_z[knot_index]]), + initial_com_position + np.array([com_knots_delta_x[knot_index + 1], com_knots_delta_y[knot_index + 1], + com_knots_delta_z[knot_index + 1]])], motion_duration, dt) + + knot_index += 1 + index = 0 + else: + index += 1 + + toc = blf.clock().now() + delta_time = toc - tic + if delta_time < period: + blf.clock().sleep_for(period - delta_time) + + port.close() + + +if __name__ == '__main__': + network = yarp.Network() + main()