From 8012719c655dc0364a21b742c1da6b63fca50be9 Mon Sep 17 00:00:00 2001 From: Prashanth Date: Thu, 25 Apr 2019 16:10:32 +0200 Subject: [PATCH 1/5] [fbeV1] simple floating base estimation algorithm --- CMakeLists.txt | 86 ++ README.md | 102 ++ app/CMakeLists.txt | 34 + app/robots/iCubGazeboV2_5/fbe-analogsens.xml | 88 ++ .../iCubGazeboV2_5/launch-fbe-analogsens.xml | 84 ++ .../wholebodydynamics-external.xml | 73 ++ app/robots/iCubGenova04/fbe-analogsens.xml | 89 ++ .../iCubGenova04/launch-fbe-analogsens.xml | 85 ++ .../wholebodydynamics-external.xml | 73 ++ doc/resources/fbeV1.png | Bin 0 -> 195608 bytes icubFloatingBaseEstimatorV1.ini | 4 + include/Utils.hpp | 234 ++++ include/Utils.tpp | 101 ++ include/WalkingLogger.hpp | 55 + include/WalkingLogger.tpp | 15 + include/icubFloatingBaseEstimatorV1.h | 632 +++++++++++ ros/fbeViz.launch | 8 + scope/base_scope.xml | 204 ++++ scope/base_velocity.xml | 205 ++++ scope/contact_scope.xml | 166 +++ scope/waist_imu_scope.xml | 97 ++ src/Utils.cpp | 280 +++++ src/WalkingLogger.cpp | 91 ++ src/configureEstimator.cpp | 504 +++++++++ src/fbeRobotInterface.cpp | 559 ++++++++++ src/icubFloatingBaseEstimatorV1.cpp | 996 ++++++++++++++++++ thrifts/floatingBaseEstimationRPC.thrift | 40 + 27 files changed, 4905 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 app/CMakeLists.txt create mode 100644 app/robots/iCubGazeboV2_5/fbe-analogsens.xml create mode 100644 app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml create mode 100644 app/robots/iCubGazeboV2_5/wholebodydynamics-external.xml create mode 100644 app/robots/iCubGenova04/fbe-analogsens.xml create mode 100644 app/robots/iCubGenova04/launch-fbe-analogsens.xml create mode 100644 app/robots/iCubGenova04/wholebodydynamics-external.xml create mode 100644 doc/resources/fbeV1.png create mode 100644 icubFloatingBaseEstimatorV1.ini create mode 100644 include/Utils.hpp create mode 100644 include/Utils.tpp create mode 100644 include/WalkingLogger.hpp create mode 100644 include/WalkingLogger.tpp create mode 100644 include/icubFloatingBaseEstimatorV1.h create mode 100644 ros/fbeViz.launch create mode 100644 scope/base_scope.xml create mode 100644 scope/base_velocity.xml create mode 100644 scope/contact_scope.xml create mode 100644 scope/waist_imu_scope.xml create mode 100644 src/Utils.cpp create mode 100644 src/WalkingLogger.cpp create mode 100644 src/configureEstimator.cpp create mode 100644 src/fbeRobotInterface.cpp create mode 100644 src/icubFloatingBaseEstimatorV1.cpp create mode 100644 thrifts/floatingBaseEstimationRPC.thrift diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..a2b4079 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,86 @@ +################################################################################ +# # +# Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT) # +# All Rights Reserved. # +# # +################################################################################ + +# @authors: Prashanth Ramadoss +# Giulio Romualdi +# Silvio Traversaro +# Daniele Pucci + + +cmake_minimum_required(VERSION 3.5) +project(icubFloatingBaseEstimatorV1 + VERSION 1.0 + LANGUAGES CXX) +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) + +set(YARP_REQUIRED_VERSION 3.0.1) +set(iDynTree_REQUIRED_VERSION 0.11.0) + +set(CMAKE_INCLUDE_CURRENT_DIR TRUE) + +find_package(YARP REQUIRED) +if(${YARP_VERSION} VERSION_LESS ${YARP_REQUIRED_VERSION}) + message(FATAL_ERROR "YARP version ${YARP_VERSION} not sufficient, at least version ${YARP_REQUIRED_VERSION} is required.") +endif() + + +find_package(Eigen3 REQUIRED) +find_package(iDynTree REQUIRED) +if(${iDynTree_VERSION} VERSION_LESS ${iDynTree_REQUIRED_VERSION}) + message(FATAL_ERROR "iDyntree version ${iDynTree_VERSION} not sufficient, at least version ${iDynTree_REQUIRED_VERSION} is required.") +endif() + +find_package(ICUB REQUIRED) +find_package(codyco-modules REQUIRED) + + +set(FBE_HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/include/icubFloatingBaseEstimatorV1.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/WalkingLogger.tpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/WalkingLogger.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/Utils.tpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/Utils.hpp) + +set(FBE_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/icubFloatingBaseEstimatorV1.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/fbeRobotInterface.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/configureEstimator.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/WalkingLogger.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/Utils.cpp) + +yarp_prepare_plugin(icubFloatingBaseEstimatorV1 CATEGORY device + TYPE yarp::dev::icubFloatingBaseEstimatorV1 + INCLUDE ${FBE_HEADERS} + DEFAULT ON) + +yarp_add_idl(THRIFT "${CMAKE_CURRENT_SOURCE_DIR}/thrifts/floatingBaseEstimationRPC.thrift") +add_library(floatingBaseEstimationRPC-service STATIC ${THRIFT}) +target_include_directories(floatingBaseEstimationRPC-service PUBLIC ${CMAKE_CURRENT_BINARY_DIR}/include) +target_link_libraries(floatingBaseEstimationRPC-service YARP::YARP_init YARP::YARP_OS) +set_property(TARGET floatingBaseEstimationRPC-service PROPERTY POSITION_INDEPENDENT_CODE ON) + +yarp_add_plugin(icubFloatingBaseEstimatorV1 ${FBE_SOURCES} ${FBE_HEADERS}) + +target_include_directories(icubFloatingBaseEstimatorV1 PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${Eigen3_INCLUDE_DIRS}) + +target_link_libraries(icubFloatingBaseEstimatorV1 ${YARP_LIBRARIES} + ${iDynTree_LIBRARIES} + floatingBaseEstimationRPC-service + ${codyco-modules_LIBRARIES}) + +yarp_install(TARGETS icubFloatingBaseEstimatorV1 + COMPONENT Runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}/ + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}/) + +yarp_install(FILES icubFloatingBaseEstimatorV1.ini + COMPONENT runtime + DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}/) + +add_subdirectory(app) diff --git a/README.md b/README.md index f19bcf8..2d4bef5 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,104 @@ # whole-body-estimators YARP-based estimators for humanoid robots. + +Reported below are the details for using a simple bipedal floating base estimation algorithm alongside walking controllers. + +# Overview + - [:orange_book: Implementation](#orange_book-implementation) + - [:page_facing_up: Dependencies](#page_facing_up-dependencies) + - [:hammer: Build the suite](#hammer-build-the-suite) + - [:computer: How to run the simulation](#computer-how-to-run-the-simulation) + - [:running: How to test on iCub](#running-how-to-test-on-icub) + +# :orange_book: Implementation + +![Floating Base Estimation Algorithm V1](doc/resources/fbeV1.png) + +# :page_facing_up: Dependencies +* [YARP](http://www.yarp.it/): to handle the comunication with the robot; +* [iDynTree](https://github.com/robotology/idyntree/tree/devel): to setup the floating base estimation algorithm; +* [codyco-modules](https://github.com/robotology/codyco-modules): to get contacts information through the whole body dynamics estimation algorithm +* [ICUB](https://github.com/robotology/icub-main): to use the utilities like low pass filters from the `ctrLib` library +* [Gazebo](http://gazebosim.org/): for the simulation (tested Gazebo 8 and 9). + + ## Optional Dependencies + * [walking-controllers](https://github.com/robotology/walking-controllers): to test the floating base estimation along side walking controllers + +It must be noted that all of these dependencies can be directly installed together in one place using the [robotology-superbuild](https://github.com/robotology/robotology-superbuild). + + +# :hammer: Build the suite +## Linux/macOs + +```sh +git clone https://github.com/robotology/walking-controllers.git +cd walking-controllers +mkdir build && cd build +cmake ../ +make +[sudo] make install +``` +Notice: `sudo` is not necessary if you specify the `CMAKE_INSTALL_PREFIX`. In this case it is necessary to add in the `.bashrc` or `.bash_profile` the following lines: +``` sh +export BaseEstimator_INSTALL_DIR=/path/where/you/installed +export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${BaseEstimator_INSTALL_DIR}/lib/yarp +export YARP_DATA_DIRS=${YARP_DATA_DIRS}:${BaseEstimator_INSTALL_DIR}/share/yarp:${BaseEstimator_INSTALL_DIR}/lib/yarp +``` + +# :computer: How to run the simulation +1. Set the `YARP_ROBOT_NAME` environment variable according to the chosen Gazebo model: + ```sh + export YARP_ROBOT_NAME="iCubGazeboV2_5" + ``` +2. Run `yarpserver` + ``` sh + yarpserver --write + ``` +3. Run gazebo and drag and drop iCub (e.g. icubGazeboSim or iCubGazeboV2_5): + + ``` sh + gazebo -slibgazebo_yarp_clock.so + ``` +4. Run `yarprobotinterface` + + ``` sh + YARP_CLOCK=/clock yarprobotinterface --config launch-fbe-analogsens.xml + ``` + This launches both the floating base estimation device and the whole body dynamics device. +5. Reset the offset of the FT sensors. Open a terminal and write + + ``` + yarp rpc /wholeBodyDynamics/rpc + >> resetOffset all + ``` + +6. communicate with the `base-estimator` through RPC service calls: + ``` + yarp rpc /base-estimator/rpc + ``` + the following commands are allowed: + * `startFloatingBaseFilter`: fill this; + * `setContactSchmittThreshold lbreak lmake rbreak rmake`: fill this; + * `setPrimaryFoot foot`: fill this; + * `useJointVelocityLPF flag`: fill this; + * `setJointVelocityLPFCutoffFrequency freq`: fill this; + * `resetLeggedOdometry`: fill this; + * `resetLeggedOdometryWithRefFrame frame x y z roll pitch yaw`: fill this; + * `getRefFrameForWorld`: fill this; + +## How to dump data +Before run `yarprobotinterface` check if [`dump_data`](app/robots/iCubGazeboV2_5/fbe-analogsens.xml#L14) is set to `true` + +If `true`, run the Logger Module +``` sh +YARP_CLOCK=/clock WalkingLoggerModule +``` + +All the data will be saved in the current folder inside a `txt` named `Dataset_YYYY_MM_DD_HH_MM_SS.txt` + +# :running: How to test on iCub +You can follow the same instructions of the simulation section without using `YARP_CLOCK=/clock`. Make sure your `YARP_ROBOT_NAME` is coherent with the name of the robot (e.g. iCubGenova04) +## :warning: Warning +Currently the supported robots are only: +- ``iCubGenova04`` +- ``icubGazeboV2_5`` diff --git a/app/CMakeLists.txt b/app/CMakeLists.txt new file mode 100644 index 0000000..f6e084b --- /dev/null +++ b/app/CMakeLists.txt @@ -0,0 +1,34 @@ +################################################################################ +# # +# Copyright (C) 2018 Fondazione Istitito Italiano di Tecnologia (IIT) # +# All Rights Reserved. # +# # +################################################################################ + +# @authors: Prashanth Ramadoss +# Giulio Romualdi +# Silvio Traversaro +# Daniele Pucci +# +# Thanks to Stefano Dafarra for this CMakeLists.txt + + + +# List the subdirectory (http://stackoverflow.com/questions/7787823/cmake-how-to-get-the-name-of-all-subdirectories-of-a-directory) +macro(SUBDIRLIST result curdir) + file(GLOB children RELATIVE ${curdir} ${curdir}/*) + set(dirlist "") + foreach(child ${children}) + if(IS_DIRECTORY ${curdir}/${child}) + list(APPEND dirlist ${child}) + endif() + endforeach() + set(${result} ${dirlist}) +endmacro() + +# Get list of models +subdirlist(subdirs ${CMAKE_CURRENT_SOURCE_DIR}/robots/) +# Install each model +foreach (dir ${subdirs}) + yarp_install(DIRECTORY robots/${dir} DESTINATION ${YARP_ROBOTS_INSTALL_DIR}) +endforeach () diff --git a/app/robots/iCubGazeboV2_5/fbe-analogsens.xml b/app/robots/iCubGazeboV2_5/fbe-analogsens.xml new file mode 100644 index 0000000..f7e1241 --- /dev/null +++ b/app/robots/iCubGazeboV2_5/fbe-analogsens.xml @@ -0,0 +1,88 @@ + + + + + model.urdf + icubSim + 0.010 + ("neck_pitch", "neck_roll", "neck_yaw", "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", "l_arm_ft_sensor", "r_arm_ft_sensor", "l_leg_ft_sensor", "r_leg_ft_sensor", "l_foot_ft_sensor", "r_foot_ft_sensor") + root_link + l_foot_ft_sensor + r_foot_ft_sensor + + false + false + false + 10.0 + + + l_sole + l_sole + (0 0 0 0 0 0) + + + qekf + (1.0 0.0 0.0 0.0) + 0.0 + 0.0 + + + 0.010 + 0.7 + 0.001 + false + root_link_ems_acc_eb5 + (0 0 0) + + + 0.010 + 0.03 + 0.0 + 0.5 + 10e-11 + 10e-6 + 10e-1 + 10e-11 + 10e-3 + false + + + right + 1.0 + 1.0 + 130.0 + 25.0 + 150.0 + 25.0 + + + /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o + /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o + + + /logger/data:o + /logger/rpc:o + /logger/data:i + /logger/rpc:i + + + + + all_joints_mc + + + inertial + + left_upper_arm_strain + right_upper_arm_strain + left_upper_leg_strain + right_upper_leg_strain + left_lower_leg_strain + right_lower_leg_strain + + + + + + + diff --git a/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml b/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml new file mode 100644 index 0000000..5f5ec46 --- /dev/null +++ b/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml @@ -0,0 +1,84 @@ + + + + + /icubSim/torso + /baseestimation/torso + + + /icubSim/left_arm + /baseestimation/left_arm + + + /icubSim/right_arm + /baseestimation/right_arm + + + /icubSim/left_leg + /baseestimation/left_leg + + + /icubSim/right_leg + /baseestimation/right_leg + + + /icubSim/head + /baseestimation/head + + + + + ("/icubSim/head", "/icubSim/torso", "/icubSim/left_arm", "/icubSim/right_arm", "/icubSim/left_leg", "/icubSim/right_leg") + ("neck_pitch", "neck_roll", "neck_yaw", "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") + /baseestimation + + + + + /icubSim/waist/inertial + /baseestimation/waist/imu:i + + + + + /icubSim/left_arm/analog:o + /baseestimation/l_arm_ft_sensor + + + + /icubSim/right_arm/analog:o + /baseestimation/r_arm_ft_sensor + + + + /icubSim/left_leg/analog:o + /baseestimation/l_leg_ft_sensor + + + + /icubSim/right_leg/analog:o + /baseestimation/r_leg_ft_sensor + + + + /icubSim/left_foot/analog:o + /baseestimation/l_foot_ft_sensor:i + + + + /icubSim/right_foot/analog:o + /baseestimation/r_foot_ft_sensor:i + + + + + + true + true + + 0.2 + + + + + diff --git a/app/robots/iCubGazeboV2_5/wholebodydynamics-external.xml b/app/robots/iCubGazeboV2_5/wholebodydynamics-external.xml new file mode 100644 index 0000000..af4a8f3 --- /dev/null +++ b/app/robots/iCubGazeboV2_5/wholebodydynamics-external.xml @@ -0,0 +1,73 @@ + + + + (torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup,r_wrist_pitch,r_wrist_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) + model.urdf + (0,0,-9.81) + (l_hand,r_hand,root_link,l_sole,r_sole,l_upper_leg,r_upper_leg,l_elbow_1,r_elbow_1) + imu_frame + true + true + + + + (root_link,1,0) + (chest,1,2) + (l_upper_arm,3,2) + (l_elbow_1, 3, 4) + (r_upper_arm,4,2) + (l_hand_dh_frame,3,6) + (r_elbow_1, 4, 4) + (r_hand_dh_frame,4,6) + (l_upper_leg,5,2) + (l_lower_leg,5,3) + (l_ankle_1,5,4) + (l_foot_dh_frame,5,5) + (r_upper_leg,6,2) + (r_lower_leg,6,3) + (r_ankle_1,6,4) + (r_foot_dh_frame,6,5) + + + + true + root_link + (torso_pitch,torso_roll,torso_yaw,neck_pitch,neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow) + + + + (l_hand,l_hand_dh_frame) + (r_hand,r_hand_dh_frame) + (l_foot,l_sole,root_link) + (r_foot,r_sole,root_link) + (l_foot,l_sole,l_sole) + (r_foot,r_sole,r_sole) + + + + + + left_leg_mc + right_leg_mc + torso_mc + right_arm_mc + left_arm_mc + head_mc + + inertial + + left_upper_arm_strain + right_upper_arm_strain + left_upper_leg_strain + right_upper_leg_strain + left_lower_leg_strain + right_lower_leg_strain + + + + + + + diff --git a/app/robots/iCubGenova04/fbe-analogsens.xml b/app/robots/iCubGenova04/fbe-analogsens.xml new file mode 100644 index 0000000..e55afab --- /dev/null +++ b/app/robots/iCubGenova04/fbe-analogsens.xml @@ -0,0 +1,89 @@ + + + + + model.urdf + icub + 0.010 + ("neck_pitch", "neck_roll", "neck_yaw", "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", "l_arm_ft_sensor", "r_arm_ft_sensor", "l_leg_ft_sensor", "r_leg_ft_sensor", "l_foot_ft_sensor", "r_foot_ft_sensor") + root_link + l_foot_ft_sensor + r_foot_ft_sensor + + false + false + true + 3.0 + + + + l_sole + l_sole + (0 0 0 0 0 0) + + + qekf + (1.0 0.0 0.0 0.0) + 0.0 + 0.0 + + + 0.010 + 0.7 + 0.001 + false + root_link_ems_acc_eb5 + (0 0 0) + + + 0.010 + 0.03 + 0.0 + 0.5 + 10e-11 + 10e-6 + 10e-1 + 10e-11 + 10e-3 + false + + + right + 0.5 + 0.5 + 130.0 + 25.0 + 150.0 + 25.0 + + + /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o + /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o + + + /logger/data:o + /logger/rpc:o + /logger-est/data:i + /logger-est/rpc:i + + + + + all_joints_mc + + + inertial + + left_upper_arm_strain + right_upper_arm_strain + left_upper_leg_strain + right_upper_leg_strain + left_lower_leg_strain + right_lower_leg_strain + + + + + + + diff --git a/app/robots/iCubGenova04/launch-fbe-analogsens.xml b/app/robots/iCubGenova04/launch-fbe-analogsens.xml new file mode 100644 index 0000000..22265f9 --- /dev/null +++ b/app/robots/iCubGenova04/launch-fbe-analogsens.xml @@ -0,0 +1,85 @@ + + + + + /icub/torso + /baseestimation/torso + + + /icub/left_arm + /baseestimation/left_arm + + + /icub/right_arm + /baseestimation/right_arm + + + /icub/left_leg + /baseestimation/left_leg + + + /icub/right_leg + /baseestimation/right_leg + + + /icub/head + /baseestimation/head + + + + + + ("/icub/head", "/icub/torso", "/icub/left_arm", "/icub/right_arm", "/icub/left_leg", "/icub/right_leg") + ("neck_pitch", "neck_roll", "neck_yaw", "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") + /baseestimation + + + + + /icub/xsens_inertial + /baseestimation/waist/imu:i + + + + + /icub/left_arm/analog:o + /baseestimation/l_arm_ft_sensor + + + + /icub/right_arm/analog:o + /baseestimation/r_arm_ft_sensor + + + + /icub/left_leg/analog:o + /baseestimation/l_leg_ft_sensor + + + + /icub/right_leg/analog:o + /baseestimation/r_leg_ft_sensor + + + + /icub/left_foot/analog:o + /baseestimation/l_foot_ft_sensor:i + + + + /icub/right_foot/analog:o + /baseestimation/r_foot_ft_sensor:i + + + + + + true + true + + 0.2 + + + + + diff --git a/app/robots/iCubGenova04/wholebodydynamics-external.xml b/app/robots/iCubGenova04/wholebodydynamics-external.xml new file mode 100644 index 0000000..af4a8f3 --- /dev/null +++ b/app/robots/iCubGenova04/wholebodydynamics-external.xml @@ -0,0 +1,73 @@ + + + + (torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup,r_wrist_pitch,r_wrist_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) + model.urdf + (0,0,-9.81) + (l_hand,r_hand,root_link,l_sole,r_sole,l_upper_leg,r_upper_leg,l_elbow_1,r_elbow_1) + imu_frame + true + true + + + + (root_link,1,0) + (chest,1,2) + (l_upper_arm,3,2) + (l_elbow_1, 3, 4) + (r_upper_arm,4,2) + (l_hand_dh_frame,3,6) + (r_elbow_1, 4, 4) + (r_hand_dh_frame,4,6) + (l_upper_leg,5,2) + (l_lower_leg,5,3) + (l_ankle_1,5,4) + (l_foot_dh_frame,5,5) + (r_upper_leg,6,2) + (r_lower_leg,6,3) + (r_ankle_1,6,4) + (r_foot_dh_frame,6,5) + + + + true + root_link + (torso_pitch,torso_roll,torso_yaw,neck_pitch,neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow) + + + + (l_hand,l_hand_dh_frame) + (r_hand,r_hand_dh_frame) + (l_foot,l_sole,root_link) + (r_foot,r_sole,root_link) + (l_foot,l_sole,l_sole) + (r_foot,r_sole,r_sole) + + + + + + left_leg_mc + right_leg_mc + torso_mc + right_arm_mc + left_arm_mc + head_mc + + inertial + + left_upper_arm_strain + right_upper_arm_strain + left_upper_leg_strain + right_upper_leg_strain + left_lower_leg_strain + right_lower_leg_strain + + + + + + + diff --git a/doc/resources/fbeV1.png b/doc/resources/fbeV1.png new file mode 100644 index 0000000000000000000000000000000000000000..8bbf6756dd34c0f16d9c42f4fe126db1499066e0 GIT binary patch literal 195608 zcmeFac|6qX+Xvo4*+NNVO`A|5*|$>CN}ExUHI#Mi%h-~oENK&EP^px`kj&V|Ft)Oc zHleYNi6P4n#xj=Q{h8>ro#)hfp5O0zUccA(uk$%NXWVn&*L_{@>-~OTpROF$*IU6M z#Ib16q7?`B?>)X~(Q?9~MN1>tmVuvisNUVaXwkYw2lnnh>22DdxI9nPumSJIc0>oQ zvrFyNB8fNl*Vb>^Z|FPu(2M2Ko-Iq31hZ)Q^0TtA?DoCBYH{y`WbpHKi@|>!^y+iq zdo9&Qgc~tG$A&)LokUCWN|NN+Xgi>p(>_BiP_S*)R4{+{w&JNzsFHxO zcjRum>5&{W`Nb-ciw*k4 zh;SHhi41E)r52gXmiZA~(-ksB$Dtn@e&#nN#|AGlm3ndmJ@kuTi}Dn!Zx5G z#D1o1Ese1mzI z0{DX$H&>;bP~nLuuAD;RW6is%&WiD1H`o_{e#j!zsA@tzxqc1Rc~ku4r11(H@cne| zwSL|h3V*qCM0oqa<^tcl-+%K-)yx0&!Em6#@j-|A(#Sj1h-1k>H-9L+Vykj*PU~h< zO@~U<=ED|wLIm*DuP*XAUWGx_FYLl+R+j(zMCP|L?bsvU%&bTq6&bJLgE~Rw+e$yY zl0uTxnbFd+rSofy{J0coTZ_LvxUmeQ)dUpTz(SC?YRSf<>@be9q7!MWvQ+|QN^9%e z?c~5|tXlH%06Q$xEsywg?^a^_LAj9;tWYgja|l>|aBQDQrknK2$Pw(S+UeOP2hw`#*hUX2>@@InHL(q$`1pE%e?bv8(g8xns}j z`O9MHwEDL%AqRTu{g0O;z<}z zV?QC7872o#WW%ncci97N;Ml%|kKf{pzZKqgC#6DU`XBHTtIrnQ;9ci(C`-wm!&XIx znVgg(n34bMz5g-Y%-`Y@f>1=UpA3#A$kA@4{cfo3O1fpA@BA(HUVXiTK5_mD1fNR9OGG7q4x4WIAd_1@3j^^;LVKh+U$=Zm7~Jz zyc};b#X%*%72b+zs|oy`$JlJUwwoScyOy`C%H1@m>cFf5>Wo8*1we4raL~mU-y^m^eRd zGdRu`cUS>We&h52m3jw6P2*Jf!U~fw{lOW7`~1qu^cb5_t?NxKl4EB>nN5)$VRaZv z0-RMJ+cilKhr)62_$7SIc6Izt3PTAQa*#O+w}j-l3ZOT#>~C3>z9Nw@Up}g%*Z)>7 zWfoP#c3)kIBB9yUVIX(T`c}Z$s^xkM6z1ykBM{Owxm?Zp~rXH z;TM!&H@vL{QS(=UtFGVu`)3SA)Ols}mJk`6`~%4YO2eNl6igK@ILaD$L6P$SMNSRV ziU5yHL=h{y?pFoJbw>DA{@D^@;2y*m#dnyA zO%AmD+3lUjp`Q+;PpveqD;jxX0lwuAIg8Wx&*Ir!cVcfE(wqag|KBZU9uzyy`Bg*= z*>xxKlcCuAt`gZX%)FoKMH_t|HB0^}YSL79Nfg~3iIhDI<%9J%zeNYz;eS{;VgbbB zf1WV<^2*s2=NEuVen;7_4(T6m?J>6Go<)*^lJQ?qd)}X|WFC_?+%e5wUv$@p`nu<* z{gM_K{j<0zi5R+aQnhro@AZGT7ziK(nDAkCG6LM5T6Gx$wBz65u?9MR=y;3}`rUWA zll7ls&Co3f6h*3lSv&z$#l4%q#mFWde~6!Cpv-$-x%Vbtq0p-Zxb9jS{##J#YbU_O zLl1bhUN6>{)0g|w(Ya85r_}w}t)(g8EOosVM~Eq}f7&m@?LB`M7eV}FzNfCiQlsnt z-D2ij$K06_ei;Jlju$Kh!Xe*xB?E!vM`DVmPrpMDoc|PSY2AxJgGZDtmJ0>g@XF!f z-1{fK-k_4-Ze*Et?;;GsAFrI=NJ@Gt-uqfYf_Z+w$bIdRbtU{BEJd1w+{NrvJO8m$ z{jQ&x@8k__eZS-_y_BE#%jDG`#)S+)@^}}^i5}g5x0v}Lv|~_ZU?D~dv~;#`XvdAe z-EqH^ z*8)>e%_{y+3H=xRNrcYC-sLK#Dy3iOmh$L7TgjK+21OkGlDA^^{k&iK_p1IZ$HPJl z9#*qIeiixe7Bm0L8tCSiN^m^)qeX1{imui6^&me zmIbyNs%q(MXq&dvE4d*Yf&CUoVCF*OeBXIL`i>*r{!b(l>h4lNeyidq2&*CXd&jq> ztZRwDpW(tY5W~8=@IW%MsUheq55uYTTV?PES{_s%Zg{Xu@E4`$itwMUWL`W9i@*Tb}@GloLkADY}zt9Ic>qVd)|Bh~Qoa2-odoq0RJEg$n zAE{Ku<;$Xk6zvH2c3(=%PXC|AIw0aI-C*3Mpr2{)x&93e@W)h6kkJ>uSFx9moVIwy z0`wBd`==i0Yt!+U9*BZk8FZ!cmqtrm@H^h~Pr>lf|HDVSCB(=rU^r9?lK0Mi|1G!w zHe_XzWzRnY2`Sj~rj}|p>YLs7`xmfBadqNpHamz|>DSI{n$I=9P4h$6w0Rm&WM(y{l%`{IeLzTJN{sP+u<8^j|J! zUe?D17Jp%hBH%xO0U*m&LWlJG*cFMjd&x|!N*RQt@J7r7@T2G;S=fNj^JCA{bN zEI|zB+fKC4&6shJC58^RM@;vZ_n{`K$@ujJbFC{v;T+`HgH=GAb6QXqCLgT%+cMa* z7gB-_KVW|Pz#(u_p5&vNh(vGPYyD8Neu7!s%FHAejv@Y)nFC=gXTJH+V8Ort&_n0~ z0RlDAt4*%mZ$v7rDW$$%i?Q{v=GeQ>GlvV7{v@WP&fU4%Q3d`rsFzXgxE!|U$Y`buN`E&=*G?~dbXr_@zW4qmAHTTV|ijLC%G(~ zheuD;<0QN0cpK}8jGlzQlI*4&mLqI!)UP`+?kbF{P+FY&-cOEkHEU+ii& z>PEl8O1NjrL5>i&Ra#zFDuu^3u@Y3-lMCfT;m{}e{qSMi6BI&+x_fH6`xDLSeVby} zET;--bC55HmskXJ<%Q3^=_`7os9fFQ@;)m1S$~?BW z$CPYo4|B|9pFUSnX`DF5W-#)iRfyd3iv#%e#-LP;S2BW@ac`Aw=r0N$eqDi^npu+I;&$LE6ag&2Dq6 z*CdB)Pg^uO6%G(K#hJWu)v!(UfO!v4iL&L94s(Ly-H2;1#Y!aei&Q0i0l3s z{;srPo5i;H(;?N9BE&9u?I%{^kIzUQZ&i^T-%{#Uoi0Cas;i34*b?Tmsgf-RxsLh> zL|wD(ePxlIrY>q*TT56b@jFw+6}P zzB^8)lbm^Sh-&s|tErAlJOs7AbUrcPp8HwjZHVxj`f5^Z$|YOhQ5-$4^*3gm`JXI=;ZO|G@h}~FxPcQl;>wiV+}vz9IdLZuBeg4`*~BMl?m;0 zv*TH#Zw@K1KwB2odDaHB9^5>i_kO&;s|ipNqWq9VvmA)42?+*z%}Cr$VgT;po6Zgs zik^f<-3BlF5Vv3D(;q*VnWjxxsR~4u4gERwTEp@0%>rlOb&8E4ji!i<3lc4DlEsv( z>dO%0UjFKXc@)C^aIp`4@eU0WF#)3~S)CCwVF^wJQnf0Ss3R1VpmUDs|5{!!DxG{* zvz?RVkaI-i@%=sFVZu&%5=Ap~(h*K_jP|VeDgUvCOE{~RwsA*NOyD#H-W6-49iJeDl!hE8XC*YOqv|$vs znb?dre)%_r_?nJ4RCE1)D8IMs?&x@r^lnMW@p{wz?+ZMt!vR&)SJ6xJqpFkJ5VRHI zfzvK~B&NZA;gUfYxlVK|ovG&fXo63ppmdSX2+;8g? zmqP9Tfv+;%87UjaDqtcf_O`{XUKGkiasks5VxdEzc$OP>2$*_XsC3;rL6~LG#KBRZ z;1&Ev<(?2M&5|{|>&O_unhDBV?)8hw!xb{}qrh z>tP8K0ceR@yV3R47-KgMMqYA@77rcT8Pk%nlK9)u%7q_k1};R_yW@vKz1n;vGv9;U zOcxx5@N%DuUbGOuxU*JYdQPRzSKA^}huTKnkc+ZjXxDCa&r0u8|22MB=bg^Epipa$*0oLw=GI z7g7_~Kqx^_*`_u=tI0C5qUwo(YS*popyHkfk#xZXP0!Xg^Fy2@oM+?nr48Xt)zdjM~hjh_h*$6 zhO5NI`|JlxR?HYABi)EsXp6937rom1aqW^wxXyHH^7Kc^;Y9M%<=jz$*nq~fUadm_6oi2& zRM861v`2v&diCOrP2FMD!fPU1R~aKN&e`JC>R}Z1EVgw|ICu286g_@)8>VS}?T(tQWB^>9yr1W1h+&uH zIgZpRo8^5`Spp3=%v1mcs&1f1cBIcEc^K+6bA5|z=*oOkgxz0#MKzRT(e`aOE_EQIUNtO6o{gV> z+%4()n0N5_2B{}}ILj{o8#^*9y}JmvMBbT!-FME^fzG4nzkePEYP}Eh?vgN?^rj6C zm}VP`z6|VDR4J`!gm@cOx-FnKUfv?RVhtyRH2iob0C!68Xgn(bS_wS-<8?iZAI&63 zs)vS@6jh+q`K({V74Ehhu9{G)Dw!2bhHq!vZkwb3@vwwO>PA+A`fG3ktb1QJv!GqU zHO|yms!80qcPAC;)~)ej6ttJbS8tahHE+GK>Og*dREy2w zKM3j+**%CTmNuJjrky@p+zws#{^xJ?UGg9_8}Iw+X&JcG*Ybs0ItS!?KrS^rVc}3FldQpQ7O%lMx@v@-BO9@bSANgsdR>2@EFxjPYEd}onkR$n~ zfz}myI*r~Ai@D&85w{=dPH+xG<7d?{SS{NmpKiH^lP4;QtALfz)Tb+UD8t!bCcw$x z_;60$)dtKIbxg~#f^qAFe(&=Myv0l_>cB<0ww8$1c8jPwO|HtpZcgVyg-Y^yF)h(i zfTd{xKP#cOyx>p_a+7e#7cNy*ex|dc8lq4pcS7m5`D?oUq4}zD@OyVmuj@?nt!9bE zyg`dt)-Ad?SH?mISI%7dt)Zg{_5A*Jc-e~K#5PO@j{boR>KhM$@#c8|@|S_yot*!+ zj7M8T#H+*VCenY#X6nVOn5QDXpXNwTF`I>{RN-{9xWNH$^>6Aq%KLJ4Q zwS)M%*&%Qm6~V;=B(Z_w7waQ;jyf{h&Zk6ME^h5iKR^54*ttTdSTuQN$?mol$s?;| z*6>g*TRvlg-ApYjI>{zc#%-{~7XWsf9{OD6^#CNXdfI`uHQuptr?Mwz2@oEiq=8(J zoA6)df;6zT8dR2*+>0Svbms1&Q24+o%p+03_2Y{yfJrM#PmvkgbCU*n)1Pa~ZW}xG z3Yq87J?nP*O$?cmND+22(#8mx9V9xat(B=$Sj*Uw4Q?YnUCSPv3jzT(T)dwjS~ zs=Q`>zoC2_vvrSCWi``Ugq5bMiA0IsZpP% zCVd)a8IoXnyxynLfEQ0j3Sp`o4?$u|v1I?AWL#c^u2j^5qIlR*;dLM&yoS zkf@4=p63hGyQYs{^E5a?-qnp=_~SitfFzwBlCG0+wpT$w)uR)HMl`BW@xeS7VrD}b z422taoeM;}S_Po)FPK%JBLCiv-lidQHDuE8=!)zh$C#y9Ln1~raC-P`4BvSFYccEM z-p2`*>z_G7RXh(o6Wre(p9G%v{+(y6FTEo|nnR8E`?X4BG-3 zq;_Bb{(}5xQ0rkGcUGyzrApM|j`eMJ2qf(hHjZ_YX-5})O3UiYEDUoas-`wt6?fmY zC)gx7t`Qe?4jlvA^-{6YX7_}N;MvSDI589jmqT)<)HLuC^Wps5gL?rj!r9m9)O5x4 zSddrWF%@1k2l&%0Je=NEGafhWn-P@xbioTCd53wON>!tiv5LTu^^li{It=*iw-Y|b zMxYDM^RlVFg7OXM)2j=pxr6_n8&tcJGv2KldHQ<2`jwpFr*=hFl~L>vuS2n_9>{JN zIO~24>cg+uM_kqT`~t!_l}fPZS~H_>TL%D0A3qfYh`o$5@!6s9Fi-j*lzcR60X3QZ zL=&)>L*TL8u&quB5jYN&=GobCe0ZYg2dU{!0sLhY&e3r4p~3s?;sJ%oTq}$f%`wO8 zK1u!cz}Zi*2?L+w28JW*z1~e|*4Qb3Q#!WI*X6_9G*#ORH=M~Il*C7`pil_k(F0f zyW_()!=7NP9w+;B-yzEeP9BQV1m>RHwB(N66Sgu>rwmZs-%N@R$fjgiPk;7)M$$ z4pc>p;R*pBP|ke)jPf&VoO4fKY!$0{9;NoNvcvIf_l67IaD1!unHd^hb%3M;pB)4RaDy44k$%DHZ zKz54T;W((Dmef9VG23i>^@5_oWZ*!sfUsddA6c1L-pqSa(bU+&k@gZZcP6_zqRfYr z{Qf=6(>S{c=EWnl^Tnf*<>G$4?Xt6=@`WCf#qD$e*>}aj_PmbZiKfp2W`6Q(?hp9Z zcJ?1)EMB^7^zt02eO9~M=*~1TZHSw61~=|hi4)z_g+zN0xn0P}leD3e8gF&=c9o+) z*r=)DnO?fS3j&EiIZjUulY3K>2V+a^(YW`YMJb?ep3c(tG|8#maVQ=f$5{={-G)Aj zz1dl2_5Ba4N>mf?r!H4PYQ$Uzo6*egHr7r7JkJb*R@PlA%?mu{C*XVIM8_y-6!ys!o(A}j>_+wL5Svd3OA5Ye}Bt4sjxdQch=gJi8gv?QL?#E&ht!VgM)r=fYS z0=Lm^)vGnpAmAi=LWV&blW38V`<^3m>$CmLKC`g8&>3Snt=>MVzjX;oy{H}J>WM9G z#z!1uH0RpJiyQf8A$Vwg`}Ah5+pU&(02;CyV8#`C4*|X&CSTjbeL=1dXhm0LYO#P$ z8Dg~06_a20wh9-47ob>x-xrN)%aGFs!td?7W}x1=j1}-t47l_&(K+4+<@n)3YP2rR z*^eSz8Zif949pT7!v@tdu#)D@Szg7pb?n%^TAkWddYTD3otn(b1khXK5Ck1GF>TXwD z07SS__R@jq4YJs(ql#d2A0198yqQMD2AKCZYU+L~Y?bJD_Tzl?mwb5OBI%;Uk|@`< za>Kq505WAWGqpfdM8M)_2E=j#>ovzip!}`X?N_!!naj*?Sl2|ti6()?fWmHIRFqLN zOA((FG>RtK*&*g8DutazjGUDbsS`$K{`{Es00tans8E0scAPp^gdM<;oHzmhrhjT4 zJFK!ny6nSt)00L3X;RVY@1OGki^I3Td`QFkfWa!g+B;IO%>(kJejX53D0vx++Wn0| zMJ>y1G!+(4Fg@4Df-Z2gDAL{fvV9qG@(%xn#nkohK4W{KYB_?qfz_1+v+)|}O_P*i zY3-c#+om{eX*j>YwP>$QCcyeq60rb8v>cL?=YGw%SS_KfnLW*1Fmx!mgF~t82kL`p z11GYl@5&qR&fnE|=SX}zL?oSj=>ysoP6#^tEuYxET=d;g=Cd+x^{q(Ipg`D*4X6+Z zk#X`gXqb&;{;5mAu?&R^-RAozwA5IYMT>20pYj!rR0v4m{?3J|{sNm_wG{ko*N z-XBOX1F>ef^LRjfg@Z;(%Shq_>=}ts;b=DI2tJ=1){|_XnSW-mkz*G2kY>6IYc4aD z>{=}q+Fe*ZIS>%kkA3Cd<;3z+%K|ksIz6t6gr7jzLZk^S`X7$88UG@TB}vT6NhV_; z@dKu_kWnqcXn{qLsxQcBkPtbJIS_u( zZCvUmNYD6i)diT(&uaLVQ#GDPR1xx33N^L?;htGZ+vMkE;MCy} zjFU2mL!3gKV51XXvI?@cJ4zX18x4WHYFhm~U;Qb7V9P55T2I{imM-t#NNh`>ZoIcm z$ZM7Do>%Qv_Ov-u&t~f~a2doS$rzmihQMQ(hN<~Q)=mnrHi)-bS3qgpC{TO6#f_<- z$l^wi`-M*cR#+5v)}tXa4*)aeVmCji8Gyu;$n6E^d2&9ndLWds{7(>wcasyt6bd6B zYIxwEo~DK+100gS8aq4C)F1zeWDq#@c4A^Ay-p`NyKW7N)*&kf0Bht=Ztkx+ZCH4 zrrxhlp*prk0N5d0TCTF!fThQQ+g9>Pum!-p!7@!;$xzqBMO3}W{|&RTgba3R$yr+^ zivVWEF)N(jJrucCZ#?Bol-*^C^9xYK)fAk+@sQSy>BtaqL%X+k3+zH@&7P=**mvOL$N z=yy7>O)2x1KGOD@D=700`za6EGi%t0HWLF0B6H0 z69qWUvh3u+gbWs}P9e^C8h{WZ2tgtIW`WKX2BEUy*B@nzCoJE3c^JgYNoYCw<7eoR z748W11PWdJp$~ye=tU6NB}4Y`Xg;sfgOp{HmxIlKKTV#<933RRxW1uG8kOF8p>4xs zJVH^5w=%)_`gfal31YH}u1V($c`*5eNBi%a7fFLXOPM7Al+kWh*_U{}@{5B8+O7=1 z)Qzt?9JOb3ZxFE??inmCxKE;JU0!hA&@bt3-a(wt_E#P@eBEE}stcx}#nUktG;5W5 zS4kYhQv#3avxC5+umdmvJ4!h!Pw1;!$7DBKz3z)q)BgNo1EqTCfU`KvM}rh$IDp-C%jh431q zSd!Iy1R}~e=K#|XK?|H?WX7cLat;(%bAMt_OgEoR)t-4O;|X#a50B>P(XAh;tx$}` zM9y?4KD8W%Z?D|uETcl*1luY`tjOzfvq!L=xjarZu{0?>;@1h^k)Y7VEOd~7D)qK|@ZqP)3~@n*hrzLL z4GLK|RS?ceH-j+eTMi&s><$z~^TQok$1ktkK!hr7WsMy|^Ht3gA8` zAtgpjD6$FBpRISwZ@7X&dC?c6H8)hoQ)Y+CSOZP}iUAE2VA<&4ev8vIHD?&{XV~hU|lGmfrh*ZVjy=|egIU` zv;8%&2T+<@Q88xo=>z4Ra+CQbI8a_Z{2gd2+JM>YRIkm!7--MzygCB{+Sup`{}5nV z0J%nOOKYF~#81gsX&#V6eB!DV?gm*)F1oF`@2Zu`YYLfchJMm#;uf&)&i`rb8$-#} zi%#~ZHxDPaz$P&30qqhf3KULwN69I7El3`NP*}UDQy|24o#NI0cx{hml;it)*?{3l zxpObB<=rhTK3&*8OUGtkbDS|okQ~He*JkUS1Ng*a+x&AZ^KeV2f$qywuRZ5?3SP=u zmWIj{@&qmZ5RT?AD@KVmGF=P0&!kLBHa`Wl9|b60rz%UTq2=>jD629wq*}AEy@8;)9 z;WgzoDYcmZa@7~iX0C$#T142!+cX4v)NtKQ3^GH`A?7d|y-HTiTbR~SdZ`SQ zL(p$diR?Uo^!1?7hvw?fUAK)h^(06UQ{YPHpm=BBCeQB7bU-_UwC3w~cc-y=TovZ1R+)_ffr2RD4e#f7^G+0K($ zzcAh1iIxfh%BA9>}yu(a>)$KSPZ+ z5ZMdZBZX5W2*Y3!!5?>4!x7rv)s@qaU;W&IK9dEg!{Cmp8L1cvT(iZh2d`KL%r~fM zryq`+iV-g$aT()A|MmC@+WVWSvN@NStH1O1rbx;3<_+#1i);lR-`?GZ@KH&1 zp<}{we(g_x(-;!w2K-i5&i*(uB5DB9+;WO$Pu+ zo<&oZg~i7g*Uv6B1-&eVQb)_9T$KauUDf!F7%@#X4G2?0HWvep;k{X&m?NNMWl#85 zuC)!(6*&9+)|C?K0>SaUrtbd|sum}j>?QqG@v8}U+Mls^xHa!h7&iiDNt|CZW9*IP z7vCKd{W(FG*mk>oX6t4x7nM78Yn>tK7oz@)Ze4KmIzBf29zdvq)GgooY?}@samw`v zb{X#++(--y8kXK_*MH)r52Tk3+YG@ zg`EEUyPQs4oU$+at~t#Xn>~aYJketA5uKgRaP&W<1STayE28Um` zGyFhx;shnt1j^AAJC};eY3V1QN7xoD+X++@(2(wZrm4lZf9j5-C6A>KGbb&mJBdF9 z)Txz_@7AMmW?2ZMgmc-BHc0zN-Bpkwxal9Y^llh2);6IhsJJkp)IqLGZXo5305)M} zo2z7ji3Na}C30;hzN@)`9T#xGynz-Ei&qHBt!n|d*8+HlxFFkFRs)&7Wpprs|Am1H zIfOuYY4=3L*_mO3Kdo@$dnf7_S8C-Qe)8#CZ?vnZ?qNw{9sT!ji7jP5F7TI0*{*Ya zk8X)oO+Qu`9eu@&C7XtgE!x;Gh@DF4rmrMoCfl(3c|Ro`Pbge8V4X| z@Ev=)c)~=QcssFl{%wFC#LWD>q0|E=9dk8{o~87}j1<1!Cmag6fMU+i95U&=OuHE+ zO2IR_(KOc$z{2r(He>k!z}bWV2%kTCfEI&9Tq^fIWc1`h_`+C?(p-Lh#20mb5Z!pw zggPC!hPm;<|vlA6vP=BffAuYfxdhNzE zw1ZyFaT?J|1wIM6bl`_211Y~>e(iD*_^t_nKu%0ObQ6oc`uwlees;UYPX?pdyNbWO z_3*CIPqF)hS^w{np=R;K5Lh1cvQImNx3%8u_HM7yp4SkG7(q#M4j1)2HPL_$;Gg^8 zU&Ny^v@5HD;*6j1H0lQ=w0I^|VL>x+TWxqI+HH+U5<5>78yqYWT<)|VP?{S13W0PgeG*|G0#Cru5 zDho_bg!t9q5dta^r+Z>^$RxzfFG3zF6%-4YAZGz4GaUH#+&#|IX4^m`+SxAIr2#}x zI51X@Hsxy`oB>H>>)6WPT4&Xfga%Gz4qNPqDzb(gKmY7w2K)}DL=pP?Lm=g`Uiin9 z_ur>*&5Jl9g%~GYv_R;TZ7=M9lUoLqx9v^Uw2C#xTW3iR88Z`$kEBMx*1e^e6 zQmM$}LR<~rIo#J~egs50g^oG(_?wxI`Nqi zuK5lGGv61Az6%&KyYAfLxHM|q%ntm)B@^in8STa7bShL$Mc7AB_dYa9f(ZJxA5nEkq$Y<6hXDiYJ=EFenEkYx$wbgmbmoCEg`?c(^kn^2 zO<^6Xo43e^Odgr1KFYHo9aO{Rvq&AoHDJ=`WBk2x-frdj@Hi9VL+c_VE1O9|#P8-M zJPZjQ20aS3a(f{2gia1;G4MmFY0#Uj&Q>`G1_G~03#C`={LCrFAPMr_dNx^vhs{0) zwtC3*IYGl5JeQj2S&cJbEV%%RXm%^RB$Ag~Yoa4lE3H$bceht{0B9uND?Z(w>TMWa z?tRnU<=Zisai&xT;YEgJk*9aRa@EF7A#_P1@LzjuJx{W#>r24Nw#;e)LuqB8onqUD zt11urG&{#Ipu0Pq2M{&^^wgc}{eZ|C28lq2A*|73GB*PJNtNT*>cQ|v>C|&=3}gx% zZu=(K7JTS-AW@3B1bP;EXo33g z!^HqPTX3$Kb*v!)z<}3%@_vwo1fVn?&`h0vf)!VRn|p&Lbb9ysJLWdPoUVp0Khm~; zcO1$wuv$3jqJk;UqtSS#p{6wgCeZ92EdXb#iuxX?+Xb)2!Rk zY$rF9-3RkLvK-{eA}Nuxv2a~ZvQie%%ccFs3)|~7DKiRo;t7w$giy0H?v}?{wiW-L~6e_ho=3bb66^{1)%yp-;e%#twOD{0zMPbE+OD z1H;|fK$EtuOIE%P469P2YGh=XODE zm5!iHvlh^08IWN!*{etY#7)KJewS`tsFNobXfmj@rt!OHV)RnYSG}`@WUy< z{k%g2H1eepjj}~UIupQ)QgD~qZ{Wt>_l*fgO&0WJ3J3N!Ma+N>d|!;WaL&vs0_2ih z#v^9pt&w6{jtb)MiLFBkJRR~atnbBlfl z+tMT;g?V$2u}?xnppsB(=wqn8i-R1Rns&4dl1Z-pOq6>t!C!=jbjRGJWFeF>WDTO%qUy`%eyqv7+6 z<olXnsd)Qw_!u)4aJKw9XdSb=R=dN4LtTH7X-S3Ujlq+ASlu5x^0OX@6Ld(QEyc zIJ1JE^QRVSU+DwPcXX8ns=*eh3z;?;O9tRiIAAaEU~Hk_)DiW8=j7zM%NIGz7Q6t{ zw(-CVtq?EQ7`ZxZC{()vpaWy+$!E0$Hx#bT#kcwJW%cKJf$j77vR^GIedbK*6u=apTuKgY%RR~TPC@?~ zY=x=`=N1*39gYbs_eM-l3fZT>9&I=_#+@!^IRb?A-0~V2l&t+01W+BFCNeY|g7D&A!9RTUD}9UXwg~?uUo* z>8nVDHC0bc&Dv+Xr5#fOnx1M+0A}^Rgqz06WAeWKA{PCZ79b`R*MFX`6bnf z;gqXH+*lB!_xwxG#n21E;I0)&17g9w^3Z)h&Oz>VyPX;i-FJt6<@&AsF`;fQo<3&? zsyrE;*-V<;1ZmTE8$4~1GY!>^_Lo+f>$la89A7%-2swIyPGBEn*pM^>EJLxYz zPbsy!zpW?+R_3>dcR|{V(Q8st@ED-g%e~yj>AGz|L+68tv%DJxvq^)XrN|*`wx3qV zHtHt=n$iN`^!@_9XL`>_c0TI2VF@_j>WLX(jERw0F6w@CVhkOam|G~8EDmgcWWbEZ z%-P`6{lK_YrI|^l>qsrS6F6dm?G$~=+&}TA% zzf6RId-bCg=F7ZdfiyW{HGxT+0=M=uglro~HKGn(AvgtD`$CAR1!fhIGJW7xg^#y- zEjK>qBpf=l)wjzJiouje05=x-Ab_Nme{q<2Sk(n!%&+VyjVWolzM%I%x~W2Xu@+5u zP^neZJq>jD%Gef|d!{oOwYb7+E@Mf}Z_6477MBK?i>~{49n99u{k1Mr<>S8rsx8=j zd7GD;;-adC04I^VHH4JXf&~)csSu3UB{LwBWe@BVc3cgibh+kpH}f}Fh@qUujpzY1 zZE)>}V|w~&v~6f`==onUJj-wxEQWlfrL%q` z3GH0K*zFY$?68C-oLjHuD=;3NMR?rQlm^}egU25LM`-!FY044fp>5`9JqZMkRWUgY z92sDD)z>tdfmnwFCx6xY0#Mapt|WD$ZGt1FmNHQMfCP)H2c)P4FqKCSqzgL7Tn0{* zUSs+Qa7u*$`> zMUDK@bKjpSEoRAy*2y!Depr%LlqSA0bm(Dkb!#59z3UitnuZHD-wvh}_azyyo!gfJ zG_x&&%%DA?3JKD~!z3RiV<|_ObYuQ3$$>tq_MDFj&J{gE*(MM*1QLKu8z|i@rDZi#E-T0PCu_v5Z zg*v|z8F{0okq8u=`l|H(JGBJv-^Q;%{-yhzSfJmoBEO7!LoPuN$Z0y5m|vckwz+t} zM2lD<7H9@lhHJ-S*PNE&aj-6hd~8nvMqLlhR9b*(N-oHIOu>WXvAVJuD&ygMBwAGE zH-#Dk<4&0ke$x^2eaZ`51HJz(VkBK>N3?3BvrNE!*FO}{$$^zmKkVumj;zq<|8erQJIWYBhBT2`T#4)p@Vtz@9nxM#&S zf{f}GQ=DE0S?<8?#Z1uFSh4oDb&ms{jI&BS(9iEp10cWoA*c0(8A%_oPH^C^822rr zpBhnoe`;z>vu?IGhIc~M05A|j_7!iM#Jf&W}t>gm<@n*oktF12SopsDrY+V4k zq|zDRV=`9hb8>|XRA+&a7rfb5?x}4u5V~bv52raSLF!%D3pDQUEr);Q*)@a#d4K#w zg)9N|va71xC&+;Jj?u()O)y>B@x{O^q7en&mQV|@(rN-VxqV>I>5EWCFy$ay*CEl# zQ2jdD->9tp^cY-!Q;$85t$6CYCtqwm)*sUSC0#JX3e56FHzGRY#$bLM^Z6e!OL=f> z*>fQ@e*CWNV* zZ50!4C%wl_3|>iojvlOXpl!sO9fb^>X3>XR>2J(%cC0ssVJU);chVesVfD3vW=SeK zNxreZK`v}$?kq$%el}EeWBz3sU*BqmQp*Mfnm&u0ird&e^&T6nIokqrJzU@sXp)Qz z9DJPM6d{q$mdcx{m6)a$vsrXG!?51rufNR+XdzH_v1Ogq@2st74c%SG=trC4K=@je zvI<09N&{Ig5I$5 zl<{I`8a1=s$PtLwWRN`o;MR4jk;qUo-i8MaWu;*UCK1Tq7BQOi7n}Bs6AJlh&_RPbpCyv!4uQl%lX)_RknWZb~UT)~j6yM8b~RUR(@Nv;d7|sI-`vvhx9`66=vw zV3=xOjF{+SAQqf5$Tqv2TbFcg42XH*KuBMI3RjWj)Zma)58bwKXoLkHehq z#WB8fVBXO%9yAj|NjJJAVCRaG0&*;yoFPFn+%0-T=#Z;lb^jx2GC(~_Yz#srM<=G) ziWoRiCax6ah{n+)%&MC_fkCE9*4`n7S6@P7Wbb~iC!aL#j>+5{^Fmg5cp;b_2!#fkj%Z_3XOaLC%MabM zqpd~#!keI(+q>lpc~lw&kFnKzKDu=|E#%NP05)E4yYKPf1o+T+pQkG%<)z|0|l zi^{!5aKn|tXKCrsHWuBw`6x~w3RX=PIx4C8ueX#b(VLm=49Dlh@;k|#9GC8nH=E%; z1!Q^#Dtn=Z2Z2M5402R7Z83KIVxuyHaYZsoaL)IIM0mN^WrG4uP%(xhWsSEvp-KmG zk71gtqvj@)=X!lbra(~;b)P#o?Lq4|tOX4oa;wWRE+;hCOFzhB>t=SI^_reNu7A zQ|GaE{bzWRGYG~V4ayv3;BZkr?c4h>ZRGr$J?8jNVDxX{FGT|WerCB&U)O#BIhlV? z?uk$GOap3)!%^5{-0(wPC9qM`Ds?a!3dEk<DxiR<%iStAPdN4OnXh#;sat;riWGYh;|Cu;PbG^#%{BzxlJfX%)N5f9#I za?eo(!LuxM`C>?B&2U0`o4Ui54LOn7sHVc9*6O#`ok z#=PD)Z_wU)ppm(oH}zNPQPPMh2Y5nf)HbL7;x#*?Oh%6~xLvJLIXG=UpeL_@{I_5@4H%8y$8H3wS(Eh+3VtwkQ<;RP14P!D-GZ z=FoH5V`m)c*0=qiS{tLPvc{FvJ)>aPuywG@P5#JMu;$OKclKvM zW@9DrnyK54{RfIc|8Y8Ru%M3sIgG$!Z?bImDAM(6+s??9^gIjNscqr%z_%89EM-<3 zL0jx-=u$ivGr6frQAn}0srX5gx@FuJdhpror4g?$i$=4$NWpmfo?W`zWJ$A(YdZSi z!2|OvvyR#32ON&wd@?t-);q?H-fHc62Xs+X|HcB&!PXPWKI8p3s!z?igk>`N%T@?M zO&qHmkhp}sQ7S&*-C+(K=^O9QfFmv5a_i~XT~_M4n~@hzUKv_@@A(wLXm?1?a3||- zC$+5>B@VB31#(qKHl~XDNbb8Tayr#cYp;r|EXu z7L;*Zv*D192E^}sQ;Pa!TAIp5coiqqxMSiZK6-)7Z-V;ddy8^?m%D6K+?ctVKvXCP zw$uOCvJfKn+TD+t4^^mP=|@@Nu=Pc}?)lPBwWz@;xJ5<29QYS$Vt$9TNuC|jAKz6& zS3mcH*9}0nCLg|mD*+T19pP2{&hQ6n!z(UzNN9nV8|-ZXy{&%^U^>qjSQaaMEKJ?< z%9=`QU{qXitG*?rq^*7q!`24}#WEfY1vILKR^X0AI3fCYp7dpSm^QfjXJ%<2N7X=5$zy|L zEv4F}!LgjenKD{?A^)URUhhhc?Hg+gXgf3j$!r69;0*piAlKIda`6}gcT7dEinM)3 zdgUPMX~Qjh0oFh-d@lEM z<1yrj)$Nm3;N^(#PYAE9?*Vi%X;!Ra5|H;f?R+9zT)bbSjL_V5uItvDc1+hObD4IC z_(HExxxF_gXDASGH1-xNR9iZXmsx7*fA)>3$|d{W;isQIw zSEUro<}5ny$Vn1I4wzys)-N1-MTOHDdOHam*&PUWCJ9egQN>#p{oqUjSGt z#n9$`&S2)-G^ajX>bDtyIJA9gk%ySZSjs0rOMf!+^Z+%)sCg-Nw#!fPUYJD))ct+Y zfy}4`3r@yL_|c80;8-Wi-BF%W;xo}jAuRXAK{1zBz`7A~R6HNG(XkM*!jb&og5YH} zq}Oj7?*n0O4u1A2kdYoL?er>8m&5MS$Uh*oK;nf#Dp}4kR8^+Oy6{P3T>-vPVc(oX zC+^j`sj$^V37s5req7G%Zowoo;v@k1LKCjsgnK8_JP_a}s2!de;YPLv*PS}iL1rzc z!Xm_GB+pI`E73=8X*J%`70qyVJMJLB)st1%*R{*s8&Fa>b`lw=j1guPlk)wpEgZC|4i$aDDU>Ek3&0 zFnnvvV67g?W_2d>jIs^pd~lW|@`4?Nag8O7Ds?d74wY`7i?LF$wF9woP6l!jfLY8e z@U%@vl`$a7lVo(LH?`XZ?r|{!e)#JWpY%*7v0X)g=;OAkBcLL$d~|#=bDqBHsJS5Y zN38ycCNrv77$p`%x1_$|g={3S-C}_6NlB+~?fq<4ivKOPbj$iJJQLx z*nxRbltr!3xbnF;x*b3wwpy%rp1oxT+9p!}Js5VoWM%5R*EeBCF%XWm+wbVQDu<;L zr(s5V7hU1RtK}6s(SK3u(GPbP(S0Ce)u_NE^pTreyX3iWsAu`YTlOn_TrU@Be z_syByrS`7=1a;x3sf6Lc9x86+(Xi9Ere|c%ez))F_@hq!ecRdJC)*|gp4W|6qK-Ou z)u@FP^6mUnYwCwADUXCQI;S2kxf~0c4XWb1zwQEYd;4!dMU~s>`B4xCxg_Mv*+5d- zAC9hX+&%^i0wmA|L_pX86fq)=msI2}>^6IX$JU&W8KzyEb_TI!`vM6)qPa+RiuyjFhV7V4r|@ z1;}go5mFw?^QLq@&P&(OUs$TbX3gujC&}lHNF8$WFBIcJ>Bp z3FD^05`uj0U;MDi{z5hAWq)U=>x?>!)U3cyY&&kcjzbxPuDoZF=BI(t9$n})Yvt{EH`e^BB^HOO2IUKvp?RM?v^+#L##hWBc$ zt<~GhRipvFTZUgt^}Q#SxzW8o@yi1n&TpV`obj`2CRbDA!O{{&t2?ZdM~~NINA_C6 z=^X#aSDMfxcAOLfIKgvC-5;KL-m{v`NQ64OJ4- z7C`HbhujHmhxTJ8`#`2exX@*NS3p!7&kZub4el9d!M2a;1zCot zNl$E=$83LEcXD;>D`fqv@EXeMEJAUECe)YT^Czi#-)#wxRYvSL1|j``9irIL2G|2i zNKt@W{dnc*;+U|!@MSh_G~@`(8qZ!XXK=8sZWvq(c60x`-tVM;2x(6x%@btPhJ%Ls zW%n%SuO!crWCd+Ac|uUj=$!X1K)QynCI1Em>w2ffpKah@9BbH>7X$dyrVx`11IsqmzLR7ocs|KC*L>p`Lq4SSAqkP$&Z+y*S#G{r z*YwQTF-F<8Gf5~{f80riM2gtfTwupfY+GQ8d3We!#K|$2H)qZGQbgM)4Y*e&8q)r; zz_us7yv-CdWv7cAc(JzZcFDS!1o;a`G@kJ?eXU^v`SKBJSz~u z4b8qAQj?y`XTuvBY5nz;UQG-*PZ~+hXVfVyp~+qH%>y+}$lSC3dBIa;1YUd6ZwBMG z6Ejoy!VB~`($YcmgWyQppT~aw^GNfqbyrmoUJxhq^OA55IicQLfqEbQQ*qjZk~t1& z*W~VY!qK_Q-vu>ht5sNjO2AB*_+GKVxA17PYp@uWQCs_DF%h;LSJw z`Ucfa6hykiEoGB}()p5Yy%s@Nno-2zQIc?ZCV3x9fo%37f(P#&M`EH`f1lmdSh1 zK>`22NdwFu#&h*$coRhi_(5~hm7FGm=2=dLT&_mtx@N7>3NeYe;KrSMT^o$OKk#7b zf)0yV6q8HsAnx@Sf$hnMrKysfuT7oQ_1{{6|9vcuLgta@01l~mq~_Yf<;p)3=pAU4 z-*_CcEylJ0}0>q3kvC$LHO^!sa36 zp)^E*!?;&Qde0>x6!KZd(WR&vPe-b6NCa*3PQbD}Y8V=kh|tOcU3$1PHpXHIL}03q z71_?gqDUneO;q6sx@oiu$x{^Kh#TOM<5(1eZEX&ph5EC{;1cd^#@mthD2Ki})(muh zDPD%sG3=sxbq+MMXr`TKmu4tGpi;NXIxPgmJbjQ*YE&GYXm8_Am!-h+*Cuoks&J4}dPluD$V4SI3Us)wp9Tn<3-+&Ik2B?ms+)7vGDW zL9uDC#Z|VI)DqWHZ70W_*vcZ3bup1v$)7*XVMxEtSvP^MZtl;~unrJL0${& ze`*tGfGn=BY>g^GblI7p2SCk8V9KLf8B183L&0UkZ3(GJnb%*$vt?o>Za@_zH`{#* zpq72i3Z-}x_bItYhl^gM< zG}w6*90ZZewOjPt5HF$2L!76d7wwebK-MTC0lrV&=V9!Xer0*p0Aa%bb|G-!J zTj6HATRBA2>06rf@KH$b$3No6a)nzPOAqB-SL7bjsYW=FQ$9aRdrd~g&d~^Kk<6B@ zQj$D62l{+B`-%bVv5~*j(v5QUeR}!LDs=zp?WN5z=gtdwOtKR>?n>*6R42xI)w#@# zei1$~?rWQ%Rkg2+Y~^$5h=U!l1hPk2l#--d1B$$KMot%!jX+*eYC`;MQS9p+&@{yC z?ky%gSYzsW-fs+ZCPm%w40aNOdv6zddswLy0i?>f$C>M?KctlEVvNh{>;XZkP~?6J zbQw9{W2^^|iLhVTW!<#Liww(;s}{cv*R(Q3%QT?Imm@X<8$1(Ca6zyB4>b)G3Gk32}3dU%=3@wne*u zn~zs!D^`;B4#XRVY!P))6m)>hEK>l2f2Jf|s>M9k3X`LOc_KL4-%wCA6FO==YlkQ% zTDE4Y+8Kin0OFlaQjZvxGmR1}(&oCq3KVXScSoH6#3=HMETbQ?r`KM6afP4Aly6j!M>`RDu|y|7M1+h^&Q#vSzF;$_x;P zwS3Dgp@bdVTTmE4y?U${{c6>1C`^3mq~p-I{`c`?`Ys<{TMI}?6$a|crfla{+a&qnwSi}F zfN1vO&t#_yl!oR2*)mRc9xn@5fW6GR!j7+vcOs*FB1w9q(BpRh4aaSN1vOP%kR)ji z)jS?arRgq_BGT4Qe3`OnIm$X)5T|&ev6k#!>m9_2x}{`mA_*eAy1}z~8ePPFiE%_S zlvUC-TS$#ww7;}w#sMQ~g=##!Zo5n?)v&_4Gia}b*^RpAWXO%R<@iMLLEj@HQ6CeD zPpZ_*a@oj5A(EVE0&2r79)x;aCpvmSrHH1#ARnPTlEDz}9A*S@l5qbb?X*<`RE~~S z*Hx4Pcz)XI*x7;}PMv$vXKPpf6{&8@?F5MJw6}H+Gu=%+m2nX6*S&g0`w|p)u_msJ zW)wAW!B!BG6@(`?v&;_R=)92h(Iv?1sz`|o*DVqA1=(nO^Kop%o0C+U`*2-aeC?Lp zLo1>8Hvm-gZjQ6PG%e1y)`hu z(sat7JPE6x6qhEpJst^ADEz&5!0_L}fw_D{sO(2jM8dadQgmi)K+j0Km8-AI`SDOo z5=B7K=A!1w@r8@0?~(nOiEw2vj6{`RGF}H|iX(#7wr^`MMMZ{v!m!VUam5$yz+HRg zNIf{bwtbiUZmj~~fy}lE0Ozlxzua*gFRR&@Lw?$g`Yed^Io`QD^XlgkB}>!$=#C<| z_pw`26fvFs>e8P!QF2We^q@)pT@4MmYg&07s4!y9<4kGBCUrI~)tyI*6WVaU%kY{l z*wF9K6%5P6E!BCn_ys-K2;3F;K@9TlKx-|249W8bbo5o7vc}cXxB(SQ?S4xz>mr_q z=m_!@!biFtGMghHspuWwn5oH@sTJ#-zgko_-x}0=3zCg&fw61Q{CKRW7p#o%$7cv} z-4xM6E)?Hfu)ad=imjSuJ_AIKvC|v^j=6mqJ=!EibJ41Zo0@!jcwwbwo;gzaNlO?s zNkt<;i571`(HH9gDr*RnoFIo!ydMz#IsDZ$i)$iRR}mC09&8CpV9zJw)}dldH?W82 zi>;+CL8+e4S;vsxp1<&72e(BV%ofddcidNslV)*&F`-v7u5sm}Z&zgQ%Hzq%+j^OG zzWXJ|;a-Qm84By0-tTvb)Nd2XmnKLs1n#wq8D~eCeRs6j=jB2$=A-N^C})GM1&>4i za;_ki_z58rj;z8qAS|>jsdQ4pB1Bnw?Ep+%DIhld>nEp4^~sZCq+0DFe_gKz0n5@S z-->Dj2=J%;-76W2*C%u^49hf-FJ{5Ax|`-h0z-K;kdOLm=up{;`UiC{o2~LUd!m~R zTsCU{={2Vc!{EVUk(f37mfUkk*Vk@K`;ElZ?pv3=_Q?w!agXAG>aW$8Y5psvY9!OG z=?=wU*3mK^?0a$BkC^CBWXtxb?zg)Novtz(2xr$&ruY&B(=V~Lzt^Qb^F40JFF2TE zVz`d^FJD1@aaTmvOHwOwU|YXN%t$KRtOGt3`Qq}F%2Oqb0?r(~lU7!-p*p1>#`Q49 zbjt;5=-te~N@LEn4TU9z=fj-MUV-WU)&v!77-pCy&&#gz zP{m8$S0hbosD7_iH7m>++e@QI4`)!9aedYn9o|eMnj)+Dt`H){341k%UHuHjqs6dW zM1hbSg8lx7Q~FFHta<8!knOf)SDGoUv+QY*^<~o~8)wxrOWsCSn-5ROx)HGp^gh-? zYS1?M4o>~f)pC|kFM$E9JSo-%W=cxU=Fk?!Z1ug(f~c7+p4a9orL*RD%Qe18z~~3x zYkAhvhJK&kI%ku}A?{KJwF#Yy0_ghMswMiiF>)IO*?yO`hG91Ri5T3V^=?#$d za@rb2=lByQO-r^(mA8;~h2*V)T9n+oT{z>F49@j!Y+EE_kS?d$cb7Q@^ls1%tDU+o z1N}p8tNv_Nah^Lt%6CW{CMhUI3JjbQct~9tI7#Aj!<@&hkS>yg#as0L+-O=$ugsu! zsd@P6JtzgoqG{7(dd;V`4X`R$=)ALykSJ}FL12XmpE|nB|8+nd5Axc9$uYJH?P5XF z*noo5U?;h;e6ib?(|24-nl7`v+n#TqQ1{*$D5S;Ld#P``uJ27Ao_Pz$*tm8B6pP-y z9#;0nDm9{Xj+N*lxU64R&+>e8d>M!?a$^-f=AI;O=mW?YAo%>Dxd1+5KzzrsfmGq3 zX#O8#^ja|BWXFw$t288x)7ipFl&{ymh1rlrYP7V&vDGZN+Y4V9VU9Pb{`w)uqZ(@sh5^(tIJw1lrYw7=)hmcn^s zmgl(Ij!vpv=E+0LMQ-We)juLDfB)(J`<<-VOr}dxU(Lk$eTUo}4p@wrjjt0$588@r zLriP$Z8JJ@H4QL?Zz)howuD@lyP<)x>o~m5P#fU$RTeV;z;3hZq*6D`dFPSU52dT# zlovBa9%l9wK;^(gg;QNeb(~$vz;Mx|0-Z-;lQiq8VJKo7hK-yo*V23mF%JXQadx&H z^e-HzE)*a4o7PBXyum(9RwWy-iNc;?aeFjBK^AGzm9=zk7lisC$!aFuhPn?koK;;_ zl}R?;P37qJ5eVhg-&Nr`CNkR&Q%3{;`|0%^+G@udc9z=7dHRU z;blsozVb#3RTh=^?vs#yOEwP}OTTxpIdcyxTWKzl$r~er8h`7DSJb}{`vf4RUtDzM-^+cvBGBAVcgvS?L!nd+JuJnJhKgCk;SN*B&)vpV!Xx`<0KJ(>T zd;S3c0zt2xU(8fJ&Ae-k8Fz{w>NHn&!NnTJUyE>q4*rXK)5{K-Ga)HI^`KuqeaG;% z?>#~QYWAdcXXw|@{h`e!KYQKmvI3`nfA`Yx)>%>^(K3hwY5jCY2Q$At-T1uh$BM&b zz&1xaRD*<+6Z;J8-PBPL4daXwAjBn%0hY23*F6^oX8X`okW*3I8<*Qyt1_8q@KZ4OmW}};MTpBgnBBcsE_}HEMN4qL#$!!SU_uR5#iD4D<}v*BF2>Yi^296f664KDb`6<#n=RYEj(jd^SHEb1FsqNoG7&9sTAt>5hy>N!S)}$l(YC zo;ZfuYPPrc_KjDuFV1&f zy#x{DQZ)GIVd=FOc5gus=XzD(CT;h_Sn}Cn_p_>t$(S2}wRVi@>WD&0jVX9J*4`EA zs+Ij7q%1Ri1pjFowhcaTta!W#M71l6a~|wC5u^0!hPrM%B-MgXo(m5w7v9rTR0s8p z$?@K|^a3Y{j&#M>|Kf^sryFwrdF|LK*J;C%=o>?oV==yq3WB=PwR0wSGPl?HaHezw zG>diTGn@XnCh=)+D=_!P9lML3%=N_4=Q4SZ?Ss6riSj3)nuxVx^^_8 zi#aRqG}hjc<{5fjI{}(CM;SbKk^$Ul_v_+!TBybby(=wKyj|6*9vu5aUtZ&BOFQQ& zkgX>YP;T()Z*&cSAF>5&7~a4ijtu>xgkvuDiX09R@{pQMAiC(~GKf#h5BrS}kyUck zA5fMZw;0=>LGi&90!o5+39<6KAWk=5y!~NDAMnmSD+S?k;{kq1tWOh{1XMl-=6VID zhCZ1wJ3ifS&$Uwp)hcuS^U%Ql6#`rT|LEw}zYse4-t1%X39Z5%S~lJMpgEIE5o3+kf&VvO((ou@v@hVFWZGPmHiDq@Niq=P_fDew#4=|@qG^qjgK3r? z2;km~7+75)))EY|>SI6u)1ImDtP|plMlGMxhw_m`abz9&X9P2LVg&xYqVsmyP|MZ( zt64J)2qIBB+g}&pgmqk%`yE;ut3R@fju!&j`=SR^z02Ory)88ek1`IHvDH9+k}ciO zlf*{mKMs`YuGqTtDK0MpC&6S=O)diF9>1h{wY!qa=~Juef~%T^1vF<;pzI%*fG@YfLF-l zdT$N4Pj>+MWxV;gwy>0ITx59=v*repdQEQ|S?j&n@aQnY=}5iUw^ zZ$c*D!46QUSZUnd*Eu=2fn&*niTFBR(?ynWS+e~k1(1ILlZcal>RM>71wP+U_waCF3T_DqC zQbfMk%4!XBquj#t)b!`-b42|e8Z)r$uLrUta*cG~`_;0hX|l_7&* z$o{f=qLjCiNOcdqE9Cx^@jo7n&jX=`RUT|z zDI$K2K*>hQB~k(s$c?CLY$PD#}1Ak<1=WtGAHDv0#4QH#sW| z?$8PJ0SoyU?nQ}4&+3;dG+~*U)3>_J1k_dS7-tD|om|l=&{ThuLVnrOwnY*e#3N7y ztVXmbFLYc9LWOS;KK-RnavWYTAJh1w9eGr`4VN1FHB^KcNYbLu4uN;}CLxzsu%(vQ zQnC_IsP75-fS9`)*A&rjB+QU?@2$pVRDCZlDem+ykRSK;rP$dlvHc#C$9#sAk>sjaEEFE`dIS!Tq4VMe|zn(vH1SqE3w7(%eOCE zwN|gq0g87f;)DUk{}gJ9AFBlKZtehfmxC)A>f;J3K4>u+*1}ws_VFl$+l?P%wTNT} zm%rGx^te?>`dGG8;_fLz{@PAj=~z6c^0=F66}zgNDhr|{ULbui4Ptwg-}RR75#=ed z?HA0CW`(%%M1;4M0uv>S*$T5j7-k3@OX;-sfF4k?7qK6SuK@qL;TF zJi4->cv|t$9k6<(Yz!!gz**o>ATxeVX#L9l#QI689gb%M&(02KChjSRZ>bAcw=CH! zaWs(~%GWbPH_YXWWla>^Ikn)J22q(E1SdK(v^Mq)L4w8#f*23_feOx^kVr7<8V$!< zUwO9+d^0xDY;$IiEwiOd)T};at_l~bm53HbnyMNTZ8)%@S6K8YrW6o4gsuTrgV^0c zYY?RC!=3c7dr~4=aO?dPn!5>Wsr6wuXrg;%|M*k7bGiHKXugHS)>*qwDgPCs7Z}s% z@dq#A$10yjfRZu`kc`MT<2o3Y*A@Rjva$eG$^AFNmi>z%q;6qS@Nw^lJ9^w$0t8>P z!9rmB|A+D;=U?yS4GH^n*Sk*|Xl+c7v8v&vZkVMo1i4nEt*FLh1j!VT3LmfzazEL= zdE*M_@)uo|j-yY}B6;RJ_6mRJyEUI5_JOe$z#q z!(O}LIas%2VZd{91`HdH&z+wDTD{cvaF)tTmgFJa$8}mmO3elF0>r5P1lE`5k`yS{ zu4Y|cXaUMwm|OS}V(qfPfaiM_g>Bh*E^DJ_fj8AASGLb=q2vVKAUB&GMnZ|l6a$2aVIPOHvKj(r3S(79i)k*!@QBDiXLb*-@NvPOFlCxxe^^w9z~q=? zw}2s{R%zw9fN$~UpTJSNOgIE@cf)F0-skk%H*V(_|{Ghz?0eyACt$@?N>Yo((|)>=(YUpny+Q!1L9i5eoHxcXCtB8;$;MZvLXy>Q~6v$r&z z|E&ceG_a8QD?N?XFesd$aXR@PSAYr+aqA$3nT*eNf)dgw;@Y`cwdL_j*rK+Fq_^5QTzj0^p6K9OD=DH@1DgLchlK>c@RyvNN6FEf;ug0`=2R9`4*{^2V zg6ztiJN+tj8=RC9ldOx{=GgK9Dv_6}@?{ie5U6`{`3O_}Ew5PfDJ2=wDX5211Hf|w zIu-?n08D`%cdGZ`H!^Y&jxeeBjc}8xjY!hNM8cB`)8{bMg_$`F%b6XEdC4JSC`5b? zjqA6awAU9!LRPF5!?#KxZ6nF_W+rt&F8}+`Mj#-$8pmHiC!e>@(zpM6rs3rjwsPFB zbb~z2iJ(d@{6~+Fd$O;{sndXHcJ`#;R*m*qQ&ye;a09OlHfh=5q3FssKTVZkJ8DhGCDip9xzu*kt)Jt=Q_B8I2p5%@wZ)W zy++0wQx7u}r8}>{PuXKWGJI%IwU0-aETma@p^)ovYYS?>gSNlT>RI*;5KVjCr>$1k zC%2uI4a8(@VAf4n9Olt4pglIopYKJu&Oz>u#qxw83Nc>~%=r0K7P7AcN`$?4tP)jX zT#J`bmCCVfjvhrX4mXwvI*uI|W4%IV(lrWvOaV;*She9$HTw#Pp^wb_o!nS!oOdwT_t8)}^Th8vW1=Sy z7A8*!%FEmuT7{C~&e9*jPtWc7<^XbZY@E$}_@iIKMUD8|EpssT>%A`&`~|%B)av=J z0enHt)bVi1?>8bim%9p_g*E8=WFb1?J5U|X zyS%Z6Hmi>KABW{pz{2#u8Gq>g&VK~87~rn05a`xGFMr4}kB&QZ8i$mbH|+BntGNhA zel+)Q9i#qSl0&~-0<2{5CU=v zkVfx8%R(@IG=E~+_SS}*0Nn9AiKbKlM*XdivN3YEE@WHTwTvl;OBj(h!IgG4^V zI|O09?%`Ub+?RmxvT(1;H3Ki;NH9>iMZyECjd2NrHoajE<-z30s}e3&<=9DL@9&?) zS@&w2M8!E*AsIbyNZN1oe@K|W-t>b^SvtGpM2j)Ta3uB0as>=aCh)U6>Jp_yiHMx> z+qmfA<~IpNw7NcA%FWw0eCcNTBz{niJu)uDxsqbIvpIbp--rWs)H9pJNicWc#tuyA z{{vc=y)ioqKnma%p>{lV{b$v+^?xcp`YuN5RJ%sFNuK)I3*hQi5(Pekh|``*&uh4+ zsH!qev^3m^uudn$-L@VecXy#0qeJ<2&s|0nQgGb&jhZ+yVc|5?s9&6Lb4;9kj1EwR zoc#Da5wYp93N>puJ~r8!vh4cbVkkoKf$21$`$O(~ECQi`C|WuBEFvre72%#kuwe+P zh83-RtJ1XjR@!q6;NNk^pvNH;{$_%3}-7hI@*U;jF~6pV*jf&;4n_CK~{m47`qp;Pyaa`=BX z{~LZ7|1f7}ZZ2Wczo@FUp*;Pi*+fVo4_1}Ajrp13(@SvHxml2_uEvdhnr&9i*&lE={Mmu8wOTMJV4uPE9;P!1w95YX(z_mC({D&~ zPNH@#H=)W<-FM723vM^quO>+3BiwjN+&@qT=b!KLQbRW?IxTPci2rz*0)|2XDBEWi zV5AFw53Hx_Ik#iCZr44_IWz#{`C(q1^e36v&WtrMJv~Eg0v+rU5J}|)hJM}xbd&=< z#o-zArX}1mOj&dU*xT4%EBl1e{Q4H+mgIT-^APy4DwO;XrV(HTL+7WLKN3LPW5>sN zH~`@a2)gb>$cKw66Et`NMxx7s9g6!d747#e;T?l~5)-eh$BFmpaa{cZ=@#?@%g@hg zwbo@bwP8oeg%Oa@3!ssDQzw{h?GC!U!bc3;`!cyit-rY(PWjEkqxV+>i<4f+K}8QB}o0{V+Qk$en(g zs{W`GrLZ%t@xh}I)ZyofytXkQ>RAo&?<4Qq1ma!m>H=x)X3Ryhp zDouF%wLmtA4qg{w2di;9HO~dxDNhQ5m=IL)h9SB@*#NACvEv7LDjq=3C7F8uBfgmy zM^(vu@vEGC))_on<*zhMb(X}9jii|ks-wlr7#Wzp_w2Rxzo_=|Vy;Fi)?ejMjMn}b z(^uC;`eoNYM}r4v-Um+woS03)2gV>!h+t|KauRBl6nWg*DS&Hr?lM+)!9^Mr??kwX zPVNjpQ^Iu7;)9w%o`)S|w+6g043!xGhJfS2hHrQSS3xRRBGh{d{NxfKaO59{WWMqN zfFEqA4+-GrZlw7sRy}R|8Q14S07Y0$2b?(W0$=^a0i%J%#WhNz;<|3}i9z>9M|ZI3 z@)iI(yq?|57C>KBN&;c5yI>p|KYyd?2a;9ue=)QKt8)-?62xELjWGhT^@(X>cqg#L z^yP$m}D%Cq+-Zluj?|gaw&WOptfj|g};q{ueeIV7nmKk+i zpzq$`3u66#5@euc90h%hz6yi=1mR}PQ@{#TSbi);)92;)5juM_u&E_4nc!;}$^y>O zHXQUWh^l&Dl9Kz|S8Ssv@qzHCjND~#eat-?>Vc;s`Gp(!X0hM*;L5BnXo5P+zTx{> z=AknwAz@>xNCB-vEo#o5pISX*cl8ALklHs&4R=5^+?ZVh!4V+TDK-mI_DXxIZPJ~F zYa>iFQvj{4*em!@nW5l<-KE?zVjQy8o2*N~8shv*J$$S7OR~TW)$wn5_O8%pk$($e zAAu;vW#jK+KoseHkfxCCnkH)A_8Is@a^|cVmghznzkWq5%jJ0+wpJrNeV*~-2pUQ~ z7vUJw^pvLveMNMaHvtF+z5-r#ggjQBo-mgk-15}OLih?)wx^`H>TD)^&X%VY6*MnDyaW*ONcDH z5%QhpuENAR_EYGu(C@GkF=hpF1(r#HP{Vv%JfP$(BTZde8Io#+;KEQUJhhAFU&F8^ zt})OtJAAlzGS>4rQm5F=m7SneOq#WEFqB8wxjzM8XbSp*e>dgcDW3YW_RR;qSs<~k zNBn^;XSU%%w9?N+ncVb304WZg9nUdj&VU0ny^;F-i=(?IaOCD+5f~SF!TTcg#Y;N) z8Cc7B$NzY5J59r;nr65A_w83^M|;2Wdiri2{GQnmllm$$Z8_=1RC^pNb8Kg9;*uVd z0yGFIMkN@26V(fM^D2pPKKCxAKFb;`j_rx zyI49Y(1!>S_DO~|*Yx`L*B?wk-ZCt=ear`xktI#9|1HXbK0SuzgH|7qGaYjHE!P@Q z2DgBQ>GnAg;y=r_6f+K^qu@mLB^i1Q>3$pG zF4K`Xq~cz3fY$@=$#eVUX873jg;4#^E{f|~x z<346VjywONs(br2W~M9*_x&c~Av}o}7jhvrJ@xH>d1*t%Ui1=l;-C8`^--_l&P3E_ zs$-8VKg^MCJ~<#iun1U5J|R5<5>m&OOPO*iElK$Yk!FQPg?{bvLn4>&UR|Lfziz47 z#A^xCw(hS7-K}(*skZ@AnF`iExzQPt3A={hrdN-O81pI4$(QoG2-Lz$n!nZ${}Y4=nt6^K!3S!Zdq6TMFr2Hp35%c2aktiWR+kn|7J7SMXp_QoUow zg`hF+Rnz@J0QmmMQAsY)oD8Tu<-%>$md&)~m;7f~xjx~*gDFmDR5#B5Qe3+jw+Pe& z_LQUJk?@ZYUZRLs|N2cf2jbi)Fzp&GwPu%eXiGH){ID|sr=dPZN8^kxgel0E!!ewW zUjHEZ;QxIL4Jvs`zV!Evj?bt1Mv_uwy$ z#;JBB1)pypOMuN`DNXG_pYyc&h4ONEGCQmfsuE7Xu^$V_H^eQJX;!({eju|HUYnga zyegsPb(ic4u0}SbG=zrlmiaLu!^D1X^$rvu+wzYW_!d_&3U)3C4IO#P`kfE?OfL!J}yaK2k(e-Tk(yI$FA! zN0JIleguudVNE7YhPN&!!TcL~sxo;9Mg0NEy3Y#Vh)D_aRK56n>Alc&Ti>=dC?*x; z+iFv{&#=U1uCb38HC@AKpdE&_IWYJ5Dooui^A4}o2z~a&CK88;ur;X08JLx0hv!XN zTA7J0DH)$&XqFOuWjlGk=}(UK4@U;T#`#{)e$(tY;R@skv?D~<<|lJcPYM5+Dl~6M z5&l%J*kGYPy-g5Tcruw5!V$JZeVb>3=>EdWEMls1R&_F>827>+ z3`>=Vk&x-xY>q}^$p;+zqGj-4;zJ0Ok82b0m7$9IDMg_z+_b1C(voaF0UlH*M$N3z za+$qw(WWj|<-HsUt?18Ym9-F-=N&q}!}8ier^h1=6>a)JRpmT)T(W6t7fdrHpB{MC zZ2$Mzz_o{3?3%maGaLcNEsFArDx4GuF>3h9s$Yr+Q~gqc?xT-@M@%u~uFTxM66#C~ zjmZ(fW4F{OxB;UrFJ5n27klk><$u5azbP^EGU-0zwu`ZOL{OQqErVxeb`8;TCjUjY z4#ha~gV_Q?qEi?kpV*9ls1(ZDQdqmwS-#Z={#F2JC`~>rL?^Zp-+Zt|_4c&KW#JA9 zIq8A-pb%i6Y!Y?#io09=MPvnKpL52E65_K$3lwPZE7zvSo=&?j(42$b__ue5RrFyR zB%7wC2I5$4mezNq7^g0;|O-2SQ1)4Dl#4o7fL=Zuj$ z;iJJLPARg3rHoqCDqHANsFN=I8doq^(osUP$+0v+YG*mGpl0iLeq%}*w#?_Ke|N@{ z6H5r188ewF5TycmI?4BhkW%|FxW~;WaQ_xyo?QEOd6J#hnsNR!V9VP!IBD|GBs^%T zcqAcELTnb+zYg~Bh@EhFQ1#vKu#B<2%_rKQ{j9E&bvBB~@C*R+hCHsP+wG}f*5oVq zS+$r&TT=BsY!tZGB=ODs;OT)q`U`iyKn&Yeh-G6VwU)TI1nv9{1aFbX4xHNKJgLe_6Vp$QXJ^S=C3Wo^%_%|F>)MfAA;-2kn=XwFtL62%GYR5I2y5m0vMm zpnyooO7X+6az(c1(&F*qTfn=~A0n;1<$Ocgb2w0nbQ}FkaZ7LaPE^={K=Q*IlztdF zdk7qZ9QIcHL^l1Z2e}(1B>%-jzAZ9N^Lm=%R$TjQB>#pV=(MMezIb-(Dx&Ga6Dmx@ z=YAqv?b+v8HGgONyc_^mc3b1^>6orK7ntG) z<;C9pvm_@5%y!9*W1?LsOy?K~!XIs#AI<^kp3`AGNWU&FpyDG|8JZOUCW0F0 z(^?pqo2Yie7b?``?B5vm5jhQ)bZ&2XtDi0Ps)KRayzEa;5;s{ple+o^GHn~T!-^VO zayH`Du1Xb&Yu?>|yN_8u3l{HBr|7c>IT$5@&dOVVKlfDCrR?8Ggao-=zU+ur*L;(I zl+-HrH9)(8aQ%?|&r5~;R-OWi5GF`X1#!+V8Qr=DZl(nJuU}^?{)1O*|D#4(AGP`C zp|KJL;@%a?#LDMC@dFn8=q988T+Ai!jWxF-QTx=y?+BpcSGV-#cruSsgGMkD=T->{xzKM8t3s}WfB_T-<13ZwV5GM%=us$ZTa0- z`#iy@+xb4jelTS`=hh@6EEfwRARP!R5$>1IrB4I@_)kEII(5lg&^>?^n0rNoWpY&Z zadRO@NjMWt8?Or}JC0kZlqcO1Zt|BXp6stF$>7gOIA0NhsfsH>>Fs6po(&v+aRRo2 zRJ2`kNf;JUdYVs4`@jcZ(dw2`bPPv+24g{;aBwLt{t|He<$e~jn<0=`7K2J8Xw#gXy3-6rx6=OL93KTis#YJG9%rL! zC{*PV23`JnN%D5blubBLu=|cn&GZ7NV?!U1qE5f_7-BMMmE2 zJimUj&H9*&0PzoglgF7);deJ#8Mo7C4jUzyfjR27xE;J zby~d+pll$bydOyX?^di3hNLd;v|*lftZ>uSeI?G?BaowIS9g6^g%gl1)!a<>{Z|Ko zTK8`Ziv&(Du@~FtnR&uB-AOz5ogZ#eD>+r4b*3~DxwYi|DMF{*?0JLtxdLPg5YZlN zMg2U7_t>Jn?<(DW%%`x>eOttty$){T%*eMgprOs|Pn9Isa~1Fj1rcKCJ8c{^IaeEL z>R>5t7YZ$R(DN=)TJKOwE?1)G9;PFGO6rsw+ZZza<4#iVemJ|W5eAW~xfu1v+l)pu zXphSUVGbriPrnY3Q1i5|BYcO30Wt{;Uf(u)2vs&ZJ(at=s{Sf<@x9{EFpCSXGOneM zoPO}%2YRYf%hj>vQG0?|NddQiTQx;Qa$NM`#|QmyYJn(1d|e4s)KCDrii3WFIar|& z6p^}z4Tfuv4txo^YLUo%+cTTmCS<7Dmv^{I8i#6uJJ*24+f794D>)(PB>%wp*;uWB z0B-tWBL5e#0;}h{t27=Ynf$(D78A6N7AsoubK_P1R}77-jxNUSm2T#jITNBRS)t^swYI-SP%e4PjAd95(7kC2uVkW=*N~!5Bgjy zy;*XoHGHf$6`=j%+YS2rgwi4~w7^K0fhZ|q(TJ3k45g%?bcYN|JJR)DH@o|b=lMO) zdEfW^|7XwHGwb56bKl>%uFv%ewuQLOtDA7zIbcb{j8SW{YmA-+OPX89E27o5k?B?2 zP@>Et^cyw+JoLWc(0cTnkto?#;>b~m>U#G4tc8-L55twPzD=M%Lc%@c;~vMaDR@3q zYQ?Rd6SEP_cF&*B`68qEGGwzY)=KI=arnK}@~mV`Ku@HSqEUu9Q$UliL-)nA5!Hit zmK<%gM+5I{cGj0Lr~bbX$#iMAtuBo<+ads{&c|U;~%slDmfdc7BidKd~#<}hU3GQL7-aml%53_zx3#h3SO(< zTWOFBQ9axUCZ)=slN}WC>n;mpR+V!6v-W(q_8Cx#HdXTbn0gYE#}0pU0envA+(W4= z<9Z_Fh&RD-@bRuYn7S8PtUW%JGsfjpqr%Uh{^c>VSUoNsAS4-64+*D0&$y8ZKtfYOXHsx6)okQL$2&4Y5z1Ku4)M%xrv(F!CQe!0t?-UU{10M(9f4E65u5iFGh_oEz zdcC*3F{=3mMr|sdFa6`=QC=huqZl^WkTCF%6@Y!`2HMDeSLJ8t9n=~}q&TV&qtV1r z3ou^-j-YIuY1i(Z$%gQdxX)bFQ`ix;?p{G{D)?KcLU_VFL@7CNv{8SOLvw+JiA6wY z1H?jno!=MD4GY=!Y<~KB?951qo9T9YuwL^!lp%2-8;+JGM=k@2m zXFWd8(P!;^Z|;#=vivsgn8^DkI{@($I45Ib#hE)`oRVvF*gDa1Lh#wOJGB3P4aS=> zRo4s_T!(VN#J}k>tHKo zZjyu7ud$xl-Cy=ob&+vtPjM2>IzwGUU zcfdL9H?^}I995OfvLmdw;kB{SzGS(!ZY zL)r|UixN?$>J{fdxgGb9fgm+80P345iMms_urzAORD%E-O$MdRZB4O!k@Ts$BNG*2 zeI+XLD=GTdD#|zC(LdsvM2y;c$dq+v-9(>!JjP;0()%EJQN>y2N7m!JFa7N#li}jZ z(MVlQa-5J4H7$pW3$f~xJh-@obbjEFi>q+W*!xEn)0O1vn*yfluEG@Q`osrSVKKdn z%rU46&u0?a-*Oy|6zn)0*lKv$wS(NCZZg8{!uVai>&b3JdWGeaJG3sYD$)&7sh7Zq z%IO03wf=Rksul2bb6#;=M@p>U2?sn@F{!NqYZw*f7{+@8yu{lbT- ztE8fl_uvYv>Vp*KOjaopAvaE`0>MRJPK+s4E!6i_;^fg$YH!p}*h+Oef2)*rtj@4c zD>3_h?6%6RI~mKbBevw&7_7XS{uf4)f>tC5wUiZmwvO7#m5b2GL97&0h(pMzdVMbo z=G@X*u&`HP`kg#77&kP3=w<|Ubg<8Z-L&nbSrpdM);XBO^Q%ciG|p5x%05h3DJvA+ z?J#%jM-@c1JxBZ#wTZ1#z=gJRQB7BU*td3L@|gV#4wNDIS-ObWK8p^NHMBW zhBia%EAWH2LE_k?sjR z(TkIBGN6n|)WXtku@Iqk{M*DgfE|iPb$H+WbUsy|}7}%_nY`=yXTU!*fl&}W|8-@f5 zEl5-`J$UnUn?A@q>`2uhqkUre_Yo8PH%^w79YUvheI1YM%gaCLEuVdvKBtopGo7z= zm?=N@g@m!s^_SJnKR3(YomQW_%?CS7SW;mQjK*{Zsw(2{APO{2opbxIJ|iQ*a5rq# z1T}$6SOb~b85j86C8%C>8HMbkGx^wH8+ndjN)Zb52NUE1ATbQ&?E+Cb>7<@Px<{-f z2~QU%2aYpvK(Iep!?DgR00K=Cb@dEi)^qd|bK3AY9@+Q@Q=+~3DHhiiBuA=3jlhl zG|UG8Z2I20oH<`H&g`u7nXd5GA6XNevbyF3iNE?+C%n0Fp|RqG?`Zrbbp zPfYpdEy}_?Gvp*Pdf~C6M!#6GOoKpH**9j;5Xj=qZc>tAMcmxUxqmb2)C!gM<7Vbx zwk0{&+-^??2WTZz~%DS!j0eF$k)=nDAi8BW|GEbdxFXAS3mhawz4hnMg0wxk1nB<;9Q<+vBsxi72x!-LGm6X3OQMs;QvQAf!kA4ZqD!S+0`C@cRyvW7h5beBZ!4T z{S-8Ke|6~K_xbO;_JD-Px_UHLrz$xvo9e~m-ZzFWr*{NSrIDrE=A4z~XcORcMUfd$ zwoj3v!;IALLp1!@+n<;G(8s3Ges9SxbXe42r(*0Kfb=*6YMbJ&!^_7NfXXo!QBje* z@`(F}(58LnIm-qzIh(956=^?9mO{9b`ohL$H_nobCmrr4*I5?`TLv{8&Ku3mKz?5M z@f^LM#-n;lxwd5YUc;h<9HWAq{0}WZPo_Uw$v9yZCT@)zdg8L+bmP9A*PuLPj9)k2IB5NiZPvoY$WB5fbRI}Y58Heil} zQOgBTpB5AUyD2f?`7y)X>_bgs18Ka)7Tuf@rc{#o7wo1^9)GTdF%7$$U^4M#AF*8| zIc(O9@iSz` z+L&#G8#43sHRV(r4cAo@ASFmDqB)a3+eT3jnfWs@qk?a_#5L|ol65Y#R)DUuR*#Etx4*B+3cRT> zZ%C%1_(L|#?pnMG{#Yt#M3Pps)esvGH!zst@n{x%q|Rr_5aLSQq~sLwsJx{eD>eny zorsY&TKKr!p4|48AFoo;6lpBv=|}*(aX)419{`g%YsFWDb9A-*JWdNa5*@)eF*WMAh7ynu4>}vD$n&i z`MR3-PutMZ1509kvHC4N7=fg$)WBUyJ@|MSF&9|=@8$w3m<@^EZ9a5U*Sbg59yE-? zb8n;JY7~3wLe6=B2jZ{xcz^$s^E23WXZJutC?Cp46N~<(Zc6fC);E0eyi9lv64IN5 z%#!I9mhQG=i<5Ga$f;4mZ|LSnbT#hP!%2vVrFB|d4%dWYc-Je?_D(6>aPJK`;$&(IkM(nQ})bII$u?6X+KNZG^HFY-P zae=s8oG*AnMiuDbm*l;#7c5G0Wc1O_cwHG(c6}1_lb6_>lwNWJ`>J?igq)U5>EJ)p zxtqP(@+T3lB(rC$8ej<|c+l##PS|g#NGhDDB0AP#iHai*b>B52!Kv1hL__||C23_o z+C-BBi%-t+oF}usBPR3Mo6X$GPpCA0!TdE5fUD7vH@t_N5#52lKQx zo*WRg3i(PDYdT%TGc!&6Sdc>cCg$S&ZmrBd{q3Grig2*rQok1fz9y8h2!Ax};Vg&? zb*Y~Hhx6wJh}mzr*B&h~f2f$SQ}hY`xDKDT7b1q1!1nr<(}y3YeHaTgs|1w~h{F2Y z?Z_Z3F9?yl=d|^Ct}3^CLSB>&gd2M>AYQG9_Oo_>!c^k@W<94hqPS*8zZ{rSQb%aU z5`!X~&7S=hqz4qi?4W@+S2N!wwz$g=E?Fm-+JMqeai|F<2#ypdK3z^d_2ebxUBpHm zx#4s6ZR!k<JOn=UuAA|^{(m^40)}3b-i!@-nvr@s}Oa?k1~8w z$5F6j!WCFvl%h2Mt5TM2f>Zo$O}odpoc-}`jAXIP0`Qb`akFz7k^pyffLbMMXf zV~9!e=z+&+S&OfqzG8zfP~QR)FG~(&J=OPyXbg|EyAS+1D@BI`|7a5Qh4ZN_bqe9q z`G%%dnuKj`9p@TcLi(-FuSnh7@8<`JBog+yMXK$EpI={2I|rkObXp;7A%V=+0)|QF zLv7$J5cBc(26k{FQbUzerZ3`QlgnOZU3SUVH`_;@1yK{oMar`paa2SA2DCR!R}ovp-WjCMP~K57-x#L)g)lop zveM5eVOVg<;nq?CZbNNyy)}N@iNJn6yLRywRLX0OkqO$l+u&DCzchnSa#kQ9DCw`N zk9!C?RvsvkR}?R&=^Hyr5CHn``3aX@4`Nq5YNakXy9R1*QS~|(035}UI4M)&X=g;l zYZOFgYl#r`5Rduiz$j<~*X;IdT0$p}J%e*g^!-_}ehz z-#j8tM%>5e9={7Oe@WX^x~7x-whug|*CEw;maFc5Ip;P416v}z^3 zjmMx>qfZMrznoG4j3F%vCOleY1A(2OvAhZHC^o4O5nT*-NwfAktmdMp<_3&@Kt+BY zcRs4v0_0;|Hn5^P&X`{e6mU@hGPn?3jhA@hLLEye_vmBl7YqW+7P@+1^74scwyZeZ zCHF&7K9oyC^C!p*J-~k0W5a;rBG!skSR^TK@|We$;MZE%>ud!UN=z4dZu^lRdXGCQ z`0b=V*D&ek#yQSKZl#}}6Z0z62aUzYuC8-2R(~?VlXn z0mKO>)%ImX{6gC*#YOekoLym?kF=01gm6^faQ39CJ!)QEK=3!gKU#V})JcRj`UGs~Y_@I{_*0`cfq3u`Xtm z>%fIJvipHlP2;zjZD%J+pL-@(HH=p?hl-t&z9l`)&RI_gqn@A+q24k;DV1?^4%xXgbDMjJAUYowVKLFFweqkUEDB?xlEYUOGD40;al9RavDhr}L7V`Em9XXp^hlR`|_Vef_pB8Jmx7U2Of~``@&Iuch(zId-=jEBNki z{^al2Toy25Lg3zr)ByROzeJ|UXyi}z>E~>?pr00eFgHJLly2U!Dor~brNeS;gPG+J zJ}sH~SG_xnD~BxsJG9n#F3&`8JeidAc|n%MVIQuLLbjbkg?ExWeh^Gi)FHC-N6`PA==Z7B6V-5*GM7@_^-d}*Q(uJ zaBlzfkh>k$_#LUKH-(&!6N1=ruehhho2+6fWV4<$ZX#Xo#G5B%I_59IW3VTX55-2` zRHZ&t1D?{(!WGjrLyd?uufFrtOVp=3Uu~1GCRf-N;ZCVgX0cD3v!S?>Z;%bgl8x3S zIeoZoD&{)Z+A2aKuPTgH47?|4y-!qhi^zbgURMh1CM6s1|0mv-BnwKZN&83@f@(&^J(ZK?dYu{V= zJH9Uy@qw=Ttc$8jeCmoX_fdBcH$k7OanFxx0KP`alIIyCh~@}B%(?& z)OYD^ADp+Sp|%A#_;iYGY+U9fU{^ULomr|alTl?N6WPc!dl$j4oPtLN#mGi z48`iSR?q~IA(vX-;nU!C6x(k{>7@b#qbC=zwcz(@1Kiir&34mpV{`P__UfE%Br7-u z_lmJx|xt9jBUH+c8ixj`3Le;W@jNrOR7Q_3L19QrHkbeQ1I^ z&83md;GIoeI*b-d+l5x4Kp#>#d*%KYG$@#aIRD5sVUL}h?b+4D{58)b4%yEJHvaIQeaxssOrhVL^5^0J8U_e zmN&7;s@Z7QAGp`;hzruH50su~nH}Fhtj1T+s|@>7$^X8B)Zl|!n6v^gl~~(r=1$wj z%KzbPyJ(MI9boUq$~^6}7G-}@&guci79+*W0LVx@Cx zSYdqy(EICiY03AvT+_Q4%ZAro`{T5OML!U!20$Hsyh$&Y)&Al%$#1HR)E&>+tI01! zrX^@{^^9)wB)tnRHBb&)LLCo8=vtgFDA>ZV4B69MH55 zWK_M{xWAZ|*Ds2Yzp!<09H4y@)c6ke(k`_hc+d77`Ae!_fU1}i^s;^{ z?a+T#3}i1x%-~TRR~C0Bj_3BfMml2y_QMwDe{%tpI@{-x6UY>iiLnXC#^Wp~qGq5e zDUoT;#FZeTrY|B5R9Vz2;<)mf-8JLw0JQt|`jYd0YyuuC(wiDwRSQ!jRK7Tp$I1=y zXSi5s!daG;KJvDQ4-dxKQ3ouBTIx=un0UUlG(37!NB9XX<;EI~@J3yh)VFA1m3qM ziBuSI6F1Ckk`AS?phCC1`RSefjZ zjfF%$tq<1P7D6uc#sT)6b38+bZPEOVNi;#{)rsmTY4D&HFRNAheIclceA5Og_kA>~ zF8D2bx;8Q2gIfB1kDiI^MK|!E?McMInPi`u{6vH6-J4|)>x_nEs&afWrPBNLY|2zf zMOcNC1U-tU*2g$1wOlyGjln>1zEa{kzCvk1`Y@)9Z*&FGjxEThzTm=!heloCAVoNo z@)|2=UIxsNG7=kxNlBzWySn7wbQDzsb*+KY^K%%VDgIW>OTVhd#%4PMk3mf3)GFsF$2`ZoWrmQAThSaGoZP6ERM#d( z_s$2ouH7BTP`Az1<_Dd(`}9M#-n$&ZV&(BC&pxecaY~}zp>86ZYglr9NH)2pb4RSx zU}T%D`Xe*znJ&|N90lX@m!FUvKfSz%H|HXuZXAcsC0~Mmg!g+PIT4{tjlIMt5c^^# z^-0`7_a6+Cjge;xKN9JM{xHs9y^_XGqBDW$f<^_MY8l!P=(V&B1~85c#3Tx-KLAoK zBXFq61>i@m9dbF>;{+Mv!aOA!yA8WVHOH?N8eSrDZ#5DM5@Oml9ep^pQwPUl#iNadMocD z++62>eY=rNc%fUBJ88mFiI%8)zW(`raSVE4^J&lO>VJ*OI8X>e_khf7%StEb4;1Ny zdpqB_tCHEg`t%~HkOAq9WUaacLzz}?mJ9~mp+J@UZQ)0=D6o+Ea_eYqCDvt-V|ew! zNA6of@C(Nm!dx%wvQ0QikDQ|NRCgh>Co@;)Q_`ZP8;ys(K1j0(_FkS@de9jHFm&3soz zAg%@xI$!AIoMWtH-WsL&+L3$~9ZpmJo_s>&)R_=f8iiDQhIivVF)p>4#;%)1W7?t6+~O?q^Z;V36E;>#@(Y@;V8S))0Q3G)x!D<7p- z>5BXEYrcd*{l&IxMaM{~Xw%0qa$^3q8z@L+GU4!{>s@Ejd7(*nJz0xr*3i^-jU@Y) zXk*2x`>8!AZU6Z%s##t2VqJ8;^Igq7NYl{bi?Y@25k!8#{v=|ji>T&X^9ld5bZF&q zufdks>S3_+Sj^*=AGm~zUkYj_=R>a#Tw4foqeD?L29lm4Vc$%enuh$mw+|hu%P{m? znkQ_@4{Q4=;Uh?I1LOE<5a!uAdbBRN5DPE0%%lZ*5r2C6O9wjCVn{@2;lrv1SMS3U zA@t(6%|i((lUJEZld(=80I_LTaw(!F-SCeXm!mY7d_S5`TbNy?6K z89l0vnM`Ic=8`yp=X&$KAX6IOoU2aJSV4AYbB?cLQ&jL8G19LFnbgQ;cOvcUzq#S$ z|7}~7QoA+Ghxq@E6r*1nV%J26G;UD+8c}&Dn-O1kmsB9euTb_ zu1fjwGskSz>DAFBT=YtPNKK?oOfvEK0})&x}?*@eFrS=T!RA z9Ika%ITxaIqum?905}!4Wgq-7}wL}r{q-5-I6q0j=RTYICXF6 z60_~oAaLu+g>XZLoAs2DG58X#!1MM|8(hIm#H~f zK$YzZb<_27WCaXTPo5w?mT`Lg<=mQJ6FX-5-Hb};r(b02jLfP>?(QR|ZBO~Mxp0{c z?}rpnLS&n<)bbaU@(FTX3}h)`S&U*|M7r3>YNKzSRh~YKN+82q9nTDut}9f@IHD4L zkxyh@o zlq_AO*EXVs(Jto~=$^T3L_r?7xcC;Pt_reAWtVyhe9k^K-%HGq45UA$-+wo2{}PV$ zw5WgM^_EapwhhZeQrd{nlcd^H8jHe znTN88LGOH6uXM{T%d%qCx+K>3Y$5j$H3Vy*4_#On`jR_D|O&wGI1@cJ2LZehe_Vmo_2s4`V zoj)Urd9NIf1QI8ff$m!Pm+m+bS0%PdWae##qc!@aynUSiL(TMzKa}VWJxJpcGNGEi zM#kG9P2d(0on}?UoNd-H<6?0h0C4wrGsFFDfnRWcNcj?Nna_u1Yd(hur)hlV6yV}iU z!a-QhS#xk2aKsXaQ**8_L4d+- zjx(rUzuncovi)^VoamGDWgIbM-g81eBHe1qw@?#V3~_~%U0b(RFkm&En;;Y9O17?= ze8r}yk*=6+pW(xO($pC^v(4_EdH+3x>UAwrh@MqNp{E8amt8{3bu_QXm9C|Leob?) zTT4VJdZvBqR_TPp9Gew6mjt8CnUonk{4E3EX%>FyQfETtzj z9?z3|4X;_pEW>LV2x&BTEa{n-zkXv9f+282yU6%#om}=of+Xo7gvU=IL1`Gd7un}T zhtf_#ygv5Rs<;0rbNH}LT64-f{KD_G8Zb{kmf@M1V0VEbWwu_>oA&}&%W}lcI7KCu z@X^CvN-CfEE?~mULO6miY$%SoFA!3F78N`L`nDF`mQs9bx4lO3(G_E3qqYIPvbH9* z#l3srPf%HXqiR1XscWh!I#O}z2DdN3uHh7zDkn5)^~lw#*k!{&*HkctC1#p$ysFR5 zf**oE*-elaV~n0-lA+s0B#iR>BSs>vVKg87Y1*&e}BExOQo! z2EDUfUk_7q`hd2*U&(|IId`_UmUbssH$LccU+?T(Fp0J5F`PHyx$&PqZ+#tzUe507 z3K&hBnhSZa;IC`DyrD zHv}T*?66Z@jc(y9wIIk(?WevKGqd98-z%->Zh6t#^;-{_vy-q8bYB)gc#Op@~6`{AQzk%uM_) ze|t{gD4B?{^=Q>giNq`_^z!+S=(&YAIVglE(JWL6*xbm*@OR7Y+cSRsiMRX6oc-D9 zj6J5jmNEO|k=z?%zPDe^`(g1WllT8R|HWc!K~O)_{wW_D9w9!)kL z?~&hJ^z)2I{(t`L;=x^|sj**yFLwHrT-OoxD76UrlC~Q@4oA? z_{)vkBuVXF2`|fKp2G`gR&?q5iX>!(gM_M~&;O07hB21#xJ+(a7rF+cHwTu-tgS}+ zFJ6n|VDMPt0vK@91b zCY;7BZwX#6#Bp5)@<6McavWFW8-Lzd&Dw4rWmlHsIzYGBj!&j$o?tLPgvFcG|DmCDb>G_6-D}J(M$`XUk|%RFK5JVr$7MORG9xxyZKrr#LhO=!p%)9hm^;(dO@A0=%8l$!(5UAN$|?Uu}}z@cPc~HBP)Yd{V!5cr0S?<8^s&?Cz~r{i^zjz0W+6a%B}C zuULRCkmVfrdinMP%4`wu^|D>Cbg(1$$y0m$ylVN3roKah%}l}44bML|72BZk-OJW98@&;P8vaMn$`y4>{6Bn z)8nC3y2}FM(JtarxFT!u(wL^ep4W}p4ew|zcx;ONyzjutVGNYadLX{gmBlsLKLgK` zDeG%^!ui`+siED@{T&@?n0XFu1%UE)<=Bx8u8%BkZCw~A$)nd2^=aYvw`-wAwt;pT zrYe|UZdne?VMsyvMZT(Zs!wb@&hDF~uJkr0=w0A^9xCS5Di}<#x4M|y-LYI%^f*)w zI)M)<`DLmbkRN@bHY(_dZ0RnNH>-vJh8P#yEqi98%p5jj=w0B-5229kfAPCyd^eJn zvM`c_(qS$JpG2>g677;q01VzfcfL8oC0Dh%bX|JcVGE%7%(B=%vw&Xn=;aw0XP+8Yq#ezu{vhs_AW0C|z9JO7(QxT5WW_Rr9utoV~w83G;-x zh8M2rx@zlyA;BtVd$6KRRa);C9mO!jgM=bIdf$+pe~ftPTx0e(%gAMq2jycH^&y9o z**guITt6Nn#hUL-(vN5^I~a&FxFXg5%CI%*o-c6jop;_;pkhK!rq;nhQU%e92`}*s zVm6l?KG1g_ZE}l(IqZ~^p~K1!q%)|b0cO7@-3deEL5zopUJIgl7ric7V^9sMx_$Za zib1=366|o0QmToU6b-UY-H)&B@AxoZ4w^{jPO+xskBananuI$_j@T)BD|94VPO)gd zv=Q^Gb|-fUToG;yfUxMH+w_+)Yi_S4`i!|*%+J-CeAILlKewVqntC!uNJR42?G9^Mye6t9i_X{R71WxH~G&hn-AoPC{pkr)mf zZ)gL)?aVvvxUm+z+)Tuzfy~<1RRYaY`1mkW^I0FLs+iL{Wu}Jd;XQZOOiD~X|5;Au za3OX-*2iHaqK3iT_lT7(Wo~~ny=Gz>bJ{P%?s;&bY;F+RaUF(sZpDqBbi36p#$ETa z`F^!|6`Oh4om)RQD^liMb7dn_atr z+xCaMrlTE+V#bcPdYY{R6|J3uxDqcFeIH!czwGN1^c12FC!>UdedI<#KimICDfkGf zzi6Fgm)+emx%~3Ui2f%Z^=hRHA2D?vO|y*P{}BHIh1G3B_}LCb7L_aTV2X{8+LCtk7a>nUFLrN0WSXK~YC6&M#z*+w4nX zRcgyMxs|Ax&SPlFLrcoOb;N?h2{4Lc!$Q^X=W8ZaLvmkalVfZ+IS+n{{(k%#M*eJ* zEr)`+)X2TB_dB(}&)(o>5&Q|}J((Nbh`D$ZeNdvgpn?v7Y{MIB8;`E67({bjk8&pL zFiPCH(GMfHcW~JjfM1hwC@4Bk?VZXpl6xN~zXBO`4F3F@^S@6=98>*lsO4Wyh ze7ld(Ldd2iQg$_~K#hh!|F)Zqd_1(|rCWA?Of8XbyRs@p!uHRjz1S@wjd@}aoslCy zVJE584os>jWEalKo$ppJ;{NBB6s<^j9?c8S0TB+^T?GAUxB zqOcUNaCt!r0#j_gdMw;8WzW5x~7lZ$vu^EcrwtRV->4x2MGUN(~_6?IYA5{_m z-Bs8ec)A@ZhAai4A>xVe`+qT0252h!W-OI41dg*4ED7!A?m`ZGKr+e#|_i zF;!Faj{dS{R2@=HrotV9Q8XB+1!j<^kIg-(KD!v2k~QR)c@CqOs-&|Md-KxtF(HyQ zWcTCqHo8eX==l<{^D@u-qD1UZ7^A1Oc-jk|zjbcEA%RQMzmiF3{-_*`W_(Pt;q>&l z^^v7+C1uH%S5kR}Cjz|hLm_QJdQYsTDky#iqey?*&6kmQ$tQ|8@r%1enlrF2%D>Af z#4al+PKI$$(*4-rI;K}=b*6}72iRriTRc?5YNiD*8EMjvZOq#%cn{U2ur6uC|Cs%4 zVDpV+7&}Ct97kwpzFr%0f274@_GBn`+sT^&UN(N$@9wQ9pUB(EO(85?6Ie*jh)*kVt1;DlnR5O8uVRmmGOf2P z5gkt0*KKiC^H-Afov`@!_$LCLIq-YtZ@St9B(R@ZZ;dHmI;hxsclv*Nz)u$kS$5Ex zlALN?xTIv=l+85J-fohcF$5ehfdcHwNIP_fR~w7L<>(_cW?Igj5~^;yFzxZTdg(D& z0bXTIjhsJN3rI~@Boh{2xIiiXxo8!wN3f(1 zoH0MKorCIVtABTiAckEbd{dRalQ^b~@t2yRZYq(^?)Y5g;ed~J{9v5+x#QbYYxpM5 zI$`?d4cdbR|FE)p55 z^U-Z_h?PNEe5S15GLr3~thtu!-6}fqCo9&pEE~!736D;LbKvkqw0jDy+(?oQrF!mI zboaulM1cP!=|&~1xg^e3)0wnJ{eei_O(!XK{xqJ8tToG65#@=G`9~TPo-cRKi*(X> zCt2?MX#eL5@8iMy^Rzteu86;xq~FNB?2@qIthQlVy~*IPZY-hfTdS+Ix-;C7$o>D~ zaM%N4-vejxzgt0LRE3x>&Ij81bI0O6GC8Ar1}+jabslcMW1M?*7?7!1+Y!rEai&~2 z{H{B^vt+gsmn{5AcbQMTy6D^vhR9dw6g#C z_H~SxczeLk6#F_x5_47Y3moI7b~iyaA?p+!jel#U%Tv>KKkui$p>;eQ1~M_3tGh|v zRoQO#_dGgEXMRHemwYjkX`fl!ZqE{bh{atT7m0VAVjMBJkjJJAB^|rDhcp;4PHt0~ zNAdYIc|s|iOuTe>9(UidW<{Y+=F75)&2#I?rvv?q)z91vYXjyz-o`3 zdN6^aWl+qWR3m0L+n790!1LbS*#5(O{G-9-Qlw>BIFK+EjHa}-`j4 zHYJB?qFh$*SGre}jR@1Aoh6>7eTd&``<{c({7U=7O9e%49}Kl0q_}2pE6>&n}*}+qVX4d7?e4WAsM)-t;ueZ&4kXOX^cOoinM& zWPVEXx>3rBX)cKUS75*iYflKvU&Cg;&3ci-Q`2Jk3Ki3n2s}(LG`-$^%lLa8fwr}l zjgorQR^NG3c_QO`{hO}E*ce5RsogfKxGjfodD%n3W9uLhV=7Z9%bJt5zk^qNZ9)E{ zZaT`8VLX&LAar8s-3)v&!*K^=*=Na&>|-LCS+?`J%;#(i#s2#z7w+V%-D2>4nkYm2 z7g^+VcNaZt1*^U0&u>8g+nCmVw*I&)zH-J&>BCrdmo@sx*IhlYaeR%rsz&Fxg|LAiio)2gf+)-n4K*bR6SI!Sir2t>%cVeB1o>Ds^UHkpSYd^zN|Ge8-uYZ=r zx#mNiDYo-dzxF7Y6&Y^8A3Cx6(KgIzDZn+>0|Ufh@2UoHspd-tKif8~gmbC-H0-7h zwuzcY2Ms4PBe!v0ZO=p}zUnAaAU4K3-CapND@#@V{=BR*>``N{{z$5qkc;9h)Bf@w-3f zRWU8<->spcxz1MP`Sr5Y0|@+(8qH#!;4EaLZP}PDwG3+iw}5%@5!;d0nu$vXJYd$-M(? z-lGVF=Ucj-5h={X1~yl0otG@l7r>$s#XJh5cXteQbyikHIrT;ETfl?sOg$}!_lm!q zaKjFNDu2E(rTE9Aqwt6zUXg05Io7!LlOq@T|Gs@(K1vxaj}KXX3+b?wd}(&*l$TM#FxHIM{4%gjK^;!mn`J8Elr1 zAp?SsOLDr+04UN6*KAB~ED_3QpfY99-|++eLew$wFVl!8*kmn1ihZu2)*oz2nZ4!Z zYr>*N%3RtYUS#6AZEE;Fb0=-Hy@SGXWdoE!%=@#Tbn)34nFJ*QC8A8Jhha9O84!~; z_+9~UDknJ%c{gQef!Zah9hiawa>+CTl{A^s@TT1BJ_|I!5RhD)l5}|mf7tH6!FgZ! z@kTWpd4PUbybC`r#krW?{me7vK4Db)*dSYt;6BIFnZ2^u*%|#r#NfsV>9*ULh@ft& zRX{>n@l2cEI3n$Z7fBTe<0x z3aV7%8b0iP7y;A{iY!1qwaBlSIl4!0;_WRS1W}WE{ZPY929SJ!7L+KJc61SHZS8=Q zJtu>`6cR?Nm&&}gzdbB;V~yy+7rK>PWhIyEOE>VNl+$PK`RkYO7c)=vXy3^g@vVVu zw#nMiOs~PAqT#!(4?v!CRx-FX1{`FiCCj^>0kqP9nMcf|~N{YP^Yw0`3)Y(G6r~5JOF+hNDbiuv~{_a0;l|5p8XqZeSxlmvS zltix5U=dpnSWerou8Q0_Gr^g}HG;Pk!oSeh~E6 zCgspX*CVZDTDqtoD0@Pe{FZ!H(pHKQftu@K$r^@8;$w0MEYb8*68iQ2H%(W2$eJ^s zE^i$mG8W(~Kn{X)BLk2QPvwV$@LL0c3Yl_R9@f(nEkCtWc_TYrer;6$J5AAQEKOQy0h;(hyIPTHWl1sDh8oNF~O8ViT4(yz%cE8q-z zBvR*de~%seZ;#%%mkbi6-I~B1jiKyvLGeSm6Df1Quj66WFJo@w`QDJXeJd z$@Sq3ylCX~|MAO=5gleIGZ~@j^K(@=f!k6+IU*jKZdySe7eSnulT-a!DuSV z3?6WdsU0N3E3+1$Vz;5Q>S0RQ2FVi+?1zZ-u15+K3T5>$XuDY^n47WR73=@aYG--g*&)(wBMmO5qLr`Sg=`x{he|^W ztkBm_4_(=*Vp5NO`J|}jbFX~OpEpLe=RtAgvY3Z5OJ+?=@1Ix$4Yl3&Y}FI}$(74p zEuE!q9^B|L@weTy^5KGV4tNLI>@U$S5pPT5@NSMzs#8Dx16ub_Vvi^ zWp_EzL}DuiAKz^ohXn_`Hu(mj;`+OhPJ4Xa65MY1=JHiP?2JdzFDAjfB6-hB;&vDG zm=B22M1zDH5)5sgtL}h@bNJA}q@BF}oO=-$OXqh4A!P;s=bG}nA1XFGB>kZP$M_jo zo3ivr_<rQoB`Ci~9xB$!fNz8>gzZ=oEEe^l&$aU=TPbk@!Zxm#KB0Du}B?S*a zh$7A6yIdEYqG~WSGkoP9o4Wb1D>vG<*XA20!}f*slYSXuq4b96Ws+Q`jMPpE4w~16 zVu3f58#?~_cuEy2q{h}KPknsl(gz929;I%)BruFf|G8f9=OGX>S#_7YidO9HwoDSf z2PAz=Gw?u~dWZWvm@oerr9|&D=Onq#!=P#cOj;ZpiXO3a-fIjz*?W<4-$XJE8K;#u z%9LJ#TCB-glbdCI3nne`W>kFu2vWGRxY&&nRj;mse~iNfoF%aVQB4v|{q1TiCd+_V zb4z{(`@WUZTm#b>-6iSe@Au&!MO!l$S1vt|R$d|8;gEQGtx15_czxqtZEK&O>Ds8@ zK~*O65;f{8;rjy*I!ecR*+;+NnBzCv96*o~e9~!M?+jj7>(GVd%W5Y-ptMDM=zjU; zN5AW%9T@0Hpbpel}f+8X+AYDX7K|rNT8({<$1sg@GfE4K%2`zwyqSSE|K?p%*5T%78y-G<` zAc#tDh6qu5qy<9x&x>_t?sxCpxp%(r|JUzYGi$Lhnzua9dCu8q?|qKsF}#MA>G54G zqjQ_(%OhcA+Dihu%dVNL12;V-78{r6**^)#Qu8%R+nZK7eqyFYRqyS$o+M9FPkFpu z!06*wIu;<@b#ifmR@OR%CqJi%+vo@Cvb=gZHJ>@aF<24id9n2=Rs0B03Nm48a<#W( z?E-X7rVAt~C#F3g)Ddnmjw!n6HGgacx+_830Um{ZRf~1fW9&;7sf;i!v%A7(c|Kaj z22@%TAv_%|GAF)ER{{OpZmBEbZmv|!L5eu4QdlO~)2qiQnhZ1lZJRgf7vxz)ogp;@ z|00dSlq=?JDbS1ohnudDlo2;NW&)vkScM3b=9J4P;kZ5!I{0M+;(l`e#gBpLFP3-wfP)l&e4WBwDt9R(kfm#_q zxA0q>e{ufhRpjPY~OIDlTwZJdlmDO9?l?7_OU`U6-;@z=$vbllHQ8Y5D z8*mUb9bBB3<7CbFArU%PVfg0K#=KA$mCcbw#2kA`nZ?e8o5;F&(hGH(n&C*a0PUV< zd7*fv@iC+Y;rdiKH~>)T5}LHumbGrN z;VplteLesRsWuA`fjIf38b$#uqyS)VNhScb$ckbrfi|i6nhG&5*tW;U;lxb)<;?j2 z4@+fn^D%?V_vl`-Kl5r2DfIOZ2%1lM%oO2kyUX!N^GczmiABon83GRpHC}EQJ-p-2 zuM;c1F}ZG5sa-77=06XaJlal`Mf}Pa=36&QDeZ4$QGjTiZ#PvOQdrCc$B7RGT~++ za_4=T3ewWvHb4L`yQH-nne;#lZhh(f_UVlzQKX|(1#OU658->yTR6uy93h8k9Y3q{ z+84S;{50v!2{UvPC)GIs`q48p?Q)gGFc{`Hcz2>kyOK+;%$b@r-05XyV)0NRl2=yj z&7$iYbFTJzR*mG>!67Izl`;Gp;2*OuorGa8(t>Ll0NhLqOgs@BBANl$kQBmD*biyi+g_JzXCErUGlh8OSxwb3i>cLqTB$SfoD1Zz|vLh zyCv9=Tr>XEt*0gnUe!|NQ3q?;0t&J4hUEZkXqS4LLRa1zYF<%qlW5wz3C}4x#h?$2 zoioQoHmzMn-5zbSuLSxASFUJFRInZeKyz8KwIke`1$cgDx$Y^6^t^o8qkN+Zn1q>2 z0zqWg^l|Oxo=Xn{vp#G?XKYK^zUMTybZg1&_v#7f9@eEUub$S{WkVV+k&=1Rz;a+z zwTV|I32TduMAB+lC){ra?LtzFqF>dOJ{=ODYRs|Y7pFqThTf}l%Qc9nc?K!2ld;SS z_ehR_F}O0yu-wQnd426ttqLhFUvxW!e#?!;Gci9K1X%H1RMvn-2TtA(^5K9eeWP83 zC{D7kA})RW+)h6;uY;c5$u(ipq|Hd{uf)>e%odFw%vJY;&Wo}*Q!ah(2bkg}_P#$@BUn)6@x7*!E&D}1bs$9VC+9i%-IL~0O( z8aTt|O5e~X>F6UB2?p`M4EvcW6odWEVE+sT{Lct_FBM)CKr7fsX=-^aoe!Zni|2;$u$BG=&P%Cxk0Lsi`s3=dCaPIc@fh5%19kJhF(vP7W=Uuul`L_+v zB1+wh;yw2$SczXO5gn`9%#R@XoxAQ0(cj+Gt%U!(yONJOG3ZQdI5Glk@+wnw9c2}~EIb_o=0QvzTRpb{!D zB%~mU%eG%PNw z^2-z0RGo77}O<`70dN5mEg}^ASq$HYOV}aiBG1$9q75ytw*oFe4Wi_3B7#jZ z9}KN}NRLGDz5>7oj+nm!H$)+XY($rK+*=0r9PYJ&=1 zXwnG6!TcLuh*+Ryp%`U)&#Ga@K`PUz#)=lP9J<|cC*XOQEY!NX5He24&MpWaSOAq=>~%jH%2e1V3O_e#J|ad3AInx9VBhi*7LR zPfWFglNmQY3lAaaQ8KQG`NfZp0gF@l9yty8*ZCic;@WYOhH1q=PHx{ELXnZUTV{i_Pdk9^mOt@Xzn5D2u?Lfsu_P=9RKuUYr`*l1ABON9xksmk5U zvY&y~XopR67e~$gaBLD}$f}6>W8<|o^D3+Pv4=B5^w*D%Fy}cNaJ-Y>ZW^M4@B3FqFIK0FeQeePe;E{IP1bRbN z$q9bE@h^TLMzfBEO(6rFXsk>~W5vqv@=7iiLp{|F#~fMxf*irkJ%CCG55XD2R^0ii zOnc6jev9X&%AlraVb^htP#-k*R?*>_1Qv0JRf6u1|2PXZrm{Q1reCG;GitJVvEIdR zMV@ojX>fUd6HG0IDiqyYh{EFEwlI_{|lNuxk2B&y(7gh3H9wMdw$7GYL zxb*W`gp#qkZG`GLy-+jNO`krmOfAl5I@ScaM^fW8=XxkUN|!Y(AU#7dmsR2=i0tE{ zF`MZoi%@q*hXtr3ClwkHF2g#=eCSlBEEtOJ4ovZCr&gqgnzzogPB_mY0n!-C8szm9 z7;~A6T@qc_oVdBCh>&w(efb*t=t_veY+MNIlKSKR){+J2RJTxg*IwnW7 z2XYzhOarLbjWLEaO@|-BxW!@Qv|@Ig4sUt~_C%qnuW7qmp^{f?W`ou~w(+4NET$@$ zH?RT0sK-FtD2iBlxRZp9Y?w^U$O?bF8FoytCr!qb*}zq`9MGsjJ&)Ga_g!#<9iq2 zGMod7IdqP_+^>#%=e|=vp^DU`>Ar2qB-nCNamd8!v{ohQul~A|1+S4c%a3dL-M@H$ ziO8#!kbR{}y&jg?eM7WM8q{)d2r1fP_ia-0F6 zzf0gp3PMQzNJ(v4t>}dxx=}?-;-BFK7F7P+30|e-xW3b^ivRqh zpP>cigjXdOUZF`*+Nn9HSOYjYkZq*}j{;o+&j7Fh{}|YeRp7qD!3CzNQ`@x*KRt@* z9WF<*M7_WCxzIWQF}5rANG81`0T^&biQ18Lx?S<0xS9hMQ+(@)QnKAQ-N!%jcKFE1 z-)i{XXV2tA%GNgvaEw)FLQ6alw<-btX^l40&QKd|33rvRbhuMpcUZZRXS<4(ZP(9= zSL6SKpZLhY>ZW0u+3rZU-hp;C8u{tqjb<+MmIIPZ z_dat0m?RRbNj!Eu+tH*SYj`c-R8+oNFyKvbPS0=jF$r(5KGmrZ%T zQu=CH&(VVFeM+KNtO>)|)zOAF)Soiz3SbuNAMkVvniQbDI|sGZ(591g$Mw23`cqvb znU$snu$(S`T9~JQ1fK6D57TW!z>+^!O(UuMV6Dt6;~#s~=y%)!;`=bRi>1Zny}jyj^r@5 zI!Yi9C1-llVkvHnBXJ{p2~tt=m?`|N2;!Zk(np^6w{71)c_HDhaOK{r&@TIW!5JNp z(p{J>if>ysg;W~0R7o}Y-w;p#!WVtHW4KLd&^3IiqnDnHJA#AhnCD9?Rg_7JC-n_= z_E9wpni-`k5;}CDzer(cTT7{cNV-Ew5iyB zyH$~6x3n|$KMVQn2dRyTTEQn0H4kRp5YZwm4o>Xd6hGf{SxF^T9rBx!y%}ezt6$AP zrn?rJ|FVemjX@Dqy&j-KfgP+wn-}AwL)TQ|j+J6V_ zzbG}oHdem$JHECdzCtwLJ89hCJ8A!=|nG5ib`h;J;^y_l| zUaocWAhOf7ijCIrVv|4U7_~pFJCByUMrxw^Wrn()F`<+>~r9x8H92)6LK`{`(ip z7pE6_*>1pi#+1gLhf4x~_4My{89&n8%pD#yy;>BzKCT3Wva8*75ox^w3>c_{`R6@y zYR5 z3A*Ip{IQA8Ssk5Gl%Ji@;#+RUp-S&AXRVQptV8WvsOs}y?aTw(m-oqKI;Iey-qFTHv@ z;SG>1xO;&xNX=~2TCB{MDs^YxM5BwvdBc2BA zNdU6y>=>dZYl+lLq@Vn0kOpeaAVeR$KwB$aw?1D-N~x7G+5E1ZFUEajSWJsyio|AD@=UE$eUdR^|ap3m69iZo#0qyw09I%Ci{|2^DmDfZ0N^=mc(Mv`_ z7%W<3;XV!GY_7OB=8X0h@7%dp2@9JtUvi2kg|-6~S03?I?HX7$iy@3RAjEKjL(nr& zU>#{lY~F(0itR?CdUIiy-UXscE=R;P7y5JsnUg#hlS=9E-VW@pVITx){Vjwb&ubm` zkQ-r>PWzyVic^8%Bjq&&dHoOy5c4@J!R6ysI_7QzwK?eBk}lznHP`LCS`no{!_2vE zB|O+UB34MjwX>9i0-&;JFoVxLi0^;l!j)h1krBdd)fHCB*4AzeqNoR{#kygG6_E

a~}4_4dBC1#a_5HaYmX#C?2 zMZ|<15j#(Kmm5kf!V<5k`+AbmN~oYyOMA@KQVymVm3juIoZyjMxCdAxt%k(HybkB$ zOd5fYSaeI1j<}$VvURjlYMEW*z$vuqsC50N+eY?}=teLD{>GjLf zn_16dvTjy@sB8l=Qn6sA0&aWc`0kRiN{cEs`LKl^Ize8H%7sdxe;|Nh;#L=n0a0N5 zu~+Rp+BgvpS_(J833M2b^p3k_lRHrrj1}V#H;-1{>zaw94ubTD50Qnt!c3jun>zb< zk*w+xkR!Mbj2TPI2v0st-vCNzm9ilr|3g(@>ZMPoDiE}njt9B13D$oWBe+%tK8dgGlq(x6Sa8A1ssLM-O6CZk*t|kMNE8 zgV}`y7hq;np05Nw`-gsk5%%yy{NV0!z7>o;v>jfYCFoDuMNFaZ9th=}9}`y^xZt2N z4MyslHN$y1RY9ARC&g|x(b2OYmzLLV?<76sD+zIs3`cC3@w9ZqC705>`$yuz6>tAz zzR`XuBW0L(PX>M3I(oMD_+=@34A@c(xC2$0iXzeTig_ReSc4x>w!iI+A5b6VUmy%m z!p#gpJ0{)ovAVmX@*5?y{^7IxH<+h`y3~8%ta!v;#@TJBv77;$Jc0YzLqt-*ad3QH zYB?1KPPYE&3-aeGkmd*&=u&>HnuWzH?i7^BFKi}dX2rFGy zb(E2w-yM>goVBk|0jzGyo$2Wd|@84+*04LQTSWN`b_%x{qc?o$Ag)5uAI(ag9Z zXgyoxV_%Gwz$n_~7t)yrgPLX$i$~-9{&7L?clXHP@@F^K2nKWl`9vc9$+CtULTY}P zIYoULN+;q97};_gm9VEuB}s6gASZkboNQ(NgqKGORvhu-H>@uOX`Gzd2M{E+56?`L zDxQ_Bx+O3Qb}##tCthfTPp|H#z!y8qOUhUJkZ=ogjE5wG8l!A%eu=aUb|zgU*d_YM z>%8v&F;N)W{ZVC89TWejvW_QS59KNwcOtSYzAG@WXB}86-`r_r7SYrNam9%eY=;vB zmwY#+;eMw277zy|XK^a#84=~))OVzblq0ekoJNFR6QVt$pd0KiCeE1f+eIWk3GWwQ z=U5-ULxwlIo|%#ME=7}mLrajypoCJf#z$s%*8Y;29~1pSImluF1GqYi=0PWNO-<4v zz$#a1WE8?j=pBT-2Cm?8f~mBud4;@l#LN9k8C~9)&UDKkNCtgDJEjrsQbC;*46Ykf zTO3z&>-f+M-mKic=$V66LLDfIyN)ugI{fH}Z?85xVlR_JbsRz7NN=D3Os*xvDADVS ztsUGtDf}Vj+^sYM_lB_~7XBC%qL9W2_09(eiI4b$v{prs=f#@+h6}1d{r`~v9ONof zS1S-B<3hy1tUwNTaq>q#&;MpV%ig3(H}&Y^G|=T{<|TRJP@)n25{0zRx0ddXZMiSr zR-tM`o$_c$DX=?=j4#5(<<*E07Nr4E)m^f&8ocZkXd5W6c9lo`S(bY7%u}apa*)%( zn5}Aw_E64uc5(|twrb*i)uj@TU#NrH$?X>SkcLU7WnS1^KaSQ)HE#3Gb}m?@|IVOL zHi11A3mN0k%jjUYifERQy*0jX{6a(~RV@tC_DL1%qSsL)e3E&|dh6;FZf+hG3+|^q ziIyBY>W101eezvH19iW_A%y7u0U^43{k=r@V0#hdpk7-a-U+tOL5L-x+uIb1-IOJ# ztJ~Q?`zCZV;v!K6LNP(@W_ZL<4ar|DK90J|-f`E;XMER#nES-gUHzQAuMJm6Fojn* z<+VL9mdh7z9|~qvuIcQpxZMilB5jSgD8~sj*hxh+YbuPao;3HHu?{=S%CI43Z%i!2 zfV*4G($I9o2&Ddkc!)QuYTklxqPIdJ-$!3|pJxyQ=HCisHqC)5pC6G?8OEX#K)7g% z(ex~4q~W4WuXnnQ{v?8Rvzy1JU9j^@I6J-a^|YL5MKHDCB%d9xYz*BO(id~Ig z+V00ukbic4C}eBw*QCP>63mw*US35Dl`3l$7t){TRqUy0Y-b^=ExyxI3sEzJ@zuQG zA%cF{&x!Zcp8+BH?LPfYll{%gnqWS!Emit@aQ^;1bGZ@4=^5~GRdwDQkvOE<<`t(j z0=^>B>(Uc$!onQgs4T@Fx&Eo*?D#k+0k9m}sXQR>?EYNN|4oTUO86M0jO88DPqLb1 za1N%^m!qD+w6KumgH|K(l+sF8@S;8C&~9gQha7Y2o9Wd4RYG|}M7g)tT6^(h{0_-mTtc`Ap$t(68-x?utmvu=gaSo8B0Ldl$35Y~r&}riNL9bb0E! zvWet3 zL&K~q43qpI*C8(5f9}TVGm1~j)~x;b2#D;(`-b2c{=azd3y9YOIQ<7omC?6GG&ewU z;R^2RJ0eWnm6JIlsMny3sSCfEFZaGJ$Qhi8TRvNSwuI@8AtSWK-TJ|mZ49#gY7+M z_YJ6#9EB5Z<>}MU%6`85n#Gpih?qaA+C+1vHYX%BfUY@5QBB^E5Y4%ioBf?TAwt*) zFulr{y;i+;h$x;6)xshE!o|?UdkeXE-*hb%eO7a*QD_ZW@n~APPw@-z>093qDsjyf zc#hQM(5p`GrmSHc#T&2~-B(r(u7|%OA8SzTj}dt@LxpJtWJs=kD>|!TsFJZvp77`} zMsHN48McYL!vt~$HV&C1!gB$S&Kt5G7e;aljyKKgm)>1^E+L%LUERHgNA$xHY72CN-F%;v|7sBL zp6h+*UX!)eJBY`IMN7c_bPRY2@lspC5~&h_T^7BPqV(4w=~Lf~alX^&@R=4SYljJV zc#4Q&{Df}=NX?Iee@i&$OnUejjNXtzhBeVJt=WP*^t757J9GE8cZ7^V2CFc#^^z5o zyzC%3J=9byKJP{H;1w~80Au}77MSRH}yg2+KxBk-CCt!D?sWD+Npn; zn>n`9ZXG2CAm>Wt7)~W4=R1NnXg+}84T5mQFG_h~j<1If_HywamD_F}eL1L@sw!`o zzmM9?Nd^Zqx6OMke+tu)u9qQ$PJYn6`%UdyPv3)n_X4>>7L}-~p)CgP<$5<2JUS7O zgEl;C!#wC6SK$|&zHF^?&#yE!0~7iNm$84eg<(?}^W zZ(Ot&q=(o0ZTsl)l^c%;0B!5Rp-~#q<)zJJHKA?5APn)-SQ<8ylhaT9sD?y?P4QQg z+%7nmV6X_S4b<;d$P~5BVrg$w!HB!{9xIsMfX#XZtghEUQC?Ntd113ao z0tXV%k(V3DJ32-7kEEn#@a_OKTkuSC`c5Q{x4!fxDN-H72EhnbP#>lo=iyxtObZIS z2sIbKAa9ayZ#zAEWo)A-$$Mn)(n=hpo+BS`@ytgJH8j|&i70!Z$K8=*UFq|&?sM>O zDQqS`qEw(mCSPhI*JM?#Q>q@W%i2(wPnjLWM~a)M0h+P7D#S%*XX6$p)h`YoYDQ4R zBjWj&Q;#1mc2AeJ*!>_98r%OdvdzH|qSZa%&9mgJsjUKjpnS%``Q5wqYXHgXS%*~A z6fJn9b7#RrYYhx2aH`v#*J?=ypQg2dPn|a|-Wn;<(x5;Iqi+x9&%gh`e!K5@L)NfL z+1uk&SkE*Y;<{yshnli1Zet|1hFfdE_x_D_cqkXPs!|IlRofyH%IpJ#pjM{cSf*f9 zzVqdrQ7XTYnkZ{<5urHVcqw-xk^1L4p?cK+n~TRuO@*K}Z2B_qUToeS0f(b3I794n z%zn0`3pW!UG>ViSBsfA{p-hqeV0bw!>Wy`3=~BlGrU}a7f_+~cU=B=}AF@@+d+CSR zj-(7waXykb57^UT4Vx0|?q4see|Wi-g~-|o202yWpuuM@4kzbOb}izx(mtG9{^7Z} zTP-REd7@+l!cmiQ2!Nva5^c7TDn0aJcMo^t4B$(ONQCiH{*vw*pk+uLr@vQ1zRCN)u@ zJ4H#x^O7OKUIUFX7t9&yx;+{dlB-YYs<=l+pXob}(Lvjn{O^`fP^+0Ht zYtxxyJEa?xQ6@>@64+3JsybOGVX5>^G1qd@2h5>aQZ7r^xO#a*KP7+4jj5|3v( zAGC<#yMS1kD!}{VYf--`XzWf|q~ux*&RHI9EFi2|%$CxA+bN1GHBEc0Gxzj5t6R$4~8B?jl-`qK(FS2pz z$KHSB1#Vn~9S%*G^%@LUB7GGZx#t}MHT2#R017>=#3#`+2=bQk0D;uCMc~=u`N*DG zo;vcpO|1(hFvdev|CQB^`|dw}<^uS*$}GB_6$HPfyrk{irWa3`#4ke1KDmF}tE1A# zI$xG`>66op-8jIie|O$xFn)gJ6E98At_J9ZKZsqZdrGjCQ60T(jNeD5GfMNkxojnp zZ)Vo*s||p;l@E!f6h)e+u8h#5b?)IFO^p13@w+1lE%VdrQA6HHK_kX=K#}KD+ zZN~`IVVf66q1X)R0GKH=UKV#X93Cn;L;|06p?JVfum^?@sf59Vi%xWh6B)(IAPk*Z_@Emg}NAvAB@9N6Zm|=^H-&J<{e^>G4W(z z9tw^cz4>7N3y0ogq)=c4JZ&20v2;W#MGlxu&b%;a0lOm;@60eMrG~#1b%Bs^{c02k zG%)q_*Dm@`k(kKt#VXON*Tq zN-*5{XRGrhuF4Ktbbs2q?8ww#LfY<(Z%|_;XDR$rD&V>=<>z~f!DG)z{ks#_@Wi56 z5&)R{3%}&5v_37YLHym{Pf|2yanUl5&Dmd}nsG$T#+IZ#C#N`=mLu5)XpyCimZv%3 z-BFQB%R7JM{_D^FkFN@^NkXLI?sRf+OEuDVnJs_RVgGi>>&>IYloWo0SJM3~{UsI` zQ~@MOfjJEiRcDaH1F>i{ma3){TSVZ!n+453E*d2fWjS1SzJb}(|I({c5;4)_+;>s= zx6j=qHb=U$qY-&eng1_%PrCz%y@#gI70>=lH*L%keIjtJK*Mj>cXYp_aEXBLdD6fS zi~_MoG4YC0w4C*Vp(1S^%S8X975UAA7XBX|ciq`E=fb9cl! z7iR2#v=hJC$bqgrdQV@_e!IOqYYj{Zxqa2d$sRrC?`9C(-_Mr@i0Q@o<+5`&Ed_wj+d zSJ$QJ`oVat6lAV-Gb@em979RPua+*G#FB6&Wz<{^4_CL|)uiU35zF-GfknZvE)9(m?qM%$Va(HaD;>F=-Y`XP zEu8h>wg zpw^LZ#@BmEq?oYuz|fQwzV5N0eMHqNfDsFdV4`4;t^OQJC|}`a^Sb&rZ1R>j1+iBK zW_@E(de9wFOC25rd;R2rH}l z`Kowg7%WjUomeY=iGciZlu1qRkx4mQfJ{j0bY8!81%_KbUGW`MSi(TMO+a+Fb{!=( z;&>eiYc+S;1*LzuW~fe)pgSo?O_2;-c$}<~u4yA|5{_*{NBs4RY3j-B=clB3$K`wi zoUj~!fD;@}^zo3nD*M*k%U(38p?`awP9%tjrhK;U8-y>Mi*yOVjX03@r(R1^)Oz>6 ztUoO$1c+{*f4LnXKP?vfvIyJoQ?H+66IaOkqnZ^CrXNA+`&Zpo&3OKVsQ39E&13+D zYr4VOF411WES22S)zT6A@Y-cX?kBAs&vwI~`Ou!Nim*lC0;~r8oGt1$pe6N+0lMhC z^^yRD?8p7~w{l-TO7()BAj$s9uF5L{FI}m{9yj!k7WL_GXV}}2_L5Ta6;uC5gad!j zTG(V=oS6nuPwM{GRsAL_tXxFaN`UWZ|cD6o!qO$N_L*s~C*Dqxnc@KIa^XLewpXT=6b%iTx7F6pB6HcsVMM)mWDraXU&xai`RKRH;9~!N~ zGI*jg_fR)%Vlei6^ZBKVSfu|Xv2+oMi7lg}v|-!Rm6celO@C&iI0>6Tq(>XdYi-@tz4R8Iv(z{$m%)7@Z@+zbcR)VM?O*KD z?_d7qS$^|?zd!cx3-0??`y7wH2bup*hu4sh!}_1O0N)Rp-yEdh=WOu0;4z6|G&!{t)Eyb1hIuvXYV6bo{S zpy87O-vuY6JMFWwn#E4Uq|R%_V-C3Z5QLL)E&5aEC3j!&4JW|6;Q;{5QDF;PG&`y1% z1H_5i+6E8_Iy4-WzF)+I=Uz-WQ68y`pq?s@pXF zBkJwW_*6p{A+!#+s8WGBut32DHH51&Qh!}noZc#Cd6xA1 z0;dj1r@P6k`RY&@iGbi>d_Gg;$2;V&vJ?JoQNvZ)GKQ+CS$_Ca4UCkyqi3MfN!`R4 z=5Ijih8Wc9ka^!2lv?5ju6~m%mi|Iql}HWYsvLc{wI^6{T3XD)g0y|-)U*ZZ>*tp) zVu5zuXz3!fwD3JWl#Ygn0fCCdoklD!RBAy;P1L~0^dxXhc8=dQ>&H(dlaEVHy#NYf zs^^!r_RRuhB_q`V1tFw-^V{DnOCXA5ohpI1z*QN=5z^*~`Bb{+cASAP@BRMez*+eB zAN&6DxUM0aCeV_fQo_P$qb3lk3M;uBq9M}fqW$1$GU(t#RO&vVGGxJi6EDrv-{J$h zVgo6!XWA*i2F;!A8uc?UC9g4fh|E?TC>`}3edEXAm)l12ZMlyb8PwZZ<5Z_eI{y9% zmh*^PT+>sijFp{dy$!?Nte2HZiS;HZW%RC%5|raY$U#~@Ry^k0Z)eda5h6q=!Rsb4 zSsA|oZ+-__Po#vKt@@hw=C{3r0s#(>vlJsW<)1cUTY6zW@6{A!JlaAP$T}ERnV*E#B<4=Gi`d8QM1g2@54xE(5BkH0iU^t6q`Bd?BA9pIB zpQ_CR=M}$U-QxD94E<=jen-%zhV6l45_zrnXXi)#&8;ji3v;3$IPz_vZs5n1{ysjX zX(HfdWaz^h`!Mln%}C8-<>izVz)V8J10j4)*|i!H48q;b@eqabk((eAPEDCTz8tNK z`Sl*&V_U&x=Z|Rtkmi7Dhf-!=A@{Nqt96C%oehcUa=45<^~Rf3Md_=A`Gu#%3<{4N zBe*pQZAl6k8#JNrZvu(TOdk^tCfhc>?m&BC7{q)CjlU15dv#%%Gl-)0dmCuETLc73 z{rn?cmf|pf;_oB*{{T-VHTGq`2e@$ZA08EkETaccSD)1gp`KKy>*2kdLKaUlK(k`z z1nkj+o@n=MVK>QTl`Y0d*#G$UmoAcMj{s+ClJ~h%_h#&_&`NyJ)8p0zi=&263G2~E z#&I_#l~A;lIRN_4RcS@p>w-M%;SfB;&hyAEPq4?WGsyeS_;Jg@_SABVdPTyS&!=8A{R~TF z&5fbqOE*8`mvYqHhr0u6EnYdiD;R)>RDu0HhIgKlrn7Ydty*>VIW7JOOL<$pvwl2i zHYfGsKgGrqi;H+npLkO|7?KPi)+O_!AbNOIL{%9rBsEc67}4c(N$tSfO$A2`MsB^8 z;CRdcFmIib++ot{>jdB4jR_CSX6jw^kqwY4Zbl1flEr(~4}jl`ukSi`&kCj#L{rwJ zrZxgZ-qQkLJJsaV!|0xEMVdkqM|%eJ^HUZ;7gpNtJKZ&?dS9ImS{M`G*;ffT9zp(b znD+e8=|Tqljp>xQ&bK?7U?i$}>3|+S#S2vm`WYMd`9X94QdPB?XS(``t4b=~ci5px z*YCs40bT#uQ>GCDs`iqrT`KZtF>Gq$CPdAY~Xu9_%8-K&s4DD-_1$tg?#MwAj^28YQB#OD5&$ATT z#=Gmg7uK3O7mj1cwaq^KG7u<*v6ydj$NO{{1=I&Ny`6yPQtJKTW`c?EFC_bGV@QP{ zQ6^`+Uvl}l?+7Ykpk$mhI5Z&4?sX{8&^1}9>k$|3{HP~UDJ2*V)&0Mwqy()Y7mh82 zyI>Nza)qV~i0P=K0SwT35o-MzAPheh`Z8gr&epquO?A|7x>%_;TO1nL7Qw@x^2C_$4XzZ9bOyXQTk7c=(v0z+?Fl?I%K8!?SL()NhhYh?rcJ zp#h{i9epd4GnYI}qISGt?9_by-%4vG_%2!pSRu|e28bd*{=`uw;I$6!CcoZ56Q=?1DiqY2Khc4v(0loXU=krszlhbje z;I+Pvx?sc-63F}rsYaVXf|=LtHluM7JCAj&F`A3)H{n0;5WG~{ZOPajpnCw_rRuw= z(-0#PgVd>Kt*(m$!MiaFA)7P)HQ5}gjf7U`*3{EXBKp#Bb>DR6BsU=GlDf_FLDTmy z-(9bYE}KX)3Y$mV{fd;Q5ofo)-NP44r^{gnv-`f&$Yt$&+VkOdep>L2v~$_n9ERsv z9-WI5F`^DLtI-b}^alYRdfv9$DMQ7De;Qsq%KfQu@N0j~ZG;DBisJXZzhx(iiaptl zxQjS2_NpuR@kfW9U(il>h#V{Kp3KO-rc2>h=+ROcIMFl?{%s+SsfhiAyQVBv$LCdC z_h}XKYTbv6YTFy01^HKt2%cC}iWFbr_58kQ1Tv8;7e>G1i9TTOhF1j%XN$j9oz31V za5-48PmV_$}X;IS+<>SEX*$QMi}a1+wRXgR72l zlvOL}dI_a7i!rW7DuwD4Hq;gJYZJ$fmLmsefUdt(I-%f#CeXQMmi1`uy+7b$h>CF2 z|5#p2Q@8uU(do9$N9oPEZ@Q*ptmF%=6TJH~y#HZ8sa+xUCQE62!=9K680z)_R3S6J z>yHQfrNd9YJ-7K)f!UF}qhJ6uS&m1=558X1eB+d7vfF>`s*p+OD^s?;b6oia2W!eG zGW+75-PQ?j;AlE^$ECUtd7#=j*934Z*5}J9cm0?RXBcJQ04*QP!n49vJdYLTv zCh~RJwQYKWiF3+3L(E@#UheBINx!tYw`=weF@GnCM5^rZsuXI?mUg`}*u@mVlkctqP`DPnWeI&6Kd`>UsGdLY|x z(V*uf%%>HB5oWo`%eW04LyD@YH_qtj%M8EQ_4l4cn*X)^L)K-8UT={>R9kTC@;6ZW z@Khuz5o!;GARL+`Di4`%?}C&NFSdjFLg)tS)57{=1 zAW4Dy9ha%A4^CJrt!_CM#M1DnPJXM4n2&0xyW~`M@+6NfHp|fuj0E(N)NRlo83#Ui z&L@BJ@G3>d&&hc&)fE_lr!4&(;aS}2XX%o8e#acfhYz~Pvyu^KDtAD_JOovH8nC{V zXB2*P2&Hy(PvzAzV#)_iM~!R`s6Rqdfe(zJa%N(8u+=<&gVi&(apQ0}yxBu<>}_7! zXpz@%S*S*&T9gosTy`V)8M16B!(W$8-ukkjTTD7^cYw?PrI`jvAwXRM4NF|3FTY)C zv2_t3I|aK#8~HxET;SMVYTIoUOio;vqvET$qk}3_t%*51ACPy0T97eVGNqQ89%$Ge&dmvmSTAwL0qo84rZtxD?n3*<0yf4(dEb;3KMk6wzNFn(q6pk`Ddcge_p~DM9%;D=jPtveU zcS&6clBI68{frNq^-^EH`@K*Hpe^aje_o5YXl>%GL`J$BoU=VeP;wEXS1^lB1&!Ey z-5KwGGPp+pY{H^z-Y_8a)G}_pnl|qKaJ$?2&T*aX)dof^{!duVjtg9PDqKoElsB)` zGea_x(Q;e>Key(=L>BVmr1_$r$^tENbV!`LJ`zOi_M@fK8BQG|iO7l3#9dCWnH_(y z-pn3*X5#F!yB_bVRxPNm0dCA5+tsI9?PQ^#5L_46y$-UU8AiVR?#fjWse9BPUHw=u zlDnZ^T+<)XW1ZqsO~Q&yTByi05{a-)H^0Fg8V>eC zJ@M1iqh6);VXjlYWv`PY^%6#qN%sT$q6?3XK05Vsj@6uAxgV{uuhLB!3iuQx*RS>6 zas=g^)6OI~_QNh#jJ^=WAZOE6@p)i4s3)3$xiG(&K(7|nM=TgdbC(X?oaXc55&=TM z>}mpQTcvUST`oFu3rM8uc6i3NNq7!fJE=s~sjT?USX;Xd3N;B6_;9KOw(uj^U4F-5+N7GB!bcV+Pi zE16}|HLghzR+;{}=H#iLgOz{XS~c4*U+y)DMDY0QXSk#@&6S*P1dPG%cr@L#pQDHG z=}NW-7_*sD(kS0b4ZQ(-Swc>r>n<5}O^7r4gVR#u=fK{M@PT2!)72X*U~uYX7ro3M z0?y|XXNffiG>#aBH~oCBr-ZsFO0wjng*dEy+qs*4O#W1*o<}kK@%JTq=|(J4@-IUb z%K9>N7AMR?wmNFh? z*C}rB9mz3CDG3+iAslUe)oP6W&~t-8Q-w{?VyZlFBGPEYM?ZmOQVo(z`h5^d$GDj9%kBr(a`-$DgtlXr?q9jjS4LbD)A`i&(Kq1xs4{5fT8QV<`jm+txocip z5$(gwdCl_{3P;@W#`9!$|A#st#qnlo6z}`>6YDHR46@ov|0ASvu1YBgzxGg>q(cQ=!rO>cyaSh* z);$CZ2<&VRSavv0#t@YsaP}Yx=KugaUh_OxTym5d$NR6n7sa}&4{P3;+7{w5IgV8Q zMZKJO!z^Zbr^bLWTX+FYsb-TYvz>WRR}6pwkQw!4tebo9by0n&MQojN4yk)C9dogq z4yq$Bl-#hTijfq#*R|N-4Scq02uwr_Z(ENCDplZ^u!t;4E_@WK0sZ0f#RYfK(-KGY z!U&V?qkRPfK}&$0C-hoV*`wA019{aWB4M zR*QK*h4}ecUbeRn-4UP$=yfWb2_8+!({!SmU;NAQxpc3^lRFmJTs$?L%9mco)cBD? z)?T@Oe%pr=XGv?43hM3OVp++KNsV#|n?;MA+2K5qN&(}H?2GxO^D_lVKL<+amXdy1 z@$M>5`J}xDPq}`SFMM_^WRKI}IoS(SC@YKtf`}V0un}B-2%zQV@s=}lA28pnmR+T+ zQdZ>|o(pumI4syfMvq^mOUV=_9iQ^*r3Y!{T4i}FzFH8q*U z8zHf%mGJ+S-(wAzRRm(SY9Qs^7&9!H8C7;?5K+?7id`!y+p(}jnYh{Js~ zh>x*>``8oh(8?ouyYSIRPR)@Hh5N4Mh&q3Ra`KLtwUN(_5^eR3DDGn!X5kYM(9EJ5iHC*XMmPfAn zLI+E-E~a;5(@m)Dt^JP#=C2-mcIn{#-TKGEr?;eBU~@8&YWYzQ|JpNH*C(1fPcv(< zmEk(H*ik-jOeK3pQhjlnkEZ*#)?hGj+VmJ8Syhc{yUf0>GnZaC&d{C_L_^C``|@`0 z0W42tKP4oPy_#tl7MN2_cmJa(Ug{xbc7a!&-Nf)0uYBvz*m7pcP+3&EJaE_<$z$b% zlhS!ft79&$j;RPba<-T*(C;rn&ZR49Oh%XGRn+b_qC4l0eD z<|pt+U$}PewE3V>%P%?PII3EI!S>Crok%i79;cc(V<>!5whta#yT7HzWlSSr*5ohI z{|{mB9nbc=_m6jIn-&$+))Hz*)uvW$B{fP_%$A^PON|f|FCNWyjE%%N*CUtsb zsT)OY=pfBG$u#S`AGSk$(Gq<_jmzPGsS%?kurJ{Cr&A4rg{;%4L^a#(jSo)l?^l3U z#@u%n)<_B={obPAMTY-0M#N`ZoJCGwKop9z6mTm5iR8onY8q;{ZE{_naIUfj+{{hh zp9J!Ye8z4B9D4vj41W_SxgWjL&_d}=vy+{K+8<93x6e$oqa9=`^jxf{Jy_|tga zEUaWZ9Xa)_Hz_)+$GykU*RFz{!c+oi;K}7s`7c*yBiwxDm3Y zt8FKkZF-L^uBT5O4u3L?MlW!VESLRtW9|Fg;yc+jsIxC_oB6nH;I9S{<%Sh){rl7W z!Zd5I?vyp%5}spUYCw@r=!w~wYm8BnYL%b`5&b;FT1&BO0*;ZdulotR(9je5MHBv9y9 z3Cs4QzNBWNFKY|KW8e%t$s<#V;!E^R44jN&o1EmcEJMLyNE@gu6=V?^HuiN zcAJA5J`mDIgy}uB(wOKk^X#hW^VU{7Vp?&=D!-1b%9gZMX{auD%{CI-xHhtDyu7Mf zjh>vp^(0&6j?1F$E(w?PCUxSOz>!Ng@qyi;89=*BsRNv{&s;nge zZ9{BAX<)33Hci4aA0VA?WZ&Wz?zj*UAV^nAN^f-+nIm-@!{irtB!nQ1ZBjx5NadFJ z4wFOge_nJVKZ)bF*FwBnQ`@y0JIfGOu9GAlCs7vR3uW`PjwzSG8nE3163vZg<7N_h zsuNiQhC0SZOzwDlO#AL*-({Vi3U;Clb^Ze6MsqMOJ?nDwOgN*Ztpy&1`n>9#@syV; zAkvr#dj{@%T~J%g=gRe8d2#9`Dk5|eZ4oY-*W%V`O}dIYai$M zrH%zTvweUoNs+sgfQFIHVuUlN+fsZW@nCAzeE(f`g_z1l!bv8ZINQZjtn(FzdW*B2 zS2gw>{!=$F=wsyGMV4A9%}lCC>s1~DgMh>BXn)g*_*fl7>1o@N zQeq><1Sg-K?qOYl zdS#Qsf3RO2s9BL#KCMo_ZwQaUt~aDKBHN@xdzQ}t?=nc|^QU6^mH;{n`1?oT(T*2M zLr8`M6ck9UJ?*2lP7fgd^=C6oyJFRovrnJ1D;Rz#(1JY{H{`{PF*6ATP2vP>O5n9qRkYooWGOC{4hV%rOrJtdu? zt7l%%6mP4!%pa53oMBp(KG;AP(KC}uiJgOu_K8@fgp`Mg6`D!8BO~PgB|`Pbz!T`$ z#r*eraWPF~cCa+aUOqorsnaA7%)W|CVN|}&PJV?t;Q%5K;zjBor^e+{*AQ{BiLD1< zsxb0D-OW*JJO|}xhvk`gHPb?`XX)oUf>sv~!-OmQ4kY8>kN#kQ+gI%w;HWxIF$new zTW;D=*c_6op9;SR5J%7JVGB@523<}T&u#wRJ)6$E{`G<#ip?cy~B?CTV6fj5 z9#inPXVQPI-t9bM+0<`j*@;r0-&gncH=N^BxI>;ufO<^fyl?)4d3oIyF~-8e>OA5FT$;K%X&34Qt?d_Ko~qXsS#vMoG3JR*}cc~=2>tHZvu zMmm~HvKN;Ty3K6wh$SI*=~rz1bShuD8CuDEzj;Sas537c?7T^zZ1tfJ1+9PPw8t4w;5w46Tdl( zi!dFgjWiaXik=;jygFH6Z{SoM8xyqTPrX6BwQhw$kjZ1rxo>xW$r=J5cX_k3SQ{Pz z8OX-T5)ZkkCLE{@0owJ60gzX8w$o{awP4c6SaN6Eg(ZfiI!0~>`x_^mv%qL`S3Zxp z8t-lv$so$jY?tsJ=5u~$lf*fEr*(&M?`+Cek+U2WuEoEk=m>o+&VO_3%vPLxLCUj| zKb`W~z1KU(LgaLeT0Qr$X$w~9l!LVwuq7l_@0-WyZqU8$HrmyQRGDx!TIP_0%qpxNca zh7QT~ZxJ_#Xb$ z#$B>eE;d>0f-!6N=1VGD@vcp#Z=+>oB|86)cT%c>c;eHbkC>H&Q}g@QdS{bchB>b& zj;#V-w<-?u%nlU_I5>e}iJaM_2fWnEeUuR2{mTJX`0>-N$ZkJ$YK#&iJM!9W+$#Hf zc7}H;Vcnu(sc7{tJntpe*}_2CDjtXGZEB+$ls|4#%Zj%dMLj4KzQC7C2b$n}i$QXH z&+6T%4G|2dihKSYu@B(O(f4GcpcX1IvicaL7SqV`G2cnb9@CnPzqKrt+@Jn?-x52B zoWHBvFHLs83SqPAQlSr&lTUDSpnXS}7hP5@l>ETQF_%sD;!x$bbD5XJYIg8V=#~cM zSBil1im3z{AiFcE3R&^2GOk-)WPqT?t(SIpalD(CBRtjB`s)=X@guFTudXU!C2Ptq zae>$n`}n{!M`xdj&w=OWu5d7Kr!X@)BBD4w7L~NJ+|dIMc!svWp{S=p{27v#V%Y^! z`8aq1exgn|^}D}vop3<7&bUcdF!dzcq;dpLd^;j6{#mMJmQPSiLGYDu4h%l^f#W`< zmD*&X$S+{EH!h@wR#UEr*H?qFzF!GeE44HksOwcm-252AG32!F%+A1*-RF92j^mGW z`!}bbg2=D0$Q7T588#{>I=qlgmgV+!j8Hm&s)e`J|1qlF#jl!O?a8`p z#?%9Jjf3DVq#JWtpZUK{k>m0ulU+)>OK%UyZ@_17P0O$RUp`2<7?wPlI!Y?|K#g-E zNyLTAg!!CEvy@?vkt(ClNW;`)@kj*A@xW~Iy9#A%; z=+zHptik$wlJhYl41DoAOmJp$>K*9uLb?dgU<*9wRZKdwrS=_d&T~!cj?V8H#^oHgP`ZIo%2q2lXIZBOQh&+48p8NINrC4UA7^^4{8Q6` z(`2G>L!CHV&NWF9pDvI5*c#2{eU{0Y%c~WyxGu=24vDGA7bN`AOj6sVZ5>oy#OZZJ zuUu9h&-2yW&1?wXa#BK^+bNB@Hakf!oBTX1Yf_yk+OkSv^z$1b2+u%nb8w2t?bDPH z&x!{4qZdE~ZHbO!Y_Ll+4wdwSLrE%p;ouI8PSwakEit*d!;*U{bzturXc@X!32s)2 z+LWVz`@_qkXFB_Elt26ZcIhASwhpBOu<^VjgB`llB6wqYbqKBs?UPpe`z6%^IsCV8 z4l#aj)YQ#B`~O)=To_TkD5I1fAN{0g2Yj3$^O1<1uB22n1=hk2{d`9Zqgni-RV99B}!6U^!j@|OqQS=vw|6< zYZ}y1Wz)vc-S4Jd%c_ly$VmSrK1OaJHAd$HK%=(sU5q-5ewAJ(!&WYf%n;Tlzs_iX z`f&Bldvv)LJsUfX*!~mj*~3QN4c6KG^$s;C`>IT0^5*!2T;>@dZ)$1um(CMgJnY`W zn)snl6)rqU{}@GO&CkTZoQi{W;ZxI(Sqe+g!bd^+&2=BP;rFg@-?t7zTZ}KEsE_O* z>QyZXxjobM@|sn6ym+_?3trv{l{(%mtZ~FFTpK0>HO=Ysb`i=0XC<%MvYfA!B_#P{ zYhstPnYq?URL%(QvdG)+I9uWK!e6HLpu;)$$jF&_oprEA4`-^epV33r^fN zflqIUJn~WM$Q>;7ks?MFz1TcM35Uc)qyrZ;(gNL8zqi*;L1@zIJGibsfIp@t)OHa|D%%<12>-(jp z!`gV&OV{b6b*%$^GF@k6v>jzRiQ07}hS#;Fdi|!_Gzmg8n_{C*t9UxggtMf7LtWK( zNn!Z_YB1j^8EH#Pt_Ge&$s=_LO&S&kKDpN@!sCs;BpRES)vc$ECcQ;9A=LCAv35Y$ zW~D=Bx|6O!x1LonPVf41_55|?4j=Hcq3N7-{WXBb6jM-fM$el4yj_taufDO$Bs#*XoFR z4ikI}-BR%s{Iz8)Y_R&_^=axh^Je!4BUh_A^yKp>5OK_j8`QlKI;=e096r z#sYs|wRh9zJScwTwSwjCk@!!?2Z&7SWHTjle3ET5=kVRH+01j~LOEg>hNua5KPdXX z=?rp15A${*+~3bUEIkZv7FqcK^r%D}XHWl00_!^|#inA!Ytev;GRWaF9ubRIz>K>Vl&-+!trdUsF>h@oga3ef}9rYX)C^gw3M~z*L?)SF);4_ zns&se($8+){JGbt1X_eA^BcW16Mz=C{($b{Jo$u`IBAnrVPWNF3LVtGCET&}ft1v( z)Gj0;-f-SrX*(Iz>r3G(>Kl?=F)NtHjPiVPunBZ8C-FurbtWpQi2pWd$2Z(K4SyBi zctP{C{}IN89<$~9-3MFRu@9O5NiW)`$z^uJ9!3EE^^#tWtMnK0*YAElM)_@c95-*0KhGf$M~53Gw`$=S{Y=%MP4iNnxs zFArteJ{0J~cI}dE)-SLQ@|Kc);|J+zTgkS;jL?2&XM61Y80EGpWd;dld*T4Om!F8e zW2OjsA=fQuST_=V$K;4 z@x$ewTj@%m&>zZdhy8~yyEM56tEJDw3i;ud2Ki|eWhR4K_mSSt{%~38)5k>m~E^CfBwF4R} zk(IZkv|K1niuNRU69_A`>0>fdxE=ef5vY?H)dqZW6eqE+1xsQhzing9R4Y6hgK=q7 zpqF5OGfB0A=w(#hwYnrbd%qL5;~A zFj`rtu;NmBn~L+Tg5fcBIz$L&CO9U&Qxu-%F!U;=mxF>#gDkV!j4ZqWY$vTvbIPCy zQg76OAILfvA1HG3TC#>;IB?<&O1cCSYtyxqa*p6k9kkPN66ws|j3|QYKNSbH5Gr>7Fw&5Gb^!jX&K%1=AjAWN8by9e^Q1$*n~S0H1T7-NX${T z@X}GzsA5h*-W5yf59>#9Qta|}Q2wVT7e$VVdfI!?v4i7I3Gjo1KG8(#0%CN#69P2Y z#oL*DdGk-icLB#OGaSb~k$xuIvfv{QxhM6Tqn^k2M(VNNbvvHtrnneQqB`@=SN4t- z$z%$IH*}p^%Q8LbkbV67=>Vkt`abB4PpFP4!Lg+PfW4g*%lG8pfw7F-u?*Wc>Cuwg ztn6iQM5m<^^F;jJJ)$Q?1h^Y+^tIvK*qH2KT}v3#+|7zK3J2!xbj?J9^I>7nIawx_ z4ITQZe9vE;(C}(5@DXYMQHmGBx8=pW+{oa; z$<>#b!<)&T~Z-wjL zl5Et3Zn^4wqpp%y^Cs8R)H(+NE$JKHPPlV-XVbZNQdLy%ItZL)9tlf#BWw->c?1O4 z10v1tU_PyPPO^Gwrf^{TQ~umOl61BTcHt&;zvQWkTAe$?r|QD`3kL{&6dy@!MU9cN za-Gsg)`>!JE2^7AK-$_R*1dN(za!s$+^5!Y(~dhpzfgqZzIm2pVq2jQhKzeO9sU)a zq$7bB?)R_~{hHmDPGT@?`hSe<^jvftiD6@_AcGWYfp}~P6q_WHVHe^30;Ku?VP#>2 z@g?nvhCOE;-~G(L_EA54va7J*HuA+5Nz1)Tu^5 zu51&ugfrwA0OYj<_o|+rdlr2pHm$~~F1@#%x* zMUJQMc$M~eCe#)v%N~GuzGX=KQN^s;R2tZYIiMElcT<13Pd3u1k09eVIebw-gcW_?S6s^u7I36wkUTVM(>*NVfzRK*UqTX|APIJy#y1$Si~=q=VUQf z1@>@JJCt&B?e#5@iAN=RhUEn`%f6lLCb1Dh%p8}P8cG(Gyi7WbMQ7ZOXq2Sl>k)Td z9r@nZZ+fQzvX?5*c`*#DCMw!dA2gdmZmH38!`=plJbB>YG~p@7kOaO$eO2E|X#0;d zcgr9dnsA7O++(mBzr}lZq20;h?WA!07V3L+bV_SrFx;nqY40H5*u-ImyScuLy^~oC_O%0?zKs2yo?1()bmkhpDkxN-Ri_GaJ0A7M=uI8WDp+ug z_>ejnX8=JP7V@_8UZ5UJ4f8^<7RC*M#>1qlYkMveiSemjr_*90KlBR)EE&hb5z&J} z;reNG?2i-@4j(e50D2Q-EDdA>AduG^KkTgE8V!~66`GA9bJ(E6lsB%SrXX_?^Y=^a zPZ?q*7<%6-NzH53ic+3ug`gemr_{s3CpRO+$=SgI4YjsUWyn>K%$t{pw>s;=*g$c{ zg-x2}Is`dV5Nen+{(wN7gTc@v3mMBPjBwOObe0EOBBmYp;3*(9rvyNV`td zZ0?i%37;|Ii4TiE-xoh;d=-*$NtEOVSv{3XuHOAVTG6gU-AZ&)#n|S8!Uv7S`iFsW zvo>lTr)Ido2V_&Vn~G5Z{n)0U-&ypsOD4s-jvSAp?6R)l9%U&U&=!?Er% z**WA1>kb3kvoO}Gp{sbDQn|jiv^DWKZF9ivsG91g{XhEaeX5_XlLJ%>?*G&R{KMSy zA3h)-ZMyD&wN{ukmMp2<052hiKtD~H;HZJ47wk5iSpe?A3L4btqK8T61#@`*g@d%k4x$_HVfOph{ z$TGwwtZGOAD~O_2jIZyuJkxWl2?OA#xPL6v)1(uYszF;IRDx{ixyLXMG@fq=Yuhav zyr>okuWf!NF@_*|8PIDm{dT0fu_v!x;MnPch+|RYaVBmvv27$*D0yWqK4>hgB=po{ zXWJ_aeUvZaMzX35_PhY=S{6@(tz@+CRewyg3 zY<+90vv$hczGl^(|DhN|4sRxm3sY4b9Jka+{z))L1D_;S)QhylNNKvpkD(J|eYc_YZq z89DupBt>W?9phcRzyCX4YcbE&oEO6S5mj$fnLDapiwFnJg32GTI=%TrplK!a7PmPA zZ14x^xTa3bb9u$y{OA0FQ#V?>uXAces9nBM+EH7AaUpZk`%H<}&PNH=IZSutle>-tX$4GM!mIO-5a!3Y(KP8K?gWmRxH{x7Bdc zP?8$a43Q8Hz5mnj`V+d=XKl#3_Z4T*(&u4yVfA8rFtu|<6>|6a2PcN9#?}x^gW#8Z zZ?&x~5F(Oxpahc06pzw1PHPverctW%X1gszUwV5s?UK}kI_|gez4K+4Sujq_6a!gp zE;!^^T_y;${3O3&fbuX&LNyq&2F1E@7Z`W z2Se+GW-PvQQMxyem!29pc0WB*lJYIwG>%FZYMxCo$eKAS{;(3-T8jZ^J>h(6f!wo> zu8jkFssdF}{a2fU^vC&cxFwsGAKJU7`xMrPm8_?EMb9uLP6Ze9Dz3$*X73GWwOazT zz#Dlx8Fp-uqX{`S98Za(6ItxuD_jgebAx2;E71ETJa?Q9qHooTG5Zv)dX#H)dd;_~ zivFW^`-H1F9lWz@9(6dnb0YI^vx0b;(ZRz!BSb8E&Wu9pL+?q!<~klz1uVs2iI7Fd`N{$yg#d!TO!q_|7euJXf#_rlES1kEzX$8sp}te{Hs^ zF7dQAM?rHnuKxSkj#4(WYusM0*$oKm_T_-$F{8EsUd;2?uV5i!7ex1kt$V0b2!+>X zK{beKj~cYr`KVMwGEP_|Q7?f!2#U?AmWu3QXMO)(uOx`gvKmD`oA6XSJj^E1Y{`^5 zk&mat^YN(}hBt%0Pq|E^-4Y3IL+4nu*%KrQseA}ZD)ZC~$B$D<3Z zif)FhJ!o98OJz^`Vl835Qb=Y<7C=rpWG5bc^b}_zB+S@YEY%JJ*KT1{a?W;UJ4aOR z2VkXaT20kR9`zHLIe_XB<8xd+sQv!?^4GiT%G0t=6AK^{-;YA*@iJ~AK!-iA62C|% zFyKMiClAfvmtfpksPc11QDsH1sNia8;M7ov6QdMPW_kw?K`kT76MYRtN{~ zbdA>4W;;z2gik?a0=Ec0{XdE2XzSpoDkN$xNy}XVw@s3a?8rBkklo`j8TYJr^xtoc zhA2tV_QQ4t4>7Xhg<98U5ySDSYWi)pWp#6#W~QIZ><#Vyz$XijTkj_zHfIz1(rsJr zdt!Clb3+YrC@gX%xyE>+%w_`KLjF3Mp00H{XobUWn|iE3xknf0?c~z3ZKnD5k#f(<=3L@l?-J6I zRWBVVA_pY7{hP|UzF^BXt>%%JG)C!4Qpb-HLmDr%#xM#%FjXnh!J%SVS3uu>0P7yj zR#_Kxi5(SW@p-e()64J2=Sni8b8UIlm2oA7{f*PbDvFQk_^GO|jzgcnx>zua3K3%& zTa=xDWc}t3(Ll*-3eeAV3GgofE$$BV5C0D+hU5?LOxTpJirSdpi+=ded_UyIUqOBU zcf4QWLa(#byBXk6P?*YNo%nJPp9@(?!XPpCL_?UjH%? zFm)Na1F~LEFp(sx-#zUs`NMaC*XNVSIaqaJ1oR$BsbTI21S5zZ@%UW*${}8PlMwff z0B%KA`8@ zU81doCXenlHIUTTjD0WBxf zEV^@>qVOhQ&G~NK3V*)!=1HUFQWFZWHQhM+<_jtjDaoJ1Pu{6#iDiHFGSm>*%PiCyN!S`1DI1->eVsm)uLR??UUY4huYgK9r)cx zn5{DqkOCYbD?PWMD@Q}+el-q-pp!I}I4}iC;Wv z><&W=WXQ!1ei41G@j#!M|pstA`{0xnve{de%5GJS_BG$b(?~rE#{d=jBGD7 zt}jsEOZ~%>)dk|Thd%V?ROvH*DXaqW2Q(4->i}T4%NMhJIlH>Zk-?w?vuYW=Zv#I& z4EcqKw|+uz+rL?QbsL^KLS&2;gu#%}FU9Ho@|)DUb8PeEYk=DDKQJMqJ$-|2<-OMh zRU7V_tA$Or@!&?twOOJPQ_qzCsl51O&>7$sH-X3lCt|b<{dm3??$USk*;*>qn3~-ZBBcS8Qho|>#b@`rN*{`vpzv^HsiUJ>~@8@)&)#J56Y^KiVK>h?*F&a}0P5$E=SH z@m3uJy2zGOuPNZp`8wNwzdF;W`FA)!!ST)vCg zsc{efXt$B``OhJH!DfnD@c zYV2Mn{+>PswlTGm3RSq}kh>Tj2y|-HA72O8Ub;b)-am=?2ri4-a%hmpT1Qp-3CV95 zAV$_l{)*N9i>i}e{x_T(d4^IQ()H^U|5V4XC>lq%;dMv&!heQj{gsx|4%`I+&GrMs zy}bya%5|_>9;;5)K3p7 zF!>_(e(=7|uDnYnHS;R1yHr92RFsX*wmR)ok@bm})atM73T(m+X3dc%7xz`C(wRR3 z;n6z-!x^+k1#Z!cow;wm1hE~@EcSj?2dp?X7Fn->cVxFmmNp*O`A*)cn+Y@))AAA0 z-@6r%B=Wg0WK`WWKlp~Mzx>Iv&WH_59$0&ibBy^lJh-{ zxZf?xxxSfBu1n?JAGBht@#O8J3pl4*su^N9ySkMk8O|h264Ad?xctZt6$oz#-ewuAne1GHuiq)?_6|0U}$>8vG|U-!>V2RWJ`ZbMiD1RWjkE~#Sp z!O)~!U@pg(fP>%mZMVW-Nh$QSAs^aJqcD189vx}+PpNgEpf=EYEky5O|r1p zb-bXpXw^)_enjoNo1;~iu5Qv+*Y4nnO0i2#scb?P)$Maq@8MqKD{Z4<_MP5bIV!t^*s_S%XNU@ExYI_-SZB;Pfg9zobsNC z0uVX~n3a^a{J!o>gsT`WjJh`wJ%mcA%2FvVAHU|@NG(fp*84KG)v(#@<9II-tUXQ4 zOR8mT<2DmYMLArSa{;H3Sacp zx;S=DjY{or@9yqV^AqZ&lV?Xq!ln2BdM)H?3as;ZE0O5lt!kd<@_%4S@%7ypF{TH7 z5z8b!H3#{~+ zI_PKiVP0j}WWE|jOnWHkUIBQj-I0_Rv5A9By8Q@TeI?X-e*TqISj>0i`R$}tO^Ko` z@0A_P1j+}X(3^{e`XOQ?;p_>vneUXOpkBAO*6eCc8LiKsAEJ9S&)Vg^$fUW--vB$V z=$hkXBWj!+Kg;&6?A5n&1?91Quz!CU(`G<@p#fOxlbvg34m~%ludus{D5>X#7H?rU2p%tpJoew|psyHG-*gUM8Vk*3^gD zpMI%eXMalv{(YHIxg!~WYhQ9+%j*1P^1KbVHq^G7jG}Czz{h99_$aQHtziD<@`~KC+tT zc}G~rQCT9+w+V0r{&yd248oJ8W)zr0IW&5VS6KYGS5whAo}PFe(WT3BlcyqOVUhCezioaY$k7#{#H*PC+>H!uf^S<=(uU`AD;j#a{`X*!p!qCxR{ zBMz%95HjHi*osY%Xnemfep)igEF0Mj1+j-qinsA_@7k`XW;*C6Ar2*#uFt)wszwtI zl!IN4sb!2@?k|~7YjKj(8x{BWp6cNBj6p>fZOoNlx-~L~!Z=mVV|JE&4S+a&G0Arf z>~37SKEv)D_k=I=9nnmv0x}27V%~lSSsr*sD>A}Ia{XrUkz*d#@`M zwG{}Ccu4Bpi&aGoIXM{#6gm?(drd%iyF) z25j~jT)QQlQDMDJi{qt#>FWGZBQ(jDldy@NIJfZtW8XnzF{KyMJi~FB0PA|ctUDX5 z>JIvY9?=g7*)M3+q#E0Z5XEOJ@HB|DRs>umw|EZqe&#^bo;A<8aK%b%kt&OAYi7!p znri=sN5myyOH@5Kct2a;N_g$bnk5;w0A-!MiSY?0%%`T*wsbR$vP%$5XN?~ni*$cG zGQX!oA6{^V6E3>ZnjXP;rFK4-36M4zLVjq&slk8w#%?`cCh)^s-ewPpu!S{&=|Zq{Z8e z&MC>Fsztu64=2fu?WsBP2Erd%kWr!r-gj+-A5C^-Bs1P=1ZRb$T{Y%@MiSMJ#QTK_ z^XaF@#lSQ<$?kp~gW9HrYE66*MZqLEX)|FngF+SqVS-_IVS#Zq9k)H601^`03n{O? zxD%;;dGQfp7;r+ek2>@zM@F|>Wm4TO?r>?(%8rz~*zbLDncZ#t)&;3gLs)2M`y7;y5*fhoz=G zlDbtt`z``Xt=Y9?Imf1l>Gn~vsr(F1wRyb_u$0Tk&0E1jKLaT(nT0+g#$2h`)!4;! zec9W)G(oeDli)4ze2ClI^4l}5*+*ovl;_DI0THH{J0fHy*M2mP0K%osaNIF6>pZAn zP41TipproFNl9tH(hENx35~rlg9`Di^TphJdsx<)qpklFRSjIOz!)eA_5rxgAELLy=u z8^0;YZv&1;r6&WvKnil%w0Ki9TI=Jc^{QiMD*>ZFk?(hiDMuR1pKu+W>;KD!{@-ZG zKkVs*p&scoxmHVr^%T1jy2dT)F3Y?<_P;Fx^pS+ioER$gUmF$AFY2xUG0fY$+7G=- z{E_BO83fh4Yv*w-TH{l{ydQjG=cncn=ul>VC*)vA<6AF639}J|E!Zr5t}K@!CC7_j#;YbT+pisaJ*_1 zgXwR(z)Yq}uT~LbYZ#<3Vds+RVhQ8>3M-9pjNrT=ro;trjUcyr_}YP9Bi8Qw98=|+ z2KxSmPBnzu(*@(@=*J6TPcv5k3XT$1c~S~gP-!;H2w-ya`Df`;i_P4_&!GWJb43z*@GO@S z*J=Z-G5I}s-l6by-&PdOZ0MU!M?Yx7JW8UvyjTuLzF}*f`>OKyeYx!FUtgdh%Naj~ z_1BJ1OLgD;FB#}xVFQ0_E#gTz(p4ElLGvi%<`U1pt29WNpyd>2{cSH7ZukbN{&N&i z)GSPGhZKhK#Jx*^lR`Qg-kE6(dcx02QJ*AlBGNVJH;r)S_*`nhBHGM_?dudI4Uzc| zkomoLE)}`Rq#;8&M^MY5q_qnUW28@ddXyeFQ5;~HGbiK~M3ykBwe z6YJIgWmryU8Gg%)-24 z%Q-RPLurp$>?K?KKf_GHCs>bx3GHOYZ|1z`FO`uA2MVZN5(`n3{-+i|Y=hj=l$Nf| zH@sNyzH0w-6;*20cDgWT;paD8EjR*sXD+Br=25$LW-9RB33&B`y{wM&&N1!@&bxdX zhI{$g^H*BP=?ZHUHvOH6S&L6Yp>6Z$$FB5z7#`%FUeNYhkYy#Mo3_ouTOh+KC&eKs zBc6#^Ox-EXkC}(=)jCeBtogF82l+3Zy+8Rd?(+k=>KhRtq4Cs?8*HWv_8jJpnkarL zU-XmX+}aDwTkrg}C&tiMzhJ}WKI?~>bRWa@!dh`}!3liNgL7fn1*q~XLO#XFqxUsS zFo@|eu4b=%?f2Rx!R1wUTf0{Gz3$WuF3?@hW7~I)UvIaDvl&FZKki7RZtbJ9G}-h! zIb241Ry?eEmc_yNw)TTr^Hc#@8ONZ`LZkOuo)0}vi*nZtuKql6}$2xO^)=!2YbTnv9H9= zvr3;+H2=Is&`7)Mf0t4@C8HZ2bq@bPx>oOd%~C{r>5|twIzuBxHWqI93pw-diuSQ3 zuWw);Tel?kFLiM_#!m4{2RIm`OAV#e^3<}0U8~jE zisY4nf^d@ePBnDHjg8m{-dl~gu{K$NVQI)1pU@Z(eeY7a246}lgqk-R{rrJqEdPB@ zD+1a0?0LMXv=;cO=>y%Gvq}8$?`H?d_gCELAz!YFG1+rLCac>dmVvmKQj$~rCt%#8 zwU1Hq60v0yXT-_#a!;`15o+=;VydX#bW3zer^ZGzyJqlVj@TzgFVBxD3`&J-aoUEh zCDPyMO%+uK0##eCH4lC{CK4XMH~hfAA@)?@U}Ehu^-Y;tM5o~v5|4ch1g<4r50Ljg zES~)q8h&|nfbq%DE7!)nSp(+`;2{N`o7W6{cS5y2(1icma-(&zG2_ZvPh`|S^$832 z=dSvcjC5GGyss-^sqb5a`~Z1#PNfBEZ2^j7l#4<7NdvaX(-%vx@D4Oa zx5jYW7N|wBmRzP}&JWoC{X?1Zmh7ku+HEgNY?xg-zKSfsxO1pj@9obo0T*}sqvQ{N z9}+b!OVL>gdB#qkhF$5L^4;ZJ8=r;!_4)Ix*t_TKUzEnkDpl z&BzMO<%SK$q@nM#<%bh{-`p&Ix5ljd+$oTMFMH9RRHj+CUM8HI!`q~PY~`}s#fE3H zZ<@6rvK0PKk=j@eGN}M=tM#8H@o6H~nr`hW%sy)sp;IDFW(^*P7N&|48Qp=(!>_xe zw1%EN;5;0;(vDzR$ms1-r*zv>6dt=Q%_F*_7nG4WH~d?kqojheBhkOmmA<>45m;H@ z4^#JJp%zw||7$Hs1(M*TLFM*sK`+6_J9o`XDz>F-y)ENzdv+w={&jicuf4$EP8aNQ zbzE2iWfgiJ_;gJ**~7t34x~gw;~BL=FS}guhR~AN*`XD@kB0OZ&vzt}4WZ9+)F6{B zI20SvwucQIesqx|*k$bKH6iiuf69yI>L3`(3geSJ4Zin)m#l=jO^|wabS|pBlFC z_jO-NwS8ya8L)0(|;dznwjWzMn5zw zU%`4uBumkh&!!ypseM^};W*&a9|EFm3fBn{TMj8z9mw&iF=t~pr}eI}$S33D3mHin zCNgeIOtVhZR!`DhtvmVb1>LZk{bjx%r1e3c2>Rp3HG9tEtJ)J$^AEMKA8txFY-^gN zXAH(6?Vlj9H(GyLwikHB&i4fC4}V_BNX#+wajTtAsG*l0q0R?qiN`@Fg!;}i{6Fg6 zJetb(eH)J`k!)qiJe6dK$gpJy;fW^mJP%ujOc63~4=pAI4V0muO-0Wmi)w;%uO7@1t zq!)^xQXMj23#ZR7IEBz^EW9czVa8d1x7K-D%#&rk)(EGQ9O!l(t?1Ua%Gz4N%_=4d z+a`fy8XdT-5EfR&X3w>Gx3GSP;dK#3YnmRp+@-GB${G25z45Vad#dak)ND?K@-lV4R!J1 zt*ha}q=Fkl6LT2uTOv|#a-8-5R(EOtS-TqTSjV9h z5tGgiN2DM|N*kef*br>h%GwaamBYQ(1Sb*SDV?xx<5axmke8-m!6K%3q^#7ey6m)~ zMl$k?y`xRZIeQGJp9xEoeFMX5Xxp(IKFSCr3_&-}9)sxd=7vnT{(n5Aap)l#>P{vWohH+8LNz6q5QCa=_ea?=p#T%_E!9vl5cVO*%s6sv#frMmyKvkYPObQ>Y@98^%UO$ zrmLV${}s~NE)4@UkdmLYXg#@y)N?o`llUWfXNPn8+XYWQDQtR7a`ZW}sqER~99ZSo zjKplmHgOH7#bLcnt+t(-3rRl+E>wngkwB20HtzlhcyHO6J*L^tn)1nEuZR0n=?!$U1cHkfm zI9yhrKt-oD@5I?tLd9z-IC{U*pB?@!bbpQpuM?QlXuLt~t`FppTtZ!Au&c(SoV`nQ zXKF4DRM`JsI!U9TyL`5-30zO+gX`J(9K=~uCbKLht9|pb^(!{;;VYek1Chm7i?8rB z!lCOa3);0oaeZqY3V5D1=nv&Ff7JJm46?*y5~)qK$Bts~qXRi7=I~PegU<&$f(}9M zh%g6-pALb4Ag7|TuIt%L)PHR~wlMMqOK_TGC>_?1wAWrw5)oh@EM+GNpslev0z-F>u;1{Kd%mwD@=s6?+euu4? zIYGr&d#C5CvAF7#i}2lCZ{r?!kKy5hp`mNx%(UG?xvoIWF7CHxKL~I5j{B_FIaK>ry z+!nZ1N(^5fJOJd2sC3I~7}8#--8x3b_-3EaZuFXePbD#YhgECn)M2gbkT{`x6_jYZ z#R#z-1T7*88B&uVje3*p%GC7d4gB^<;R=WdpX$#p8vMNmmGpuxVOwr{la}9+3wO!i zn91dURI<=^JQU%@oukG2s~NJ~zs5WC2nl|C=QU=hQ4-Jt>%~_hew`U!|0O@;4>Fy#=@B-^?w~!DnuNX^j9#3x@sKsZ~lJK@8IKK{?8w;_MZ223(-bf z((+vLMXzhzc}MQB=&pN?yPyBmqj`vtSZ;i$PsNFC#z~`)uUAG5%G#GDQoTN2r-oBM z1y_yF^EK?CSF{f?Z2bW9?AAhLrN{(kgOr~Imbu6|O@FT8B0|S#ASuJlCI?GxbOb@Y z0IczL6(AGEZt52$2D9M`oc-&MI+f(J+SBA`yDfa}%`cYjrbZBwP-VvQ=*%pvk9c{3 zdis^B-VUShCs|ZzPyKymz4gTF978+2Ro{#HffspKtv^Xu87?U&{OW>8SDxPc2WbAc z#&RylTu$9`Wr9~X-;F^ER6AN|TY-bcfQ(7C%-u~Kj}{OdbNOe3O$}YWSw8zb%?Xr4 zKuiiA9Pt?U`5BVI&@58{zbcFZ@A`hjmAqII-0FvC@3^I_ zz(S4pGhI1xjNjEX^wPK2=V#BP#PAT0K`k1mw3s}LeMN!{y7?LuaNiL}bjE<4{e=x*LpOp<@U|1 z5hw6NxlUxeho{5#98WGC!;-QC)aJaDQ&T(&JQaXTymtd;wouE_3H+GT_7hc?(m$Gp z?SuU~YG4c88*SIgm@Z(rA<5HGPlRGH2&gr|p2guio25JPE-|BSQ+~&qWMw+?~WRW1oO;34PNp&q{D(&hCAef_is5e99Va z^S89PyV9F}C(d-%9A|oyPdb2vR{N;?F8)A$TQp8cbS6c7{X;n*lcunW;VYnFFvf%bC;a?M+U%+I|QIY zy*NbPb>aGQ+fmR$pDw5%FRo5@ouVnJCFVLOoeaRdegH!}_yGKTZ`q!yfB-uHXQ+a} zCS_Ypr*XC|*y4j~W3y|Vp0g4FCM%`ax%hW&6uUibb}r`P(ni2<)iAO@PE3_T@3Gn* z9Ea^#q&9F*fh1%$j)2Mjg+#7{D$euEy3*SnVa$FyanvV0x|a@ix;oFRHiR>Dow$H}DKzxpOOfk0 z;DG*FD}sZ`V>Rjkg$djRn{nv+rKLzyFv)AV+luKRL5wB1U@wqiMe zmfti;|LzeM*Df^;HCRj=97o?(>G4Q!Baq4SF3c5L9WYBsBofEqdck6-sM9s@@~ z%Y%JM>NvwIYMs$RYbKhpyvq8%Z<=?#eXMrCra-0;yyeFz*&1;lrc^9CqzWwj@#bryntkcRL zF6Qmv^;iVo4}!|aJA1)4yPI1P7r65W}?kla&djRZT;u7dj zzNA>?xqId6_a#F=qd{MYnkoCp-uA6^pR%_i4)bEIZ>~3WCp#yvqvty?9?}@$iYKun z(|gb^D~jQYsSlyfS;jWZI*!}f8typoX`02Ty)6jYl-S$Eexg$7-(>2s2Nt5LbC14F zG_8ej+&s@sj9&%z7vF6o>2;>>!N0%6JH@^f%GH~4^s1BLC0&$Vin%?S29XI^-O6oI z;8wn^Y{O15xA^w9A6cPWu;r`X4I{j>q?RsW&oJr;s^V!yX@gP5R6aH_0yFbh(4b3p ztR>V=>ke-DQNlzJT>nNnDWF+Eqb-1k#}0%#n9HH)9nr<~1DR%r38G6fB6U%ji0oss zvz>75GY2r%bA~nouKByM+5nMdHh&b=fEk{u9NfL`~ckT zzwS$}b*jwDE-R3^!#WC|Q~T(Y_&7GQ(DN3z@UL|-FI$#GE)m!dQZPx2 z_zAIbN>$4Rh!J3HE4MP{qM0$+lk33iU75jcIz^j4gbP@L`%;xD$f|bkf}K~n?+OB| zb+fM6^s9#G!QQ4~1=oy@k`NT^KGgL#C`28>J`M`#fo3DdH{*%flfG;^cfZkclOiOh z(l$)o>nO5lXC++jk|1@PS+0{9|57T(|C&u{mt{ssRlJHx$$YS?P2Kx!MSLJF_Vcgc$(oYp8fk%I6 z>svwtdMCq$`Hpqi0~S#>buc03-z}a0PF@0G#Iwu$U^7Ph3Tz=oLATI^aQo;$2YytU zV%(*f;0sBav^hcfUSMTJ*aGPZ7W$rGSCWw1oRr+i>bxWIWC|oyDuy*A#8k|NEwC@Q zT!BgT>M8nkM_3U{K{|YoQKS0NOlOvxy_Bw0?EAjc3?Y`G9Qu^mfx;L49)Z5sr_Jei zD+cx5b|e?fGL=E!japT`1t0$1+%>`}g>Z7RQ=x=I!ox0U<^lDq|2Hs+md`aFf4d-- zMiKv@w_^cx*`W9vuncPSki>`pXRV8h>3$irU8YbOR)kiUINa+z4`4a`!B`5Vi7`dTpp1zj=%ho#!y z$lMU0(BUNUM?&KPmCRJEM=1!u*D&Y^$j}EEK6$eWb}Fh=FW=g0ZRo0G7CuZ?#73}GBCpUn_lUpf` zF|qQY@q`RdWN_HC@*BgfVQiXbeSBV!-gFkFq8?=;E-D?9;l{5qChV1=TlMd)1ZezB zy(YJ1HRgypp1dkigHHfhKPlC~?!E-}>fHwS!ojlt?LATR0+l1g1WLts>kBf$Ap~Nm zwf9uaPZ{>=)>tH1GO^|M)_BG+EE*L#Z-eXT$BcJ8;=m{4(cJi9v^J@4s8_0hGYbNWZZIW(o$^nP&x;<-MPIZFjm zadTH+I>8adz+F(2%D73iLhD>!jr7IeXECAgiBN9cU%F^~ny_!Xm$DZ6$)oX(cSlW3 z$XErbEST4y3tDC56Ou53?~#UM9hbb|Ymy%7s6g%F#fw}ayqbFAoGzMQi5MPSA_e#p-lAT2Kldo_M95&3__XV2a zBB@T80sE%m9!0yV-#xOx+Ri)=E-X2%2|yL znlX@73Wq)ECxNH5%5HZ5x$fRe)LNfl*^#X@l<5WhJ#QN8yW3&&sr7aXUtUMlMg(!d z>{*Ips7%wJ-~5Znu-9_%8CbY0jd&?75-4(9s_gAGJ{fT$M{F6}H`zMcUZg zyU$tDjqEy>5Py1mHa~wctz*8890v<1T)|)9;Z_dX&gws#DoGqa1RF==MKR^zlnP?1 z`iANAS$vG`3H%I#Qeeuhu@h5!{)S}B zu@q^z&9$1;!q8U^HZ&O5Ri;%Ug8q zDRC)IP%Bad+ieXDm!%B7Esl}X)$(0$wz!O~cl{u8{4Ajl6Q(wd3lCMLsB z$7q#sj9S_)-RGonZ@^MesHKWK$ymV-7!+BOmp!OxyeW)~Ja5gflD5X&Db;eEBxr;E z45JgDCpW?4Ai|yeif;-Y>-WB*##DmLIUq0dU79`@8w>+_pUsiN*hN=reGUR3Mikim zup#%fM~vUwINld5y(biMw8iQ9JjP!AYF!K17m;H2(X|%;5Xp5tzj|9UTEPkNM-CaWrnDiVs9oFD%>_?}@qBMidCrlQO4xq576-39=3?zn;!%-4quQhv;(G^9_yg;xOL_p5Ka*G7h7$$En!BA)6H0!IR0s?$ z|Lt8r8KIuY{I>G0afK4R@@vE6PXM%(Pp5@R#HI(fE-RdwI(qkPZ4)vbq{l0AEpwhHls1LZha^teur8*q|f*)Yau!Kn~&^8xhxJcE_J0f zT?$_-#=y!!Kt-8(#}lD{eA~a;+|SHZCkco^3*#(H3FVyyN=JnVb+i&~hOh(`nU|3- z!i%3pF`OES=hp75(dsk5YR}CYWS%E)bDjF?EArYXBV_kG)TN;A-aQ(f5t$Yt!Vs>4 zNeLd#%h&G^xob9(gKqa4KFO&9EZ+FXXWY`8pqif**edkc0-HscxVTwlw$mac zI zdDP4IoNKGsziRTCD5w$z8s5hk@iJIl$tWm3Lgk^#35YT7y9pz`w2bd*pttH6p><9v z*H&jiXpO^f;BK>iU|ZX7^47rwVou_Xc?#5s>yS}PCp2a8(A6TZR@X7WP^HHgA@+0`0xfm+c#Y5!@RBR-hn-?M zD{Ux8B~DywjPCA^A$IpEg(1HXjlr-*n^ZO|tj0zGa5~zaC&kamFNj3YX?=rzjG83d zXQN}nO4r1*#LL{QTB;>+LZdEf`Pn@+JEhZjH^6X?{|uZ06G0`8 z4UgwrOTA3_?x_6Rk!jY9j`%0r)Rlb6H{x@uWy|jp#j)!mM$9U|xysJ}! z)L&FV(^llpCEnxzH6~g9fCv}>5dmFCLkU;RQ~0Pb*q>vPe<076WvvPaqpvom(#Ahf z_gJdh!pU56EOYbqw(W2_@zOgBeZ{XmNwmfqJ>RI_Lv7@=nc}a=TTJACGh8=hQad*}n0VUG2eZ zGM4XPYn&=98>_IYU4UZd^ht1~v#}+Pqp+K!l6-Ib#;3X^K~_%Qh?TdjcF(y9HhPMdV%STkE&&a&Dgt ziCXOm$Ek}x7M2KD$4~{u^x`unWVAE6x!ZOn18=u5wB3zeHaSmk!KUfb(X|(lbJ8rnD-2?{m=nb9vGz06EaM@EFjhZ6>kern{ z`<|T{cpP&avmKu%a4Z#?LP3DqyQtH(g*nHl_E1#F9)=hDsnV!2N zX5=M`+Vm4ETbw729KgP5a65*lHl+T>%W{B-ep^?yE`DWoJawkY-IE;w$FK03?3Lqr z>c~K9?iUy={F-UpL;v*Ap@UmihC-faS_1ZIrM0roHYEZN5iRj4rGUt&iV3<;r_YmQ ze~Rz=EmLOc%=(1S+aF)uIO8$33dW}zg6GC4YfNC2O#+vvTcqvgh*~L=pOFA-3cpB8zAH=8hge)!8@Jc(Oro z?1P5Bt86lL82<7ZomlyIO2Pw9OhBG(If$|kyK6>Xn}&0S&`Y-l{J1UG8|1b2!{Xrz zkAp(7ux#;BpJlKlAT7s~ymo8=<e@FDJsCr^AD(+zyIYkG3L);9JlX=ozUksBmUtn6;5_sesAK6K zL)ryRHg48;V>AiuXLH|i5|>V zr>F;3tF6K#y0+rsKAJ&Kn$ofLeUnp_&8&WGR&5_tH4gv09r{;_f(6cm#x9nqg$HI?*#yM(Mbb(qWGp*h$pqb1(s(kLA$MyI4;v2 z)>-O$Yx#KxT*CZd)EE(7x2dSKssO^xM6A&#jH}1M>w_pTELqhlIb9n@6lxW8W@$X{ z>U~cktzi)o#>zM6M8`wTOf}Q~A{v`=%h{rm7Qn~_5`_>R?_F^3EiiHm`97^R7ed9; zfuoVR?tdhh+o3qDvaesf`9cURoM#UjEVWA6Reo#c;zA2xoX{m>+l6fS&C${1jC$wz zJ+>eC7tdm+5Xh>q_*xMq5UDJf9xlB5N<9!@Lj4%bF_n1(@DwSC#zoo~(;pNne)aHX zqryn+Snnb(ve4_SVmXdcwB%}5L}hd;1Hc1+#e{5X@v;f}6^)NdB5I2`B)s4V*Zl=2 ze*JX*WD}35H}?af#{60?7F5`N8g$m(|4!nBn>NCBcTN*mj)0kRidXud!-1H#!WdAX zyTrRVQpLLKQC*-gIJb4w_}LL`ws*i=*r}Q+&^&cBuoia*C8v`Izp;F(Wkq07*bfTg@qbF%lv%BteK)@Ht#+9)Cn(P#6-6O(Q!%c>yLXw_N6JVAA6uH#lCtz zPB+z`+gc}VKKGW?6fc1`#ga^^lP(-=t}1B^83%N(0f)`=xwfrqhW65cdN^xxW+0j^ zgrSF!TvXd`3Ql!{*@_Wg(wVzjXm5YRYhOc1OoxSj!=HRCl-xA+-Tfi$#k-IF&-C7B zOlVB!s!Qt`E^|cqgCiFU49KotWDdAnusPbPawive_C<1<^m$HGQ2yUVe}AboBVt_@ z?jqL*@=WJZGO?HYOg+9Y*zd0Qnvktcr$wOASEyDDFA!`VBQ&Y)O(pcP-B=h}OAg&v zl#FoO0P6`WmgV%0FRQuf5-jizA~=8yG^iX5%uVu-d(~-;v&n_t6~QsGdaYVK0g*K5 zo`A3+UNczVt!{s^a`+`|A;i?_s(U3X7-<4h4l!VzcN!LF+Mib}85D&F{-E%_Y_#_u zDJ(cbQ!k{%oJ;nk1gimJ+!#?($;}PBRL>a?q3q z#+%kvR_*UD&cvI92Y>iBz1|>zz^Eo#7X_~orPY%A&E>uu)+Py-`;OMCgoTXPSzEV; zoz!*2tU%MYfg!;3)xIY04GS0W#x_H`Xs|2; z!1gqHpI5zAJyW|6)~|IUo|-6n7N3#oJm#nTl!XL?LMZqzf$B&a8X}i#tyXF)``KAL zg+F2>#96bkKNq&mGf3Q;0b_4<2$vr=8v`S|o<%dt+9QlBf`1-eSICOj*YEUt=e~itc~8hN8kR?i^E1hl z_I~UIY|3KmS*T8oEynw@JT}3*aV5{{qMwO=wY#rR8I`=nU)027(3o_kEF26qCXhr zkA;4KSf)kVd&!8OAq^YIXGTEV>9Mzkd$Dx=U7y?(o2Q7M-C0TsBkEmSz{z*ZF|#f^ znL?T{&AuY=oRHXvr0+#M$=y?cAA8&gTP}^(^vePK+tqIZFgP>i|5L<#@8U{E9g zq>Ib<_jcBH0QVq&tv8KkY&2k-or|ik`K&O%Wue89zPFmpk4s@xg*>G>uv5u~pB&k! z-fs)h>Yim9C&b5rX`rUF1S%^e)XMaz$!k1a;X0)tSGmZD{x|G?Y#-lw?5vlIQr%|u zyvZ&xm&U9^)%U(v?+D@LFnt@BhhEU}+&;E&VpNvT# zjKbU)uOW?YS9{L0Ab6Z*T!eI- zG>U`go9u@#)p4mrCO~t|8JpT1%Ai4*DvQ1pM?@^ghzMPyU)_3}2#$tOYg*)e|Fg^j zEjQ{sA`5jb zkJbvi`|wN_2H~-;0zp%*UidO=M$u;X1FF`~^&^~@<)}pb{(71aI%h`I1?Io*eNGx@ zYNJ}gZt(%j6nhyb)*GK1GzN|z;_W2(h` z;zOwBv>y+hy;&npuuNC`u%x3Hap%L4i{*xdYtXw!5Vqvxir>P*xm&<=v`8XtB}{nO;vB!xfJxAu zMyh~J5!>MkIpeoHET(722aTRh_W{{`OCV$V)hEQ}@-njP-85Oq`GU;9icbP)CsxOsSvxw$t zpBU3T_xKSLA&GZ^pj(hM3p70U3p^N}L)y?U-x)doWDIKGFK$FGGkiK{fwZeVv3)g8 z^&&swVurnQ6a1vjj{}q=nhj9@R4z38(Mu;S4*sl!sy3>ttVjRR*Ayc~tDx>^|Ba|a zy_W)(SgTF*yVS)%z! zTgpZFRY3bIf%-^V0=4m zaIq+CKKhraGFL(}@M~dST*X$Lvy z0=O_M0-`NPIvy_qPK|@z*^6kmb(Q>PQXi0$n696iXJ$9=2e(g}2V^n)k z(Tk=}80@X2Z#a7m_K=6778~5fng>nr1r4i@cu4g=XOutrgUXdBt7~NXZxprWTp(Wk zA)bW9tGN=8ZpZ}V5^F+N0;!OHu+sL*UK~t=yo3MFN)x$c3D^fJkgiF>cLS=mcyvlB z@TyU#Yi1}kqBXAVD|C>q9KDP&wFQKoZ9ort5U>FyPyYpFDH6nApk(#x@v8d%?Nb*G z0y!q;LOizI2&DWFv#8%9E$o5NCgY>I6gyM7MsVs+&VRNyldJdqYH$Kb6riG7pL7MO zGT&^)0OlO?UX4#HPdUT_Of~tS>tclF8h!|%8Bm(U*)mR%Zfk@e+0|)?l}71?3Kq5e zP)mUJOy6&*uNaZDK0g2+?6v}f>qOKH;IKxXy;Kz$a(B{K5QnJp76iIhr~Z4d@1H>Yp8ffb|h$f-7_PL0Cs;3t;Vh6nR=h}cch2-^-b`dN3yzIU?7MKILi#jD1 z?b5fBCG<+}eL?rdY0&~%lyqCA%`}Q%1MNBQ@$9E`rW>NAwsGIOg?6O1T(D{Ed!hB5 zY61yFLqDk9vWy}TGy-yvu*1N!Cr^)3Uol23RKwER-Wf10-~Pd#c~f^V#G{D)jRvm( zP_&lW6H?k5V8LXhi(DQ!#NcC_uXbqqf`OnG5u|K>0SJa}A*+^ztedZHJndvpw2scc z3f@hx^l%j*ys3&mTJNOB_u7~r5RTz3z5*7XL8_ogd&&=->sE_t+=4&UJ`}a{T4q^I zE*S7NdEf)Z&B{-a#h;696*22?iaJ}4;!d1l&jeg4O)~6D-&4xJ^riqFaRKPWVM_ZC zkvLGlmw7uj=Cpr`U0FP?>3-K+?WY0Tk8SC}j8#?s8o%cbElW^N(3Cq+ri!dkW;<@d8N)_~@_rTHDMaWf7#4$?>H#l566;X=43QD6mrw4VMj@wE`eK}G}{ygvo}le?c#Kr?;%Ar;Wb zbK-JR(V#W!+sT5u_ZToT<-F;4@XmtJU1#oyr2xH$@X#&5kOZGci7uw-LpvW{!Edfb z%76?_cE+|v6B?v{uZ96aFOYK1!xi=MAZ^nhY`1<7dt*72*Lo;o|0b3@+VS$gsgTv) z<8UzXADVr!7ynJO{|SVp+}?aKXNOosC}Sf4y#q<5)SbbU7qec>Odg z;9N97Q@)dnLlr|O7;Zo*m={le>5(M(ziF03rLnGmFyFx!8Kl+o;gqBn0qxMa0|w!g zS7j=Ma1PSGI8dfh{kO;OJ=Q(F*9YmA!ixXG)C2h?@ZG40Fx{e^J(L5^Cnd|Q0RWf^xAhLSiO~1Hg;53|U&jaAFP>fr(8+d1%zoeZ#VN{`Pf_1dEPc}JFz<5wV z3Yb_(8S)*T5(z0_{;LFq=^M}mBHCQm+nz}>Q*8tSwL{Uy864N2BbNVq=>Fvv%a>U7 zTsk@U7jGzzukr9QOzi|7fnX6M$1kf^fHg8;A4p%7A`5FbgQl2k*-+9fo<)Bz+zHDDVi_*&->2h@$;q z<|sG~S}x71Sk6$y-^$t&>VWh|ZHYkpr14XfI$G^b zobN%o-9z0p<9ry3zj;gCv6p2T^a{l_Rqe?)?8NPnlB@wLk^7^>}$M!c0){@EWY_5(a>0 zocu2t0P?Re0Jrf5zHHJ3@aSdLq1R_wS@kgRtT2|7z>NJ$K#DF9_$*yx9Q8YW?8<=} z3lwvzTQ8d+;lM%38MC6M3nJ$RlU0S+u0T3We~?KRoQ2^#;5}|Bza;$#QQo4jMI8_x zqSOxbpNl)8IehK1n3Bl@!LIrLg#e2Q|M2uni}>5sT?3{kol9dMWsib$!7=QHJ^aS= zNf%^RZ|(=Y9NcvE1sUz1oaFU3I_TW|6k~ZqT{p;7>&Wx@kN@bSp!hXk`Pbc_s;_%U zS%*u<5a$2l$MH*YMkcX&tpvc}WDfydQ}tJg1S~mFB%=TCFFa(d080AKkGp`|n)Uzp za3jVxxc{Mkt65S7qf!4NzxKL{3~zOpd{ul!pzG`Zn#0SHU&S;Yk)XtA%p+~o;jMfy#!^V*Rb%jhH|L=M9hz<=>B_@-MgG;{CiL+D2RFm6$k&=kmfmI_f4UeJzYpA$k;qyA5#BPAgTVd5dUEx3Fs`pLQJ*HVTj@IU+Q;^t-v3aKcq_>_$cj+ zU;mXCp(x9CES?^&%WHE1a*L>u!I(U*gSPZhZ*}t50 zVSk&6VbNe)_`0f~JCo_JkV?$>tKK{?UrCtebkl9RJGnstvQy(>b`q%lEqhHM*MEeG z)~Me3EBe0gPy7X_+?*u|{t*U1=JlNNIQJYSNvO8f3WL2HHh~{hOfnUOY~8 zk2i5ANE*d`52I1!e(`g_7^FeE@p5`)0=jTsgOycB>lqF!X8<8(Dr=5Ivx)=zo#w44U?-h|&k-E>e#rdawq4*a?4ciu zj6-Py(Mcy@(dPwj_92olR>X3+(&X3ma{Q$dX|CG@8?9jZSkh?;jl2%+biN6t1fAqJ zz7l~2D>?QmlH~Wd=@`J|#p`@p)=x3-LHa1G{*gYQCxHP89|YS2(=NEd;p0?{MktpS5G`s|#w3}5WM>E9SUa8)h2O}@){hqa@H|k|?yoRG>R3KIwhw$s zADMp(eIX$<=!c;6@5%(Op7%lL5o}>upw)?rPJh1GwhJ>s==#5G!G-ZX1C|S@o3HGX zo{ec+#{7cPvUgVwDnH-^b101A06g(u(PWY=!)O|`OP@}^1Z<@qO&pFKbUhrFP-F^X zm#}SAF4(!R=Md@ZOQq8Vz$F#{ynG9^I~Jp>L71{ zV*lP@3$fnT_Fqz<-ku?C0OZ}ojRy|8dzh0a;~(E~94M~Uc9_1s%1_H&0Y2HvI|rUM zJpqnjXxuee=IDf5!qZoz*wz3Ae#BYu*nen6*HSZRVnsiy8Z&t7<0v`ggud^I86)deKoe~F=*^)6(_y=6YY6WoN5DUHJtVJ#$?dxt? zM6-W+eaB$1!gYmDgZ%3x&@-yfX)s=ee-e-*o6yz8d<-B}*$nJ>XKS$AreUPJ^)HHg z2t4sUM%qFBIZd7na*tW0mf3fcr?raw*}7YHW-3{Na?i~ITEM>Re*Ut$C)2ak=C)n! z5&1DOot`rdki;D_M(aTJgL!i&7;EOgcLOj$7jR5rYv;j{lK)2Eta*n;udau#TsGTY{X2nz6B==7ezm56g}Q z&#kg{D+a4l(5=9#>K4WcVk8C&|1`#m3jTXtq~~c*ECZEtu}46E_vbZ(=A>tmg-x?! zb$DlX6){ENDi^tL>1xOu0(QI-y5EPRk^^mBA46L`rj;f#)SZKydkKeIda1TM*o-PP7OJoRCoc zT?iO-CqZgR;YXzbz4^a=ss8YYa!L}30Dc8}9&26O9jbFH0f_n#gf^g4_SAOJx0r{`e*v7 zRws&^P87QFnBXt)AM+vssH>M@$tJsYH;be(H~hX=7(lZB-Oa-2m;+fjP&n_Ao-sP8 zufLr>P!}tLYsV?MvBGcrxr(1)dPFpjqSkCYr-k!x2?y9TMIJd3S=}KG?ljY#Z}4ui zhI3pK$m@-5JExHip)_g3VCs}jjWVpkFKOn?$%OQ*(0cpw1z`D@zdrQPAjdlw=)ALm z%2M4aE#@5B%8`7o2eQMQK)y%`BXlTDdG_HH98%?{Oe3LPHiAwws$nxgTA=bXGu}hq z0tP(5YzJ~u{HFmAfRsy6w7?CkRx-HAZcMraSP&@0=UG184UjrCKq&#DUtx0xFdECL z^w@C=1l;|_zf4FWC<_)Dlmn1ajuzqQNkGI@J9_M--Ga1ng%EX(=MR&B-+b^NW}Z(l zWh$?4TQ)ztNZU2h^b{ab&r%&@4QUZikspD>;=yb%J52!uy>eq~{?P;P!$SwoU%BG| zWr4Ey57;aHVjdN*{u?3Sqp=2DR)Ppf_?v1-_MLbX&ma;D9V>*M74@*-=1$shM;4Q& z&p9>1ugcJoxPst0h}Kzf`ZjK`!F8v{2YaibwS*G>{?O$~Ef{pq8qpVL1wO0zI9khL z!mE|lFW{by(kDh0vlTq+YJ#5^X)vFdBjPr5a=3j0Q%D29im#~k0{QnJS9or8Ap=Z| z&=G~Fxu9we#8m~e#68Fnha7&DO|k1S@}F)nF#tPQ2>qN5KuuzmDTN8-AD#?7EWmUQ z{%r0f-{_o(S*|Qb>qLLa1;o|cT2@hx>ORi%O?}}i-P(UMs)JaPNyz!X{rHU%&&{#p zC4fNf+D6$BTJl)xy#S`aYK60-}Prbjxbk+jjpXYx8@ zn&6O6&BUA^my?;p?J!K1(M6ocdY`=bw??*pv<#W}D-ixZSDjY zDg!3}1A6|{(elAt4FWBpyuYV>}uXhu%8sp%PXJZ?8-NggjbY)R3BxGM(c)w`ge5|3q z9caIGnrQ!UubI&0#(+L-S$L6+CNEw65Nhj}w7m-{J`6&Cw;_N0<&hg3xAzr*VCc@T zIyGiR0Nd=PJ9cvs%sMVS9RqUHY%q~EEvjitYuS85x+GBqc|O4rwTPj3WUi7~_FtHU zB`x7g0BQl)s){W7KaWtJj692&6H%FL1QT(G1iK(L!}Z#a{5_n)>ZkCC~2 zW_i~!3FO)|9Fv1U4hVe6G>6vn-YOa2%5LVtJ)~iT~7LMmoOdqXl*9_ ze$}q?&;sBML0JU)hkQ?}Mg3~mN|Hgp^xL%zGX8k>>l$V69*umkxSD@33i)M443`wh zGaV=H8@0;+P+FTXb}1cbBWBX$^pyv!i^CL)pE!Lr__tv2D|YxVL2iJ2|MKrVm;}caen!WS0_aw&j36Fh)lf+eWFRDc&ngunL&sQA=h zctCbO>ICcHN@UB1P<@pc_S*L-0bYR?JCb{wY1*K~f!ty0i~#aLK6|RMObD4ke(B?j zWp2<_a{h61VxR`|V=|fkZ_wp`jS}anHGm(o3Y~REuoUS6>scvua3Q>vcNwlatAbgv_S_Cc zb(9G_MTyf_#K?wlg=>f*KUKzoj?4(-Digq!Ec|4nqNa^->(ND9(Cyf{Ca(ncUklwj zdR^qRYhvRoc?^*3WxS*e-~-=}9uM>MZ}3v=z&V5#=EI{6bx0hqb^#LOx`Byw?o7XW zs@HF#e%^V6>7yzz^0b_2jG6&uM{d;w;8@*;fWP(-{m2g=fIuRK^tk*vdQcI~fQmpB z2r#|3SCaR2?He_(`EG*>L`~dn^>f1L(EEoPzdAfY`@X){^|?O7fA&EC<7+)Sa<1TzXGZ&uR(YiIlxRtmY63j z<>i?3=tkQMIf3iQ=@1D@lGL;A-af(U*^t5Lw)}Q=I;&wJe+uZiew$BhJLKde1dtjvsy9KH|x zL<)hsn}AZQ{abg}qyBkPxbk5a5MNE2;}md0OImiwq+}0>{Wy(+79%W664QaRdJL*@ zhmZMthE>MRx&1u1%k~f$8<|k8nQbmMxNj<@f{Sg29n^P%Hps^n4AHgCGs>y_`*pkd&+awWsen~SwbDd? zgRgswcCj;CK*v9s8mD38X=%LCw!=ai1f(4uDa%QDDFFK3HE^rP{t*CW#b-vi&|NP^MTIp&?x7FuE#XAmc zteBoJ$R0pu@NibBmbwC1kPPhZGxuG{vYDb!($Q;1G`tql>=5%>Mt&wi)yP5%5hD^& zjWutw-gKyXPK_(6SO*VMJL$lF?q5g1o*$Tqoo_GyLBS^xUr1pc3vn}6ZTAHo#EpO0qwy*tD1Bkw`e zBK&hj_JHzHA8k%{&ZspqWzLdiJ=YaTt`DZ=F;SkDw+e8Gmy2vgI9CI?2m}Dw6@v$e zD(ZcS!>X(e7RH;pEx=)?4M_=9wX7x7l*1chHKqFY{XWr4fUTkLF)PzxcjX!+!r1217p=OB%mo)I0*8GHrS`b=*1otdVX zCwU`6Kgt{dI6%^orpNtmYu^?f2iQ`x6@@&Y6Px-LirmoZKY5Q;9b^=C`H+?eMqHCy zx}XT5`}A0TGhSrl`Zrh{Os2bUB%5?)8oW9Yuz+gC{u01nJRMx%+{`hCtn&-U`E;-? zZePfy5&}gwZ*BZ|S*i@(RExPq$;#bt5z+?h%ikycxN(gn$Dma&NFq7#QWY1{IciY! z_I2|7MV)~qh5n7&?=@t9{6g5vN9+8+{nD`F2q4{6JvFpJS0(*yKqB$WlMaoizwXr= z7Hi6LwsFq`eagE0qDDeKXs+b{F^nvXIXt~Q`3}u;PmceAr{9gZ(9}Raa{ESH&c^0X!kx?C>Oh!~q_p2yCjfl64 zG{V4xv)H@C3tSSSOn{;H>cshvvvi%In_!$Kf2-=jMT=g3yUyEB$~SQjxCgD<`PLua zPw;*80W8^)^<5wBCA&54@vS9edEHUV$4n#a3&sD;fv>XaePE-W8n zG}I%3f#d9@is8i;o#ZmR44#xGi3oQ98c< zl3U5|Otp0?>(h#07=`C%Kc|ggrGi$sC!$17WyLA*b{jD-0&(-4S%i7b_e#d}tS_i_ zYb&&$T`hHo7Rm%>R0U-3wn=y2{;XQ-``vqT+XAS}>RmxyH!d=i@FF>_AZgS5 z&N+PVaWB`0g-=Um{$eqQPi04E$7GCtv z(9`s=z-jhaW4wNsS6vg`?xk#cB#kdN9MI6nYw%gDviPO6P(Vx$Ui@RlNv zyd(?XfC?;Mvv{g=gd*+(esyt*+WBySCVu`@)LB`>I8h_}rFYgpXw^C7;3Uz1WOW)D6-qI4FF+5`$eK<81QgV-40 zwwb(z%t)V#vRl^HKSY{Aqqr#jDAsg{8^om}B zu8#F$OR?_42WhGLVt_9H(vjHJfm*s!E{W>khkqK7jsC}6`oG~vB0$(3&hf=rP;{4$ ziFR3@a)vc)-=%uid3>F(tgQU1*2 z&$RtgOz%+NjNp~cE5G0U9%I_hzQNb|{yXQFIpi3~)I&ov%-;d8W|RemRA)3@*c50u z)6i_(W|ExxuIJ+>boh&5UANxo*MRgd{>0=-I)OVGG?wjCG#CDOfZHR*h3}ABD@L?z zFr`eSzNBT}XSdjQo8%jkbgeGJq?HE#U*{ngqGfd{BUR}Bt_Ke`!SAZWGE(=Olb9c0 z+7{tHPH9sNl#E-S7R%{92fH-_li>|wM815(p#OsJ{;|eChi)MM{E3CYiX<46Z^y?r zxr!-Hr}67R^32X{dH_CTzWKa|I{*ewYg0&Cad|iX+U)z!q>D6tA(@{PsJnWM)_|)! zm0NXOD)a|3JYQ+VPmXxwnFEBBT`jGj7Zv5!}Q{gqCvs4DIh4YdrfaLA>`>H{6+#8#92^Dhthh z-0d{j*-vk1%FKMs&=kpBGP(x6j%4Pd&UYizv?EQR@K{(4nQ@S(pV^(@OV*y;?{7B* zBL!~R=1unELq~2*0|Pp@q#HaXlk#>~b`3E}hkq~7>40uA3Hdzt9mG416j@DVYt{KDvuP!(CZ6R)yr;i7jHJbkZ|Hr0 znqT>zXm9#y=_ZWK1p~qOHLbZJl8V1XH8=vvq4$XzhuLWTcdD1zMB>T_6uD%R`>$Z+ z8ADU$uyfCC&$zVv`9#@6nzM$XU`dV-vdRgvEBv zo24(m`N(V@1$%9Bm=y+0&)c=D$4~OyHeGm3DsZh2Y@PfkZ{L9D)I{&ba>AfM z|0P#B0I%%;#O5o{AE@e=JhO}d5-4)c6J}P+N6H+GG1Koq-^&%2zNC3vvsqu$8B23` z>$ZZ2>;bO+4^u2orJWf&HMe3Grn{7)rp|BkG)B5MY(KqZJl0ThO*K#K_X8HmnBuBU zWof2+3iD9%z=gEme2Bl{_jCdT{9eCB#%L>3cUA5AjuCYNPsO4Rl_V|=Gaff&-1+b@ zJ-`^RW3@;C~W!r~E&Mn)9k^istdOJM86J?TsP4*FIgFzTB0^SECK16omO%k#8z0Q9TBR!ZT>NeOGdgdB#d4Bt1aHlkUAan?xs$ns z_bYCELy;)YUp~)CB9M(wW5V>swo7`Kr|N3Ks8(fwsKe?p8wq+p&7cu{?SlB+B zs=E%T{Zcr5ZiH?A<@~d@UmG`hsO#Lg3vo3&F#id&dZvbxAS>g{A3b6Rop8t60h<1* zA{`LJq_{jeXoJ0JCXKAS0fczh4rK7v()`sWIHlV0!BVx)n8aFv5M*vAYc0C$Nv)Fr z@dRdV!ltJ?zQ3EWoaxO^0^RTAe1k~GBYxbfMj~ui^;PIt3XpIt3H}H_V z2F1nAx^Fp0RtI{LfuhGmf?Nn^YOaRB-2}=8ZMAFBHL1lK$&9y6;Ki-LuRCTamWVDBdIVf#y9K>JTLB@ZK@O~qY=O%PFcxv%>xwwZ9Y zT;OdF)GIvq6L6dO-xrU3BGKilAZAJr`CtU3SZ4HP8pupN(0u>L!)|EI+GIG3V|&)W zS-LB2tS*|i_Vc7!y1n}^tNUiv|L(lTPTAV6U#r5 zjhO~Dcufd4<4{UX`K?dp`k#Fk;$zlo5UMVT`Yw%I{!}j=B=2H!C%ml8i7$pQI*yX( zOeQ6t(ILf^_Jf*&!b49oBwz?L|(7 z?`5>wm5v~()ZA|Atu1KTlz~!pv?qd?Y#13T0x6t+@$+Gk1TcDjKb*VDzqSEmo?4*L z&sQbwaK0siRY-!89HzXWG}}5P=`pdt=rUiLDdgA`-|AA;S*N?3T~~Jo=21{e{AV7; z`5|4;v%CWmcb<{QfD|36cz&<0ejPw8(gfPs@y%z3L=<0zhaDUluOAt7ZGB}y4xUPK z%n|n%+tfO`%BxE5bHIM2EvyRR{7B{1dM-JoxynOEt3FCZ7i5QSlYrAF<~{5{BW9Zc zLBR(bij;I;GrK!AC-pJP)-;ygrmJA~xyZXf79xA~4By29^BNS3M+#uGr23cr#~t33 zQT>3UPAmGroqPHuPU~v`6HQLt48L^!8{p&q-D25{d=}UOIQ>zO4qb6aiaK{OgGgF= zj>jOqy{zNJORyP-EIbj;&78vS=^Xz4f3hRc)+z5T{w?Y5zTM&^M@P;6jP}`s(Y*3Q z-V^)J5SnLMdruu!AxvAJj7iU+3PZbQXQk+sG>YB%T1?62{)3RWCOR1Et3*;%oSC)} ze3lj=)(rT&cdSRuCZ&0`)-T(R-Dm>?RLS8izVZ$DKlr&?-zH}`zry3;GT0<#TN@jTyPhPn~ zxO*bA$`{Y#(4C)_5dwcJsAm^@T|3kIT z^cFSO37_vzUXdMjs`Ig~$1tZI>!80{x9RWw@Qt_nN|dFL>G;G%7_oL9jl+PzlbBiA zw$!AXeKQjs0|j|MAlS@hT?<0|*b-eJ#E(&=qkVK*`kXEV>Qy%6@L&p`*Qf)4nd^dv1FUnM+JJZc96@PU()PBH%-7O z-#}i4o%@MrGm%qlvago#dH^~XA<#J}xfWj?Z8X1@cdVK4o}0M=07Gg=hrh#lroLcB z8$4j_1iqnG5VbO3Q+88|Y70ijNhm$aa8Tt1@X8TqlI;iGSt_Vc&vM@8$C>%AZI-We zlWmbd4b1AI)-Ym9UvUFKF$t!Z)s3SBL_5W#zsT{-zF0mf_iGA)^?Prfy->OGqw3s) zW`JrUlZs&-Ow783JkcZ1Y@wDCX^w8m1cTXy6l%i}IJ=5BF|6%r|6G}Fmg^#%{?aYO z@uS(o6{@}K>lbde#tocom^xR;>#coNT~QN@k{A|jcUAN;J121Rp+|u&)uRIrN7i=$cW-# z5Rohie!bSY#QSyS$`n~TiZw`LOZyOgZeQ$>=75`qNPQ^oKFpp|Av#1VNXpe7u5 z8fS_eqo3+TurY8@iLKqBtt2+gV|MDL@VgCFaY4h&(bcu(Li0R8dSB1uLg)&0F^_B1 zgRr){?wQHq0^ZoDGUN9>5M&mNXU^x1|D6BlYb1+Hi~S$oI=d#gNrh=Quuc-`=`=iR z{#kCkY@=q(6nI;kfX_{L<8)BsPAawspEl)7Gy^4Hha~HrS#H-|ja!}{T%ltKw(};^ z_fCavSqpik_ca~8r>CXuwASn*Je^acS0f>p@=-z&nC4HljxlYx0k?p-slP%pRoxBn zpd^FZ(71Eet-Z^q6|=;G(I(5z>7F;fdhx=c3>?|u3-Y>UcX6Jz;AbnVo$0E1I;(MH z$Mt#G%v)<$Yw7~#zphe^sE_vY!_UZhx9{=sWxa#`)b|hi+@~8gGo*mH@X9wvc~^1w zCAmzV+5XGNjwT=l7i%cx*yxHY!Zv;qywLRjTU zWgsWbRRIfep^+OV*vEb>qjxZ{u4)6(68C;-_kb+7wiK-6SQrv4bGrJ?eHdwzZOM{L7?$OF=F!G zea6JpsKEC4TT%VHGuCpu#9?x)2n1sGYSG{NJLrWB>dcFnr9oJwX^D2iFKYfGz%L}*fThNW zLs+sdQwZ;u);l!ND~LMXQ-c0N-VVZDL)xxNe?#nxlZWz5OReu>dZhKomp}>8K2e_` z%;LQ%>9hEm^zsdVdQ9e-X6R`&Egi0C*aw3RVFDcAci$BtVbj6!cZcBkQ ztSs*-7mFKD1Bv1;kSO%FH%V`FdF10Dhe(@-cfzWCyFEoezlQNPL=GkFT)OURThRm* zAzyX17LxQBJ%>LF{31a#YsjFru>#I&Dxfo$PE_(v2IHGpw9S~Xzm{D5HB>VRooi-R z(3z=#Q=nwmtk#-8cWsv{tyn3nhvFt-y9BNXdd9;D?JZjd#9RKT@harZZmcE4BJHy? z4ef6HDGK7D{_{s#-(;+*kYzuR9HrmE@#X+#{}mD=QQVH?JKP&?4g5g5=W% z_Sn6cC-U>K3f`;}1^nw@Kc+xnYt=c=JXR;y`fFX5%i$~8rwkc2hJXu#y?%#z`ZpIK z@@B0QBCr`!CU@9r55T@p3^~@E7W1Y64(F3LMHvp2yYx+;3#T(>Sfh_IZlZC;o8{)a zYxdvxaO{Sf#s@b_pdCyCaG5x0=-r7UE}$(^#UeK9huj-9JSM>c>vNw|zco|BrH;b$ z?p%0H{&B^O+SSHgv_ZBzvLBrJb-{dBV#tmU3Np!#TQRk+L!zWe)-$S6deQpQLzp#g z637?)*N+u0!S;tKnWF#Q_#zLpLFtg`Sj%&)tC45NX}r~aVU8)3bQYzOqSImE>W*ls z5P!t`&Yv6hJY-p^@EKYCpk;J6M}~$;<7NBkVKS zW$6_Lz5Bt#!%jB#mSM`2(oLbbsUfrPh+3Trz-PZq(=qm<-+TGC^XdN4Ptk%}+zB=0{kufBQJv z2pAM%s;ot_O;e_lHEy3f>f%ph)*6W+2Y6`OMWdY!u3tF$R&iSGsvFQ6l5R!`Pe$7J zv(g9v;@O75V_z5_!z+NbpC&>z4WiB}#TG8m8G3z1kLkpvpZn>p9stiHIfZ`!lCRs~ z_(P^1;{|B?bOM~JZ5IjqALbEhjc|Q<>}$@n5Y%>^1OFzCuAGrWS(1+{aF{c-8o5>f z%Q^zH;Z(x0enQVQ4(iYi!~?8U-%st1I-MMr@EbX<=xpD)d!I7H7Zm>%^BnwY6_OrP zOrW+2$%|>oj~lLBOpR^nL?gcm>@>4}@`GQAC;qAuS(|&Wb)|Zbfr{1lpD&)5jI65naO7OtulO#QfzVY<5<3EBa-th$yvx}jbZ|}LV52dB|oDD#ujjTRu)9W&jRgW9gIEH_5{qkK0hWly+jvnaeSmp3^;w3CLN2aZlPjL-^LwbyHXW+9;1xv1*{z8yJ z%p4`*|Si2xLmgdp@9?cs|1Er_Z!fTK&2uW4Jp>TT3UH%SKWj`-&_<{x))aj3dqK)FZKjK5lB< zRFmfN{gla$^eDsulvW8kxe@JqUeS3b->`m+Of5)n1LbkS`p35b_JY#*0M3jrM~ENs zeOZnR)sU8j84V%YlW;pZyS_Mfc>Bru4CSsMnC_%PZLXU+y|zfyh1-v)N@Z!+kdfxq z(OgRL$B%hxF4nTg=r~QBMoD$b=0CxRH3?Nj_sx&H4+pPa$$bD0xyC~b3gh0FwqSs3 zrL!zj$nU#qAOQZC;KDeIvqF1Z^!&F50@q5(13sdJF0jKjqn6eBG}mQUMM zcRMv}oppQO^_)X8-c*v}?@ckFmT7~;BL&rBIN40+zo;)T{s}gXdW316kavr zaEd4S(hC5%N*}n2uf_nWPFso*_Hx6Nv$oGl)*wu;EvS*d1%Pc`RsOYZ9-nTgBktc zRol2|rNe)6BZybe7a8B2aNha*2r%dxb@?@&K5#pIHK+NE=UnNl z#-Xv4&)1i5iy>!v{~#D^?Lcqj8X}%)=^B?6K^c36#T&pm?W?6Ne3MEdVxY8qTXgit zJn$>56MFbXpubp685s@hMX;jiIrFGp))b<=C-Y<<$~&Ou9&T2z2~1>w@vFcsN=PHw z=^W+yv)b(CfKh0xv5W|y=02){dWv}G&v{Px0kGpqyJsG~oT5BcxPpke*SbWe@# zrIyW8;dBOtMeq=H7#rKw(+QFcYlXhVm{6c+~~{OS1Qm{LnvQM?5rD4UNJ3vQ5~|9g!8q?CNs+@n z925l2(%JVPCr9ataR2hLzbCS}*nhyR{nmQT;E>C1hVsAtt}eirmiB1G75H_jc1YY6jB@$o#4*h6N=@6AW)^d z5mLt;V^3|Km&->oHM8;m}7d?5H+xce4T^7{p3$F>8mYk99jZLk@ zjKIANu#d{2rTBzN2Ca?4%Z#K9C-ninyS4sVD6*m>=wL^7o6|@gHw+l|l8q>_(y8#R z_l=9UgXjPDC;^y40LJ~xp#w|LfpRT;8!QLgbZvIzp0gCh;BAj~3yAQX1?w zVH9qKg%O3v*E?6GZLmiJy!FM8jFg^7B(haAqlGYbMA8>tqZ?k;^W_LNCT8tMb~!+9 z4D# z-H_5^{`!IS4-f8sa!3D5qXRDU{!Wmca4&on$Tzx{SNKQ#xG8X2hgtd8Xz3{aIEr_@ z31jVTTd{T*f@B5d=~;w=peNF&9~;BaY_A$|lD}}Fwsky1HM>`?I3>Mq`dq3_$*-43 z-V&}`OD}}mti7j9=4Qm-@7g)W@d>pB5JAJgN&ZXlTZ@jlUo4(mqlEPoC64XtzZA;YfyIqXfpQm}ed&L!1Qe$9uc1z4-WI^< zdqN$L7J9F}2q4m1c(^d>%_1pqKUPQ=Jeb*TFtuS6U&mkp>eaY#liGU#sp5y(#dTuu z>6#?Q)g*;Y8Nq2f2`mf_SFeSBWHC8KQ8Yzqz40#Q8(N(ep9dCKBrqn^7s4jZyR#Jl zKL2^#J!c;nN_F6UW3Fgb-rW^E>H?Iz=pSe$9qF?H>BjP)<95pCI(3Nh6UKCqqD&$* ztIS9!)smch3~tels|CkuMg`+B+$W3PP!9V4@1Ld5y@L&q9QC*xijLhqKH}!sksGG5 zJ1R?AmEI*FvV_!A=4Kb@U)9b=-g_>0J@Wi3&M&vKrOsIgBPVy0>COz|#HKiDa%_;m zaWi?1Y@@d^Y2=X%O3QLTt_IIP*txqEVl5aqOk>|lP1>3TfAk{0L;3OUo)i+?J*m}o zn9x;nAQqO`R-55%mG|6ot=1R2U>=j7bnYD0VP#faIubZ9IGi2{2IwC93q&-z6)j~# zp#|=%U$<@S5Cq2CzXrcmZ@n63)x5vXs-@suRjK|Y|5Fr`g(Za0XpD*j(xpvULbPNu z`@kE)*ntn51>9X@k5r$@Uf$y_b60t+2Cr!E)%&`aTI&s1kk9d7Nxl^{=GQs`>l1lc zN%|bW3JzzjuekItQL{3wLv`(-qj>G|<9Z)nRnt5!RL9A9V{3UDI5SFUC1PiFhF>w> z&|J>@fKIqAN2xTdW897QbKoq@6q+NUBgFV(%(M!+_#)v$= z20-9a#Y6SaK9}&wi_#9FG-M+MR04NJb1Ob$K3xz@CT!e%%5{t%7b@lCj1{tI-&-cQ z8TLH|zzTukO`v=>Jt|fX8q+fGRo~Fd+Q)11W*sVk)4b%g;>~$~p*(hHtR;DH{?HqE zo^RkTf*yaXJVf`{MbgmRZFiFG7VpoqLZ=k2U**OwiZcD??4IF8)BgglF%rZ>Tty}` z4&?4osFIMF>gESnBu%0Kis+N#YDE6fZe#@rOA0JNzc%RvDe_7&Qh}-{L5hYYc1#$| zB^GAkE96;(fbt33>Nwwp#kEGU!-<>8Z@hX_Zf+)jWsd;vlrAth z7mo=;0(+#*an@!qF;wk#c#yrYjG6LVI%ayJKEL}|)~bY)lUd`HFkbU;IW@+@))p82 z29Da}c+EXu^GBSS&K!VC3vV+In#X>vgHd$ZSJ7|?J*rm(Rxq@*zxxn$9u&Dv zsAZz#H`KCQU?cypk9?FZf@cViyj9fwZeypi{S5b>6+lqRdFw||@?Xd;c`(pkuS-USwGnT-C1(@mOyImQK4fmgK44ciN*sY~} zc(jsXmy;a4D%iyt{T7CDOFKj@E&5g&4*(DNnujV-(UEJFZN7pK|nQ;=g!U z&Gn+7dS#&+0)L5={#JC*Yfi<-C@rISdDyprlo#5D&xhi;%Ne*dlCRsOwP8H1rkceQ ziMKSYy<@uR*_zE zO*{$KW&=gyr}}0=4Uhz(Rr?nq?{!=Nkw^-rGiwEj3RFVE(^HpH+%$7c;!xOpABFMS zciX>}C)Qi&TC3H)uH>wNr7KN8rRs`=$}dB5!XwD(6f5~@G32DC#dgLmdQnG;MPSiZ z5p{#J$jx`?&l29YwFX*CtX+b=yBiQkAbzGfLtlyqv=%w7iZUvZf_tP|BQ6^Fq|F1+ zmju$>(8-Q97co^HM^(OVsAJm5B*f%8YN>2dpI3|-T$K*u z$V2W#SJAv3vV@N&bf<1+PO!M>;_Lt}=7#$78MB^IEgZa{^rgx6rL@q33JgBjy>JIm zE+oPQ=3lD73YtL7;jvtux9BTKgoM-d8~o`@D_$w!w-|ed4|%>$f@GyxM7o~Ok?OMI zd*+ia0O_#%Gxg092xTyDMn;3E=cO7-aS!kCybXJWeyu|PV38ic-OW9QUF2}q{M{+f zvi#+-4O9Ny2#`x|lP=~cvipc(vns*t6yQB;C?$_c4ziVd5Kv#;MXro98O0ubqi<_- ze@{pjEe5}5yt60+?5$_U$%2x2s7fz>;2*dapwA_SZo(TKbotn}w2nodEIL7l_X113 zwu^y)@VqK3P-6p^h-Ul~v0!t+W`z&CfINaVS!Z^BQClUJes-X=)Cp25q-8&ufqZ$@ zFmVMmBq5XFD&6dg0@9rx@4}r)|5+SZVD$K+-WMX|e=(3K(Do7@(fJf$rt~DNxa-`U zni6r^qQa*CWBH=s52chWe5d*N_|C>%;m`iMxY^!R{p-hy=pEka8ys^TmKR<%Zno^< zQc5;IAB4o5!{8s_o3c?av2^PTt;&?0E7vH{0AxXNZ4ozqHwE)$&ctAOZfFveH|>mw zdh1Wi1K-_%i$U9zV+j+R55-shWal!E194-BFxZU(WE&LckzUcx$@a4VP0;I^B}7C) zyrzJCQrVlJ?2K-3e#xRZpW_eiJ%n%3*ma~XA(s*z8a7MUd&cuictx#+&*Jw5K;d70 z(MHUG^I?(RYZLIO$;1Pj>K(0o0?%?RRkEtwGH69Fz6unK*s9bIgJB`g!Flq0*Z)pf*;q`1MIv-1p|V?h-6vW5&9n3aC^ zW=>m|NXUogTYm7vPECojx-;0|8XC(*ivfn6$uuFKZ0#wNdZj3P6bq!becPnA{~9%W zZb6snz||2r5eMrLyiDoyQy-_Up?7()23}?CVtU7-=Z6g_MCZhhi-}8Iy|)a~3{`>o zHFz79fU}xJ3tY{#mRxg9yXNa>6Jh&v$7gVU(m7Vc^q8~jS;*`{b93?eD3>2kb*N({ zEj!X8HEw%SURr`kJar+UUa!*6B`U=yp<#j;|B6|<6zo>4>GOO-I_K>e%v4Je;{=JZ zvY#TQ))2lk?)4a*vxWLk{(+M$^RCWZ;4p_ZAW|+wi2y^|w?lN5|8f-lW4-Y|t&%63 zqM*?i<4A!qIEv?NJ{07{_EgPeSR>0J7S1=2u_G6V^saZu@pOi#vr@lcXW?m&frf!$ z5wi7K=`WrvVe-iXwwhD|GlxKXmiH0c65$abqlmE;S-=3d%-*YMQ!DBM(^Umkqzv(!+ACDCm5=*T5{OnWExY2Fop2vlTqP)-J!JJ%?E{7nO|3AiwII2-;S<> z5es8_V>d5awN{7bWmyNb$iz|pWRnP;@@3yC0SQ*+WO&!dV1I~)!5_}0{p&u7O}^jf z$1js{r@y>{R={oD!O?g?t8pg%IqfxK@ z1zT6`>t7)zb7eU{_l%*M;%&%#eqv2xF5ymk8s8Cs-Ei;(qa1k~w{KPXSZYrzk*!44 z2sg?nG4;fl9-{JjKF;kf z4x9KNGkD~x@h{w#(^G0OlKzUzPc9lYp4dpQDqH06;{DxHtGLbftWN@iR;}}k=S2t@ z0e^+}EF`z5_@vL$NOt~bq{v)10fjhcoFrkIQ;cbeI<5L*0F9Tw#_OduO6D@(p$2b) z^?#)G$y;oG1Vwx=6IeI5!Ir>hGVbKi%gitSicwxxiXH^pyO9>*G!K z{P6YT*(-4R^KTlC(8p7+f(qMFaf4U}u?Y1m{F-*7bPq~+4#w5~{v6;B0Yz|S2j>O& zOZTAj!ExlBZd3ZH?RUscqBL6fIvGnhLM>DD>>?!6Irqw>rJvflnsutLE4Mlyi~dqK zsZt+czj)s;gm!80zhAdIzj3KKvjN ztN7Hs?FogxYsHT-fBXBNFT&M=qvBH8mJ&c`=czJ>N!~?2nBUX_M$>sbEDr+)n$m}@ zvVSpNJC4G8Yu$dyIPP^j-{$o4l5j^JQTp(JRJnYb2u_f>=uHUTKjkmj>&gQe!bK9m z*Nr4Z6PVLWV{DPp|IUVc>2~@$ED6xHB901-WV-27bcXj`0=}+FJ1XY)u|qW)$%+@L zGz>Qg`aPp3t=W>o|H(3&fYVs>4Eho$`=V&kepx80Ke1zI+9v#Vlyl7QII_O2l0W-WT=tzfoMG0t3DQPgkgR2%?n zzIW%qZqB36u`j)u2s5w?>qE!x^RU+&URut&xO-AXH~%`9mhJ%06r4a~)W~0MI38gv zec-_2q=&RYf3_&JBWnUBYC{UrMFGP{2VFBCvU<)fmxLGxk*cBD3D3R@A&-a{I}ro@ zgywC-BpF9=LQzH;U7&-EPDY`Cll{)KDzLx(BN3)h>gPWNB(!1PU-`p)sbZbS=ZE3u z+iUK)+0^?a{^e27AiFZ%^+!dL22a=GoYeET-Au>SYDJiK93>ob2xe170$Y_1ZTrZ5 zprY>>kJ6{M36Q+KA<7uRx3XQ2yZUy+d(&o?5&Xzmm#Sic(;G%Kd#v$ebAkh=Iq*`Klzg^ z2n2biLTstDuo@neIWWgFZ+smmoLx5c+?M}xR4D4kJ~W(DP1htmec$_&M;Q)pA490R z+G}!0X+U)q9wAMwUGe(Ip$1mI!xB&CZLy8rqx90abOub}cVhA~&{{I*G)dAWB6X-7 zYG4R6D0HLB_XJP|t+4!wkGV!oW#)f`YG~=Y`=PC++|AOui?zY+=gm(S>i}0;-{hWq zYd^<$Lx0sH$cG~1os}G&_GeGHiddEVIh3qoqB}Yc25;=+3{w-c`B_G(6^4D>P|9nOb4eylsP+WZdiu6&7 z_o%&a^|bOH@ZQ`6Vzk|Us0^wI&sqiE%uM_3So8xwL%s$9MCqqJ;!~eziQxZ*sD>zb zAGT<}8Y{qk&&V7QOdjhG#rCOxO}vhC9M8{7ML%X=$9vWKfBcwmG7I_>rs3;^GJU0Z z1=vMTRdDasElD_q%yHlx^w<^Jg=uCF=0gI_Z=n|?i0P4y1OtQ4y`k0BqsY6V4g7sz zDYj2wHKbG*5oyUxHX%z1__e+jpbd5Kw)e<0W_L3nvMw1a8U7n-&Kz#>S|f1gDll@K zA*#QQHbFf9*}%9$pyHr%|IcF|@BjPQ-wsdk9P(;YA=-j&R|WPwZHO>;C=c*>B(*Sk z>q(_IX~x%@Z}N4~Foj#O1N}bmd`}VA!M*jZbKCBBM0DQfRAYj%x`okbPSlJl%JJ3F z<<|KbVgI%FZ@nJjDehVAGHIJy?XF;0#o*}(k0YBMvfBXGk+BsJ1xhwC_nGjW;#uDa zEza-5_RtL$59$%iZL?g7Y{Z2+e3X4#{HJrI!f$sGHlP|I4FN9Wq9 zT9_&SvMOYx$2mbcXaiDq$ZkM^`l#-3@r5{X#d!H2MCf zN3kRDbP68jOu?7?*CP842)@Pyu=*xi)*ab1NmzlV*m%`DX*x_8+itVV zL+r6dphI=+$p8$@RDo`nz3TPAbdYLjq*UgQtH*()V-g;TbPdQL%dqPB56gt|0rZ>vWimL1ng+~Qn^Nx$X8ig}{ScXT1gdg`NFZ`rdbY%pupcAZ95OeZ+kV zNb85eUAn>?-AzCl(oc9tW1S`wSKX}O?PpXb%WpX?BpmDIKAlmS z&4GOxkc$ukxucfE6+Zz$ST4~?Q!-m{*dxq3K*?gnDR zk9%8*-xhl&+3iGe!TD>5TFuF3a*0h)3yoLVc@MLA)AL0fp5y*vTerXb^htVG@T&Yt zEGk62$CYT)I~%F8oo4d!tpizi74Ou3BlP)3>}a=A23>?1s=!ty<;&ElHF)TRdQxlE zLY)pi9B?75N)+#`*sBZFGNAsZVv0|=h28hhwM8FV#ZZ5a66MGEk_{eeJ!HzkBuFDG z2t5B}pvPSQ2Jn}G7vYNm$CebY!;ER>D6(Ek1NqKOK9@P)w97OT3(g{wu%Bexm_+O~ z?*|nwC&Za)>GtaJoQz{?K0`9e6is7XtZ?F^XttNpYxe5z2(KXN?L77%c!(@U_Bu&n zC;Nki88XYmASApPu^f3_BH-|e0Oop^%qt%xSMSFjkm>@g<(`vWidd%0LX(un72bIU z@YyKXT%BU0+5#7%#zpy1^}AsgA7a$I&UoB}JzzXLpXicB;kS26U(V*&1N_W%XcJLY zWc9Ol@N%Ff;HqKWfoNgH#2Bn~1&zsg<68rLFWuDaW@%#6fChz74HMQyxSolWJ&NUH z4byz2I|(|P?Mb2^>DmHEIMY;-uI)HBJ=t*dFN5gHB;qACRK8ro^RH{AcCL4|1y~CG zoRVQraihYI1YDN$KgK0WCvHXb-v=SWh3H<%13W*#Ldf6guj+lTdbEtGK$L}7l-;8r zd=V1L68y7diEftMdSWeZ!<^%wF;{MgRxQH5J>mxpH!`e1SZEwXyBDv-d{ThJBH}t! z9Zms`{HK>1j@)S(5i3}Py3KQR22`NYR%1D=z#UKoT>h$r_JwjxH^4C&n+K!aHQ2#S zr=<<+qu%D^$qJIhBoxOBaAvL!x{78s{EQY+|j6Jb-=P@lisq-!1X zF;?m5&nXX9`_nc!|HkSan9H|ov=dQY` z=<&K{s?G*kY_c;9=} z%fS_cZy|>{Zn6kZJ%k^dRO0bUD=;g{{WS!>+`)| z+U4B(@+$pX*s0{LV;f#6*00?%Rt|O<%99E&|A7QIn8QprOL38Rl<$`}${ni?jVP#Av8H;y0nOH#!3|h1h!S z#tpI$jSkYiJJHM^c|^$kGRH~g@KsCqHO)i~8yBRL394?}GXYz~+*3v)b>>G8Me;CO zNiGNwM}PWMq?%YxLq677^hfkkmSSopSsB+pqM>(JKBk>nh#{?b5wK7(8+61KuAHMQ zANmyJHLJ|)8lju-nCH&%bLt=BnM+hSiJ}yl`7JoL+K{_#AX4m0mg@O3)a?2V=}%*-Cxs zKyxdf!|x{heGsgEjWgc6(-!373ZMwTQr(DpoB)Bzgo~efak(#X)znj{MMPxW`iIO0 zt%>oklcs<{Pm1hcS=%aa<@H!x-XLu1Yi(Zc%IL*9ZlD9)pSL@ql^s)pSl~mR(mYH> z?VZgd{6ii`r44lrC354Zn7;~~j4oXsCgHIit5eJ_NqmyC3X6=zK<^LWVPPBg^qA-} z@;J-wyRT_Z&FOYR-h(P-5x($u5uZEIhMhsXA4Di^vDc5fae6#jG&3x86RO9k8J7M> zaK{JV9LZ3Xz$0eS_lfm6W-!YO&>akDt~!@%_&kNzc4So%RLjRoy!W%}GSN$s3<-SZ z5>D$l>tWq1_AHV0z852W2^)P&Zl0wK@QhsA;M+4p2g2>_welAU6FrvlXKDP+Hh}$m z%T+b&aj9XZxLwCUKGA&V99X;2x2X&Jz7m!bCael8|%!Bj|lf{gyZa{eWF&r*@ zoc&Ipxf`WkOWdI~ySKrmazfVUhr0@1k43mIPE+ygUXD4&Q-#?lQFzEEm)l3-F_w-+ zktlFTmXvqgiuBn@WhOik8N-b_b8ME!-09F;q6DJ3=(6zIbW9o;&k5PC-$pr~Y~HWl zb&i=jPhee^Z%R2B4#~COBpIr{geR2=QQvFaVNLphwR#g#8iC6ePWC$G@*f``9LY-w zn6qKbl{YysL{Ngj+?5V|$QW>E8M?&5%0!R4-4Pql)vFIPxq4&+k=k4KCGT{Hr{X3gM?}5#-D`6nU2qfD*FoJd;UMj$YsduD z;Mv@f5->*=FeUNG2IiU^qo-bg7*`0|i^JoV@53xQx3VzNLiyTRKAgw&9Zl66o~0KLYWLfj(!p$FiTtzW62;u(!gGej7de z%wDMsZ7~8pNUEux86MrS*)H{tZh=mJx98Mykji28eyyoO3-9;l(g_`Fl5U6Va-Z=G zz|D|3#2}!|cjf-ob^5|*wr0^YQ<~xpTChK|9WjIbk)s??v2+@V=PI;+;Znv-^sL3? z2E`!HnVh`l+zIePb61zc&O>i0CkExs+`ck<%tG5Xi9|d3@;M$>L=_X>b^|ImqnsnB z4=dCHx6l4aS;oUb;K-+T7yFV4!y=c*X7t(stG9#)GxH8b;^-5_r*&R^Wp%jj!$NW4 zX8bVU{SM%zGQ34P>xpaD6MFN8iJM*j!K#-PL?+*FHlvb?!j2P0(de*8A{Bf21(;^Z0+-`|fzE`~Lq( zIgvWb$jnGlaw?gL?3F|zGD=37$I8ea5h9}`Co+;Pd&Ci$#gV-;P8@r5>|^}iA8B28 z*LC05{r&#&d;ES^kN)uR_&DP|UhDaMJzr(wjLMd}l{Q15eDz8o??|vHME;>ZL#@_x zZTy@VxOER!R(XR;y8-z&&%;p%Rx_sw^2h8TSKp5h+(tG{*Lfy*TWH*lj3;5gnBEIV0)d=)K@6oRC;4M@JH5a}Nr1$4TB-7WD? zW|lmC$DXNmS#pQweVuFurTTV+H)(#+sp)|J!Zvyic1QKyfX~S-%1noVafA&M_{oWFOMMjF-3!ANCPD>P#Lt@Wysb)r9|*_iS#`e(^)N_*-W1 zov&Gm+@vCjLk=MfFDI~7SS1Q!_+`rjj-~vzjT5zHy#+;;qlpg3 z&*V}>i7C$SiJH{#=PnD@-$NmKLdtD1mRh&qHb#2SU}1k52#)D5LKNB3!-x~+c%`8I z=2rQFsxNUT^Ih@XneC#=pjg?b!Li?XN%y#E`;{^t%0c)V^3xJ%9@Bibm)~q31OG+Y zOYO%6n{iNiqAT%YIi8z8wNftZ600-6i(Z++rxPP~tZTsPx5#Q&acZJipZbzbWJrhv z)k@r@>R8RH_G%rijSKA+d6XBzQhn4bfx_VN1>1rTBB9s^S=G7X#L@>}z?ppuXO?cD zbXP==(a1(IWn@CYdY#Zd*{H`(ka}6Zeu!vhflu(+^b{jO&@Vx;Sl9J1Cjh=rQSc1| zBD_JURZh_rls6{kJM+Asu=yaRyjZOd8AcjDQIW$xE~wwb%GlHdcMe&ke(AQkF%G;C zxAd5C!yYz|dAdQr-s-L(P6xW7XDNNg6v*k%pb;+pQJgCvkB$QcZ!W%>&a8OM#T$w_ zBQ1KxJTnYXAjDy8ukL6PYQyX_ZO%ZBWAjFqn8|8gz7p*;&gBw#|AP$^S`2>2zd?-? z{{0awE{za zq_Pmm+_jairpto0rq^?;9=Q*7MlrmAPfz=!QGF*nGU{CWE^=9E#e0Oo{Nrd)Z=?RUm!Fz!M}&%=6tOAS`WyN{mDdmp^S87bv712xTAcq9E5 z{Fo&@uKB~lfizAxymig_5^T^yB&=SBcqDC@{PIc4(?{kux~doM-}dEHPwJq&F(5VZ3fLPeQkBx@1Xc-Wn5tvvMVCqZ8<#^}^fLf&%}W$ZA9eK6O=D2kxC(I|p@utG*NYZul}442F!FeB z%i^Ov(q6O0logxFl?u00dIp+L0pTjz5)vkXve}qdKngAEnX%cep?bu*`OUY_d9=?OLa(Sp%+c58PqaOc-(heV(K z1H~0~WN!-dXYENc($fgObnI(4p#WPm10eA}J77)D zO83UUjs#phggR70$q3ls2Sol-nkcjE2anD9#E7o+N}B2vK+5Wxi>YatP}dld4B{nb zsYR@)$3?XPzlU!GMaf+4AgaI3({t_ok){i597e^T_DFYW=-ucprCepo;6>GqDri)TR4f6^d!pbR1>b(`fh*z$*_ zc}6?|R28>{oZV7~{ji4tPuxgH%2E8X9?%{=Au5|sZJ;AA1QL2`?>T@F<&IDu89j)& z6hd@NM;ibZ@eZyIfJ|@7BP#U~+%+vwLOzib-I!yL7nI#D(7ycfST)RQf7H=b>iYd` z$J&Xb7%#KX9BYgbFvoX?MI$QPE?-Tq|HcIXIx6ib1j!XM_qRl;#fdi?0`pjhFQ1E}L@=7_l=+=07Yay}+cWrj zzX!3zF0wPkKpni~xNZsOJMxoMQk8?+=cr(x1#cZdd}_emVXs@!1y}uLL_&UTT8W0x zOqk=IVlZ1v;F2Slt)=T}F}3A~W+4WgY#LiO{VtVP{j6HyJ)27bLVPGNw~! zB_0SVA=&o(IYG6`1^ZDDZMQ)=7w}yET^nbh8DNWNdu}pPWqBH#@mK?;RKk;WlDAeJrP|Qo6NzowOIZU!0ED=wcg% zfF{~#>ySG6v}Zuni3o)gAlQS@;iF(G;?KCnzY4G9z`RRJD(S5Kq|P8t0yS{M^$PwY z6nD&kFf$ug`haSHzH^&DrRH2H)16Ntt7kPwyy4Lo#r=c^s<@>-r-jD?M-WZ3Hg|wA zPLxd7eUL*fo2B%)?od2;b}gGQ#VvLD0Fu|wCk5(uPEepVn{NB<7N>$$dxkoPpr~5r zLg9#^77oX z^0)o$S5a-B&$S#~V;}e6Q`HRR`N&UKjSg1Kt?fR2KxLBCLDU^dE`HpRDHf9F*HECa zAo*guig{#Qr`1o$tBx?k35&v(;U|>jGm;|_L6IKI{y2^V{+b*fg(s)9KSYncGeUqa z2`BG*+D2WON-4t4clWuL`TfU$UiWKFDx5R>b)x3$#tpQMHt>fb%uKr&NRWuxlXWX> zYRgcMj?#eUfwa&Ljr6%FSDl(*SF4$~yZr&TaC`)AL+P=XQdsk~_>KwlGy8A8+r^*3 zV(658q{g>isZck|Lm%j$zC1fKN_#OYnOJsD?0_hd3SDvs<(^=wqC}SG!u)hM_HsH* z?|L=A+OtNo&UWs}2Rrf}`RO(ctY$ak!LEn*c1<(DUjrVUM8kn7gV%fLmM$hdkX^N< zia3h{jv)fm>h7r|*f|>Zsq@N*4zqp&0MjyCCs+9~R!+&e6|TQ*+eq)NA(PI+lPAMu9!sEDeJv?BRYoZwKJHh?N+N5=O!H|7fTH_x+o5ATqFFpvO zJ?~%@E}D5j<()vEZJ34s)ffl52<|%DKnM8;5y$fsvPE^@C&b&b%(9u3m-JRC+SK0x zt&Asm10w1i_S}zfJSt4WMKQ;g+Pw^)LV|8>wNkNTHTNTg!BE&VVd@0!st;CFQb{P# zB=NEsGy*p_2|3jH63;CG@Q!nWR>kHJ!XfrN$(@6Hf@w?YVEa4YemHq>#XE+QBixlT zxc^lLIYKV1U)F)rf0Pz~hB9);sUU+^GG{WpxRfB7Gcn>}1O1KqI@%Ww@;A?`)BAy4JpIB<`#-={7cG_jhV z-rY~+VBxVmblo{xzJ3WWf9!eqN@*~KF$93#fx)9M3+T-1?3=5VddAi)a#CAT`(olw znN`~aEJhe{VDXU_ou@Dv`a@Vq*|JVEvlN%enr%jaY-6ZqKVexXeeUH-ao~OiCQ8F#&v1;OYBlKZ3t50srq{K0X|9=cDF$Ue zDK#j|!UZgd8=NF#E6=K9HAOEooq=BnJj-!j&#bbNH^G{5 zEZ=oP@Gwv%Hr&|F$`DDDjlo>P>}s5()v*o1+ePW>MNANbw!}ME#&3ai%!y|VykX&> zCA^*}YDe~J)p>e!ur-(n7J=Ese4$(RsB3k%ogE3Z(%8&)nE`O_Fi(=v1F38EWK72qTJHKCdUOb_C=HV&x0GO{!$)selRtZd!NH1_dj(BMJwC-)Th9oXX zJxY^II*mL|1^_?LRhgv5Z2?YK2CH(gVj8^IZoni}6RAYqPJa4zMep79-IsD&1Mv;t ziLDSZHmwhx;_O4MUdxE6l$KJ6yC8Q-^xU9*9n4}a5bwqMR4u^0#(oe~=#r;yI{PI_ zw`OqV`^_2{(V%2ey&@5IOgeyOB5b;kjQ>bNyU?HI2gOq1nfIR^`^Q%FfdSqB z)U$Rt&1Tx%J_a0at@B(N*h0n}t`7bUTe1APc)i|Z^)gguJF*_6!y7_g676dIAjc8i zI~t%wU01JId-SnrOHVzm%1~#$30ZSolxe|}%mE>~J0HWUhKuE_^K}YdPaU9)j<#&K zBC2y8l$^asg*?+0+l*IwB`rIDUUh*$)w3kZUg2_EJ=64WyY0^)^GGt065?olC6gfc z#V5E?(O61{WvD=19_^QTH-n`cE6t>PHZrF^wX2fcpkz|Qsn+0#Z?f1cP=si${=i-` zkg7hbe<1E`+;x@>_TyqYEb37a_6{r2c1FU@#`?E*_=6zLX_OYR&Z2(x#K0a(MoOMB zPgp!4$~BCkkua7s2N)VtR@AI{S@><*R?(N3=R9s}9DHt49Z8kJ`i1@F8SghH5`t(+ z$~tp0%pTv)2t&k4GyQk;s_>{UKssRz2fLCzLI7ph2v#)WJUEtN9?gd)aXHj@#4X}&r>6Shs6Roe;8z5ew0X4rSup`)Iz z1X6|`8OBUS+2IgtsO!F5VjINeeJkVW`U+GNjX%!9!>+~>cMdL3L3ud&Vp#HebKpog zJjm*XIm^b7G$mU+rH?nKrBpttE>t05)Qk409?Hf>j&2TLHkPKtaPj9ISE-!@fF zsHeoyX5E@91Y!@NqKi}ELB$R#!E#%KN7;r!9|qk4Y}NVZ33hrHfJ~$~4zgB>vdvjl zi!^uL!W=?W-;Zz?L{O^O!|D7v0T)x;aglsT-zln(yrbaI&}R`#YaT6_vZ}X(Pptxs zqjT9XJ1Rk5(pG%_geogmd%$+i@u;}dUB(HOhL{jn#{Oi^JpPeEJT%6}a<62$k+>QC z#U}~ypsVe*^ZIXs9>sM^t$R{L8(&POrVKh;34-$H8U8a`Hj6oTuBYqy4Lbx$h)W^R zF-mm=?mg!c^?H_5badLPeRBsK&{E~Rtw71@{Y8TYS_TTG@s-?DW;qdjXzqKe_$prN zpnCCgUd>glx@QPyrlF$>PUtawF;Fur^~c`Vo_45nZPB-HjxobFj;FRSTX`SIxJPA{ zSjl&*=jsD2De|g&9h-Z+Ykg72>oBSctC-UrH?7T#(92P^386Lphk-6vrayUdUBlc} zBle)=Nhh6zj)Ha&qp?Z|@(&#emQv6yGb$0(=rfb4ZrWQ&nn-?z_bJ6k!Ar+Yv1ttj zW=ry9gZ!QrF+GBBH0Ls_T)&E1e&^!-s&%>b>mm<2AEaCV9usk2@`{l@KU8VREt4``xd-v``_{^vUn$^rA+w-76@niqkT@dIrMfBLLiUQXp6LyU*|W zIFh{C9?J_;t{YE59CLy)A9=GX^jAS?N7;Xz%(lSGBD(R--Vk*rQuN!n}TDb2S zx!eMGxSb)2GOT8YS`>m`xtkZSy@W(&<0^rsYuveV< zbH)qTP1icz^`$xi3oCzEfNl9VyAD-Qdg)Pj`DBmKQTt2#;6H!jfOOzcWk9xtp@bVlh>c{zhz4iJ($}30d^Te}{hJGQLV;11C#)AaXw+j+KmEGMgTZI^ z)aIN8V7$h{Doe%=gv`9Se1}r@OG3HK-4xS^wHC*PFa{hzF7w)#*Tli z^R&HGQk1G942s0;$W}x+T=rf9xc%ZXlKEV7XFSUphA7TZjsV`E`qN_*EfR%bD>}IE zCH8mXs}lVnS<#ypwC~rNMOmJ7lYcc>19qXiy5NXVKWNGo?g{3qJ?m~e|7{m)!@aT8 zmmd+mziSYh)en+oRTFCiSHZD_Fa@1sRZ}KIPT*YRon{q^(b`)4^;NP#!2?9H>esZj2rWRKf*2E}j0nF$r*H@3vyJRa$p?XM2YQYuauC z1t0_Zqg)>NfPoc%;0S*Gb*2E|iM(rXdZPw187}Q?%cEf#m5H$b5hhax&5tm=CTsR<# zIa5!bm9cAQeb>0(vadV(Eiw6MJU8;L&@{vHrHZ2g9|Jk^R{(pUNO;Q54E0;W>Tk}d z#Q+fZtamH9-ZWV99~tYfHav+2XChHJW&mpa?Kod{b0;tW{`o48Dhxuh?WF9p3zw-e z`z-+>62Rh5ES`yeZ~_ZV*%m}qm%qYq+J3PC2ro7s^nE zk}Kae2gL!#axlb%XPLH^#{m_PUC3*Q1# zgt#3JP2cYBZ&`Yh+W@hr$#VqB&rOJ)Ip&Sj5SuG?CzDDZupiBKx=eB2qb@(!NI=0as6OW;`=z|78ENZmXnB`1#rotO6QkT zx_H;Cqb-F`R6tB&^=mhMQYOfoXz_gdhUzA^uxEacsx?1x7HSL#>wp~ldBjfqR+GJ! z2oRAgX7YwfMPNk@{LR9D#wMY6$B(hcth80Y31PHbWG7ItSPk_xsa$S#uGIRYlXg_}8^`%zObLQ_=? zy8!(okz4Z@z7|^$Hqkk$@My*{(AJ%L zNobhL$0j_-KNl`)_@3%{22%yi`?nrD6>+dPt8Eay{4Pj;n`cA19RLptu`VJ28$2`# zD*lO2uIWEWXjUCC^Cj?x^I)=P-e_FFlMzj@862?4qteiD%5;D`pj3O(CTLBG z^ZnFi{js(WK<7?%#BfwzH7OD0ou^7q+yN|jZ2{=5jys1W-UAH0lP=LT_stiZv8hIg zx}tWcWelyvCLqTq1Cez5KB{&0^+K+(1s8yLEfhZs1x4x{(5rjh^6BYefSg}})9H|} zO6M*CR-m?tHQ*JefXS(^>%GDDY?y(13dDh51?a!sm0m0ok*X#W#F!i$d_i0a5A-PB ztCmA%X`xsyBNHSiN_SXAB8he^?K3<6_9(Mi#@oag2zY7$-afYGFSV2Xs|~2y%i;ia35AbXNqdCLY0&Q{IIDRlhiX4#h(m zh6o|F)?z5Q9|-S$D{S>xtQqKYC(}=$6bW0`x7jf+(V5Pt=e%3R4jOXn_nLnf`V^4s z&*t8%IpIZx6@0}sVelSAlbO??AEF-dQVXlhkE9bDxd$Fk2D8Lf1ga8Yep#KjL)Bc@ z=46>DEHLVag>93Bfp*>d6bu(@Z|-mo*_9rey<-;LQ}9Pu|g-u>nS7z;~XgmU4RB#62N5B_0h)9?Vjk;UV&z&``q3z$l5a~$65xo>XHHf zPfh^Ll6YPW6f^R6Je@c8i_y@rd+d4s;LW|1bo$oWl{6Km4-s9N6U#iH1bwpdnKfwa zFcm*sRZold3U7cU(LHC%Ku^#ro}0Dg$@v!F2Hj8qeoHJXMbiZ*xk=e#i%+ey8J>H3 zKxDnFGE+4RUmm0jG267)&fR^`N&x; zJE~S?vOA^RhRk&RIynVr=)|zS7o{5XuT%hBPhElQ zp)lN^l_gP83+4;&0F?q`+P5no04}7fN#dM0H|UJ>q0y=}b(VX#x10i}5=AUdtGOTe zGL2G?Awaxn3bYEE10;bAXxzL`UUy_y+vLqpfV-E`rn`!!v_bpLP61&)-CI{)KOiAK zCHl^ZHDCun>H^DjN2cCbHwgLO(PW%k?t>acv(Qh^N!iVNm>#5@8}rPa2P<)1X)}q> zHLU?qVg^NA-#G$-T{4x?YnD`gy|c!>S6;^wo9=GqZFMBaBy4vC(ZH`eNjAW4O-Jo#Y0or2rG(=;Jd@WC*S_T?J~*&G02Bk#qP z^hq!bL0E#GK@6}qPfr^53Fu>JJ$Xg~@FS_zQb=gD<+Jq=S?1(akMhx`cvBwM?49lh zr+3y(QsmLIE{L*uf!Mes+a@_R?*ZS&5n~uy5W!SomYxl4bgF4ak&Go{?Qt8xL3Qk_ zpy6-6fmOMJJ#^q37od3HnSdtC7qY?E6nfs7k)GkBg*AYK1}SSg?h(i>_#fSyr`OlX zngU1mDz9~{?TptF!v?j-&}FmE7?J>SS9B#h=2lzNtM!``Xb^V8=`_Gxj!4!fP9wwj#Sqv8#}nfg43AyrI@Z;0_GYGEBrcptCT@ymFM zh*Li8=9eUA_;3^h#-61cb%4Ec9gX!(0VHp!R|fZY_%!SgY;gdyz=i#(w>nfrm6<3} znpJ8r+3Qm8fa6LRW}1h_WU{+b@&)8cC-7r@?hE?QkG1)S!=w2`VF`jZ-Nw#Fy@znn zaH*EJ*wtkKQQO`30JA%!SY4K(fwrkbo0PFZ`t9td)0&Hrl%I=wX@hRwO#Hml=SYP2 z#RLyo5@`0(ybMjk&Kf&nae5pdR=618P7Cz$1DwRI_FQtx`>g09$sQ>%FG&Q_;zF>| zeiwJ&@K9ocM+_>9a+t0a?pD_{q{hv?61r6{7{3Zys9GuO4k0q)LC|_!O6^u!A({pV zNu)?V2X&M53F3(d3wA{o4J20USjKs1Fp%-}3&)o+&(BSy?dCk~jbzjV4MX>C-y3*{ zBS~m$yl&QLM^%A8U~${>Z&t|h2Wk%hooH%GkyzpjeuZ9+7X~zeXv^_RR_CKaG*Tf0 zRZ2}22QU}-jH%S4Vk&#*Bp4BPk{w*OCub`T6Z@JHuG!QGJ&=1t_aQnpX21{gZcO8&+rN_V&;=-C*} zNeke_f3Y!pf;^9C6!@SAq^lrGSCE5@xUC?@u<=UvWLt|fzP=&vByM6$iexyjbzUwY z*mD!AA*>u-)i8w;7TI!8e%U9JN*yJ3oq|ODM%iCUBHQddMK-vn#u3EqcDC*l0Xm20 z^uK(rdnre6JJ!uVbM5ucUYLk9nkoh;lYM%4_yk+6-K3w>TOKRG#cU@g-jA_MGeoHC z)fL^WaRaotctTpk4D$IQj~XAlv?9p?agzJN=cVYf0e5!$zx#e75j}AdSz*J2kEdHi z0Z(g<^tjLLs=wf5TWW-;7yYiW0m{DTuME1PBG!chsBuv>UcEi-Cveb+5+J^(s7%r& zh2(Jw)QZ5%zuD4?>MW6G~(e0~m%;a=y z)qwETnGR{|XZ24_`__x1l2dLSn)eh@WP0-6ow5ugOWj3WnrRe%p%m-WmMM#oD?)OX{k}V4Byu-+ zSm6X8SLUoC5Wu%0RP_Dnc(5xVZ?;YJDB;##Rlrct?R%sd%XzaVa6j?>GxEzp9j{!~ zbxPc;moDjI&{J81DnXE`LELv=60cy}%+Ff~fyB;ncN_pJnUfmjnW45bN!H#zF>MnN8MvH& z=+_E@PfoKqsWGSs1+1~1jL+u<0yH%O60*N}kmWt-B~~PMvSu>vQfyB(%w(Qn;6c-C z`Qj;nnoUNuAeL!A`kY1a*wOP3fYx9N=!H#zVlYcpK9e3O^WJCjzm(PiUk%9tLvx9s z3B;Fdf<0kKvcWSQa37>5D`V>lrL0L9tXOOoGxe(cP2w! z=ee5aO24#CvQEI7V|ymsur89-Zy3_7(V@1+J<%y$v=?GYxbprXZP9f3K^oa zOyG_CNXJ-P(DgoEc~JQ(r1Nh~-LXZW z^?c#WPL%SvWN~~B&59fRGI8B}@-uf)N{%@LrtV(k3KfHrMai5D#9Ea;9B2-x-8JGC zP^DBqyh);z-r)<;rz}Hbg$K zrp7#ne2_w7pbrFfc@>nuz##Erg&55>;FLKTAN#NuvpCrUuzw1a!pLMji=JG*x2B8a zKxHxu;jp;wZlowExX+)a?=;H>`WQ(-gHz%;vSN=37z2ys8&+$~2Njx9q05BZEnSj4sAy-^GKv&}6dznY=j3c%~fm7H?R;z9%FOM`mW65R5l@81+Mog>VpaZkM2HyMm?tS zLlpAsH@D9JElmZQVq(EgJ7y#7bmS2Y@Aoe5KPt52T_4j_usQ=OQC$j{&0g*W=3)i% zB(Y}3{|f^6Aw&fXoQkdAJ)inpBFw+8Hc)I~lk&|eQrK?Mp#GB&`xB=|0Rm41RbwP~ z%=8xJnA5j6f~j?s(&?9jU8(Inxo)8{xAdAIMXlCUAQ~N<(+#wtt&BA8_~3twoO?Lo zPsjk#r$6X40AVShXAflRz4*Q-T|gRsOJBnz{vp3A)498WyFDZv&W2yQ@31Zm8jByG zY}jqjT)4<`0Lt`!oHIJWci~|5?$NdEUGN`kin%^pRFX&C6VBqI|FVVUBuvz{-rnH}f3mY}ezU*}VNCkbrRgs7jYYFfAx8Ayhy9Ij4oUut68e{o z-v2<^@geUy(Hd(Y8TVjDK&!w)XK$|*Vjide==+2{J2%WtDTmW*bd-=roWoA)a;=-; z-%4U&w#uWgvsFN^P$cTs0nlU=me@y=`LC^$AGn~_Nj2x_K*=Txu;<@p`l~lBar-P7 zYcM=}BkXqz6C+9|k&JKOe!ACm`XUSq3zUCbe)c1mpy%$AsKe&Tm#;kf9Grl;IY)YU zr>@T23H)H{iJKXGDC79b6N1O6aCAR@a;0?TJJ8FhI7E#)=#Srv$x-?9p8|6P&%t7@ z%b|z3bxNNV^&6ZIHntk+g$tT!?5p5aCL}cq*w`0b>9tf`T-U~+7>J8alDd!Cy-dCT zwyF3_z-;vuG%x=4ezZ%^biqmI$i_$;$qO13YI%HFt3{!C88K| zrM`kLRPf*=iwK@^9%Y3}L`Hj`nLzOdwNPQgE*m|!skVAn;dtPEnhi$q(iLVQ@Zmm7 zOi1h(-*-#U#sW5)66^HIi-vRuVIJgBSKi_zt}J2qX@PgZYfm+zqKFr9iPJSxjf)Qn zjcwu%iS`7UUtUm9St8O7LsxT$3@6W-gAu|CDp|o@sSosqEBx`jfAgqNK#tdJviIbw zX0el+O<9*Kvl6zxh!xqI!RT!qy{HmnIWleWs<8jbB%uf;BtT-@b~#UZTtS1vBi?S9 zcJSdVXXBA3W@kcvpXK%CGcmlltrbFFn*fgx>F`}`1mjv^{jZLZz60Mxl=fM%WscC{ zcU|HRBSNFlM;#Q|W7sZa%=n56bu6}C7#D~=>w`ZMu8D&dQpEZ$T(r)o-G|weu^I`s z@S~dKLJ8@~eLEJ%9`NE4-5Y(t(lF%;!uoMV-7!J}yCxfz`ht66rWlrDQoUv`&MZFs zk;MqG2&MV?iI!!eMgBLr&{nTi+f zukY)-WL0*uYsp6F>&}|Qv_8(DCzhRfkEmc!UcuDIpI=@yf772oYba`r*;*l&taj)T za0sLA%dFvOW#xVM!8dW{IcwK%WFvYX>`%ST$6pW8Z#L5L^CXLND0w>hc@m+(L9m4{ zQtyvD?$=`6G1+p^c>ACRpSx`ZM>pE!iP>0?b;d*$A1)B<5ttU!ARUbSG{x}CA^OeE zREaOE$sw;C|KoZ4&7;HIn0*zm4rEUlkBzG}tQ!#=&prf)bEceYJ5`GgK1Nl^P^J#T^NFV7=o@6As2+$}R7{NTKOO;`2Pp+~?WJU`<) z%;Fpq&A1nQlV$DO$;9wP>AS)`#AGz29wk=3K!Y zov)5zwh!uI`;SB59dIUYBfV6w#RTlcEZp)fd)+dC5h?8vIP?4K{e5Try=GG%4J2Ou zb4&33(f67tCU0d9gO0}Lw$x{YhDH2sdT=;A;_nUvo01#6>db3zf39jh-=M#7`0LK5 zxTtCro4^>;Pn5wdMTXDu1}_fu6im7HR79+7U(`?mW{WOZ{B5fpua%X-TWuXwvFR7!Y=}0t zZPXWW-EgxQckSIU>o45YX7V<6DzG&8{q_E_Go*tWtmR6{27hV^zCRkiW!x2NGh$w* zYVa*ZP2rl(LWe_k>OHV2cMdPC7)?*#^(UCup<_O5TW&%^hHV>MP)oy=@{_}Rdn4wl z@&e1A^?V-$e*2n^2joGIY?&9rW3O2kC+kz-n+({*CKLFh@TbB47)N_W|A|Tev61ZH z0ETJb)){53y}5V$emCCxIJDzNOf<0dTL;y(S3Cf`Lv-poWbHS4Th|NMyUbnHK12jK2dNj-YPv z73*6>$MFrleN$9RJ)tgQ5!*YvD6cb?!ivnzSYs=h3!kYM>Grbh&nK9Z>y6lKUsGCB z=#ee+vUGxUkTC!G#?x)TWzLVg1)OiA25M=!mo zB6=%&9=PkRgSuV_9OEECM1ufX`}Lw^f)ydh1-oHm2ag`>Lx=o3XUpiV=R+d!GW4rE z`TpL&^UIhaU_XzuzHxn$q1)DLnI9wD0(ob~Jn$7GU%9m6qKe-1a7`C*5L|h-cXleQ zxLHkd;I__+v;&r_KhiOmv>$E0)I)==$8BFzL_72d2p`(;(;)g-8mlWmwe7ch%rG^` zyTBOkbAsZ6zud@R$Us=f+Qs^^_@3Ur-{)t<19rN*I1PO}s8+x+RxCMYEJD_vU<0Cv zMPowsI&Pdrq~(nZ`@eIxg0M2OJr)WaUqgr==j}Is8C3=B=T(Wj>ma~NDe5;93r_sM zDZttsoao3b15&wnibc_&rn*GyCV+9`ePq+WT%$Lz?HZ>pZl5`)&GD3;HbBw&@&#Ka zwt*l15V5&o-?-_3CfxpZ?@J@;t)Mo(;(%TBoY;C{K~xEx!^eJ(Xl=m{xm zOlxQ;5zWLWPWMd43q#j;b1RlvVV+pYSNPH>umMObGhT!@WpOx{+HI3ryml{0&wBup`Y;gq)7)K zq&Uaab7HQPxcw&R|0n$aC;a~>{QoEX|Nk-k7aUxr*s+8DgS?dZ)qhffpNj(Dqqu)k zfqznge^P<3#gKnef#2nH%tHU90{=-Wut4ydEdy#j2sc7c|KgtURgDcGiBq;VrtH?e zU%q2qUcpA0 z2O7vroS5JK1oHBSBmy;|4!dyPLn2;yv+zIv3CoQs6NAO5m3ZzTA|`<`%Dxp}UNp9P zG9#2JF>QbEglWd-5*bs1sgPyCbsG*GpH`3F2gf&k&rS$sHg#`w#|sfO;w{~tq-KCp zhTejh>$`xynccY*5y(&O`GW^A3?KQmD;#%r-&Yj0ywHQ(gQK|3z%y7vWd4o=DxaWP z_eH>2F1WvCC1LcoblYpZ5{*Cp*|ne`dqe?anwwwt1b_DppN6$=eBOECb!yT5;h0cBFff=LEegJ>>FJe&vBkE(=Y0m?)skq1Xd|`_m(+0wY^oPwpN4@9HRjS)BMyf#AE3{&6e+MxFf!Sn^-JudK}?lr}TH zVw@@u-t>xb_8_<%Kn@P0XeG|CoWUp)^OT_Q5=qDOs#oOS`b601!WKS%5ER=}IBSg| zkBxSw!yO}{)x|inwuT|FYexnuJ_sSaoxT{NVo0#)+r-J1IJKgJg4UWHk>B9}6P}6>_=u0uc9UgDuHOhL|GmiGy65*80QV~NX&&*nOp#-wh z(SCT7Y$aOcr^@_QHcVv0hevONQ>GzSVNMIS!}3kQutrh;uzF)j zD4y5pL*{Tv?|7t!wX2NfN**fW`gyzprg8KnxK!4u#=3PYbfv=`*18r8^(TbH;tSTa zFdx#R9XW8u^H0DXFKXk>{Ja+U>5HilqQGh7F)F6%y6Z{Y?_XTj2HfSfuJa4;!AiWc zFeGsq!R;9Gq(srO?9#0eaxc6=g4-qM5rN#t!gHwH@qhT} z*1HeHa_Cy;lExIi+T?rpbM@KjT-bK4=2O{rxloPpc)K9o=K6BGn3ShCwhgLHX#^KT zNRj0}ZXpd4=NnhAS>FE!qR@(NS7%e!6}IdDCS}dr2vc{a?KXzFcmjHCfMqRZoT9z< zL-jXY<>xo~k3LIqncj*tDE;>#D!^wk>!ogwiv3oh+@Q@|)R{>62}F${|LYeWZwA#O zmrr^d9$Ywo_lIc}PQ2mUYl9wuU`Gg3oYMH~7lF`8S%Aq|-tLbadsw>v>^G!su;owI z0Ti=;zg)mkG0g79?>2>S6cg+>r0$RF_@{qwYli`qbbumF>pjF2vE9IC{3rmXAm;z= z_xSO3-y$k9Q$F2zSYp=uH~tC&uD>6pg=GSO)@Ja%>ue|+>9^H7H%sZ>Z=+vWw>gvk zyo~Oj@S~y^zd3k6y*Cd9guAW}AAV|`0qd+io+cpq4Nf7?_{-D#(~AJq#i13z5%(9L zM}F!gT=-UT{aeR)Yxx1hl}BWG%JTIV0?lvnH?G0<_TeT0O1tDZz0o!bl{BY+Drv4t zWd8MHd_|A8FY6lH|GU>}{abN8c}`#xR}YQAU4G+o7vq9G?B=n3RSS5(9fbAN|7_Xp zAi&8v2J`h~ZrY^$bn*Y*tF=i3l01V_L!f|Z->6-q`um0mJ%<{8MV%`kU zAY|CJ{RB5Q?)vMKWOoN7Pogu3&9~ve0m!KTZ~$t0$Zzzfuc5(@eStjRA0mji@qivA zkULX9U;T;n**6D0`y)R2>&x)nkAnghg@c!)*A}HC&*~d#=s$4q+m2D#7=ZeK(6aRP zT3KoQe65`RdNF>Dg#Ni!KZTdS*}i|S)t}#pf3DS^CK&%K*J^w}5an8*0-{ia-u0P{ z_e-FQeXURQ91cAfT=sT5g6)aiPCFJh{zGpstfc`$J%a7m*nD8K{OS*T*!K5cZai+B zr9`FUd8B904}17`5|*#_Y&)&n^2-A|<9`Z*(0|Lg0zTFMZ85^e{*~c-M|Kxh>Ffai N + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +#ifndef WALKING_UTILS_HPP +#define WALKING_UTILS_HPP + +// std +#include +#include + +// YARP +#include +#include +#include +#include + +// iDynTree +#include +#include +#include +#include + +// eigen +#include + +typedef iDynTree::SparseMatrix iDynSparseMatrix; + +/** + * Helper for iDynTree library. + */ +namespace iDynTreeHelper +{ + /** + * Triplets namespace add some useful function to manage triplets + */ + namespace Triplets + { + /** + * Merge two set of triplets output = [output, input] + * @param input is the input set of triplets; + * @param output is the output set of triplets. + */ + void pushTriplets(const iDynTree::Triplets& input, iDynTree::Triplets& output); + + /** + * Merge two set of triplets. The output triplets object represents a matrix while + * the input triplets represent a sub matrix. + * @param startingRow is the row position of the sub matrix; + * @param startingColumn is the column position of the sub matrix; + * @param input is the input set of triplets; + * @param output is the output set of triplets. + */ + void pushTripletsAsSubMatrix(const unsigned& startingRow, const unsigned& startingColumn, + const iDynTree::Triplets& input, + iDynTree::Triplets& output); + + /** + * Get triplets from yarp value list. It is useful if you want to convert a yarp list of triplets + * into a iDynTree Triplets object. + * @todo extend to non squared matrix. + * @param input the yarp value list; + * @param matrixDimension the dimension of the matrix. It used to check if the triplets are valid + * (i.e. the row and the column of each triplet has to be compatible with the matrix dimension); + * @param output is the idyntree triplets object. + * @return true/false in case of success/failure + */ + bool getTripletsFromValues(const yarp::os::Value& input, + const int& matrixDimension, + iDynTree::Triplets& output); + } + + /** + * SparseMatrix namespace add some useful function to manage iDynTree sparse matrices + */ + namespace SparseMatrix + { + /** + * Convert an eigen sparse matrix into an iDynTree sparse matrix + * @param eigenSparseMatrix is a column Major eigen sparse matrix. + * @return an iDynTree column major sparse matrix. + */ + iDynSparseMatrix fromEigen(const Eigen::SparseMatrix& eigenSparseMatrix); + } + + namespace Rotation + { + /** + * Transform a 3x3 matrix into a skew-symmetric matrix. + * @param input is a 3x3 matrix; + * @return a 3x3 skew-symmetric matrix + */ + iDynTree::Matrix3x3 skewSymmetric(const iDynTree::Matrix3x3& input); + } + + /** + * Given 2 angles, it returns the shortest angular difference. + * The result would always be -pi <= result <= pi. + * + * This function is taken from ROS angles [api](http://docs.ros.org/lunar/api/angles/html/namespaceangles.html#a4436fe67ae0c9df020f6779101bbefab). + * @param fromRad is the starting angle expressed in radians; + * @param toRad is the final angle expressed in radians. + * @return the shortest angular distance + */ + double shortestAngularDistance(const double& fromRad, const double& toRad); +} + +/** + * Helper for YARP library. + */ +namespace YarpHelper +{ + /** + * Add a vector of string to a property of a given name. + * @param prop yarp property; + * @param key is the key; + * @param list is the vector of strings that will be added into the property. + * @return true/false in case of success/failure + */ + bool addVectorOfStringToProperty(yarp::os::Property& prop, const std::string& key, + const std::vector& list); + + /** + * Convert a yarp list into a vector of string + * @param input is the pointer of a yarp value; + * @param output is the vector of strings. + * @return true/false in case of success/failure + */ + bool yarpListToStringVector(yarp::os::Value*& input, std::vector& output); + + /** + * Extract a string from a searchable object. + * @param config is the searchable object; + * @param key the name to check for; + * @param string is the string. + * @return true/false in case of success/failure + */ + bool getStringFromSearchable(const yarp::os::Searchable& config, const std::string& key, + std::string& string); + + /** + * Extract a double from a searchable object. + * @param config is the searchable object; + * @param key the name to check for; + * @param number is the double. + * @return true/false in case of success/failure + */ + bool getNumberFromSearchable(const yarp::os::Searchable& config, const std::string& key, + double& number); + + + /** + * Extract a double from a searchable object. + * @param config is the searchable object; + * @param key the name to check for; + * @param number is the integer. + * @return true/false in case of success/failure + */ + bool getNumberFromSearchable(const yarp::os::Searchable& config, const std::string& key, + int& number); + + + /** + * Convert a yarp value into an iDynTree::VectorFixSize + * @param input yarp value; + * @param output iDynTree::VectorFixSize. + * @return true/false in case of success/failure. + */ + template + bool yarpListToiDynTreeVectorFixSize(const yarp::os::Value& input, iDynTree::VectorFixSize& output); + + /** + * Convert a yarp value into an iDynTree::VectorDynSize + * @param input yarp value; + * @param output iDynTree::VectorDynSize if the size of this vector is different from the size of the + * YARP list it will be resized. + * @return true/false in case of success/failure. + */ + bool yarpListToiDynTreeVectorDynSize(const yarp::os::Value& input, iDynTree::VectorDynSize& output); + + /** + * Merge two vectors. vector = [vector, t] + * @param vector the original vector. The new elements will be add at the end of this vector; + * @param t vector containing the elements that will be merged with the original vector. + */ + template + void mergeSigVector(yarp::sig::Vector& vector, const T& t); + + /** + * Variadic fuction used to merge several vectors. + * @param vector the original vector. The new elements will be add at the end of this vector; + * @param t vector containing the elements that will be merged with the original vector. + * @param args list containing all the vector that will be merged. + */ + template + void mergeSigVector(yarp::sig::Vector& vector, const T& t, const Args&... args); + + /** + * Send a variadic vector through a yarp buffered port + * @param port is a Yarp buffered port + * @param args list containing all the vector that will be send. + */ + template + void sendVariadicVector(yarp::os::BufferedPort& port, const Args&... args); + + /** + * Add strings to a bottle. + * @param bottle this bottle will be filled. + * @param strings list containing all the string. + */ + void populateBottleWithStrings(yarp::os::Bottle& bottle, const std::initializer_list& strings); +} + +/** + * Helper for std library + */ +namespace StdHelper +{ + /** + * Allow you to append vector to a deque. + * @param input input vector; + * @param output output deque; + * @param initPoint point where the vector will be append to the deque + */ + template + bool appendVectorToDeque(const std::vector& input, std::deque& output, const size_t& initPoint); +} +#include "Utils.tpp" + +#endif diff --git a/include/Utils.tpp b/include/Utils.tpp new file mode 100644 index 0000000..53058b2 --- /dev/null +++ b/include/Utils.tpp @@ -0,0 +1,101 @@ +/** + * @file Utils.tpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +// std +#include + +// YARP +#include + +template +bool YarpHelper::yarpListToiDynTreeVectorFixSize(const yarp::os::Value& input, iDynTree::VectorFixSize& output) +{ + if (input.isNull()) + { + yError() << "[yarpListToiDynTreeVectorFixSize] Empty input value."; + return false; + } + if (!input.isList() || !input.asList()) + { + yError() << "[yarpListToiDynTreeVectorFixSize] Unable to read the input list."; + return false; + } + yarp::os::Bottle *inputPtr = input.asList(); + + if (inputPtr->size() != n) + { + yError() << "[yarpListToiDynTreeVectorFixSize] The dimension set in the configuration file is not " + << n; + return false; + } + + for (int i = 0; i < inputPtr->size(); i++) + { + if (!inputPtr->get(i).isDouble() && !inputPtr->get(i).isInt()) + { + yError() << "[yarpListToiDynTreeVectorFixSize] The input is expected to be a double"; + return false; + } + output(i) = inputPtr->get(i).asDouble(); + } + return true; +} + +template +void YarpHelper::mergeSigVector(yarp::sig::Vector& vector, const T& t) +{ + for(int i= 0; i +void YarpHelper::mergeSigVector(yarp::sig::Vector& vector, const T& t, const Args&... args) +{ + for(int i= 0; i +void YarpHelper::sendVariadicVector(yarp::os::BufferedPort& port, const Args&... args) +{ + yarp::sig::Vector& vector = port.prepare(); + vector.clear(); + + mergeSigVector(vector, args...); + + port.write(); +} + +template +bool StdHelper::appendVectorToDeque(const std::vector& input, std::deque& output, const size_t& initPoint) +{ + if(initPoint > output.size()) + { + std::cerr << "[appendVectorToDeque] The init point has to be less or equal to the size of the output deque." + << std::endl; + return false; + } + + // resize the deque + output.resize(input.size() + initPoint); + + // Advances the iterator it by initPoint positions + typename std::deque::iterator it = output.begin(); + std::advance(it, initPoint); + + // copy the vector into the deque from the initPoint position + std::copy(input.begin(), input.end(), it); + + return true; +} diff --git a/include/WalkingLogger.hpp b/include/WalkingLogger.hpp new file mode 100644 index 0000000..e5989d8 --- /dev/null +++ b/include/WalkingLogger.hpp @@ -0,0 +1,55 @@ +/** + * @file WalkingLogger.hpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +#ifndef WALKING_LOGGER_HPP +#define WALKING_LOGGER_HPP + +// YARP +#include +#include +#include +#include + +class WalkingLogger +{ + yarp::os::BufferedPort m_dataPort; /**< Data logger port. */ + yarp::os::RpcClient m_rpcPort; /**< RPC data logger port. */ + +public: + + /** + * Configure + * @param config yarp searchable configuration variable; + * @param name is the name of the module. + * @return true/false in case of success/failure. + */ + bool configure(const yarp::os::Searchable& config, const std::string& name); + + /** + * Start record. + * @param strings head of the logger file + * @return true/false in case of success/failure. + */ + bool startRecord(const std::initializer_list& strings); + + /** + * Quit the logger. + */ + void quit(); + + /** + * Send data to the logger. + * @param args all the vector containing the data that will be sent. + */ + template + void sendData(const Args&... args); +}; + +#include "WalkingLogger.tpp" + +#endif diff --git a/include/WalkingLogger.tpp b/include/WalkingLogger.tpp new file mode 100644 index 0000000..585c6f5 --- /dev/null +++ b/include/WalkingLogger.tpp @@ -0,0 +1,15 @@ +/** + * @file WalkingLogger.tpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +#include + +template +void WalkingLogger::sendData(const Args&... args) +{ + YarpHelper::sendVariadicVector(m_dataPort, args...); +} diff --git a/include/icubFloatingBaseEstimatorV1.h b/include/icubFloatingBaseEstimatorV1.h new file mode 100644 index 0000000..0e53d5b --- /dev/null +++ b/include/icubFloatingBaseEstimatorV1.h @@ -0,0 +1,632 @@ +/* +################################################################################ +# # +# Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT) # +# All Rights Reserved. # +# # +################################################################################ + +# @authors: Prashanth Ramadoss +# Giulio Romualdi +# Silvio Traversaro +# Daniele Pucci +*/ + +#ifndef ICUB_FLOATING_BASE_ESTIMATOR_V1_H +#define ICUB_FLOATING_BASE_ESTIMATOR_V1_H + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +inline double deg2rad(const double angleInDeg) +{ + return angleInDeg*M_PI/180.0; +} + +inline double rad2deg(const double angleInRad) +{ + return angleInRad*180.0/M_PI; +} + +inline void convertVectorFromDegreesToRadians(iDynTree::VectorDynSize & vector) +{ + for(size_t i=0; i < vector.size(); i++) + { + vector(i) = deg2rad(vector(i)); + } + + return; +} + +namespace yarp { + namespace dev { + + + class icubFloatingBaseEstimatorV1 : public yarp::dev::DeviceDriver, + public yarp::dev::IMultipleWrapper, + public yarp::os::PeriodicThread, + public floatingBaseEstimationRPC + { + public: + explicit icubFloatingBaseEstimatorV1(double period, yarp::os::ShouldUseSystemClock useSystemClock = yarp::os::ShouldUseSystemClock::No); + icubFloatingBaseEstimatorV1(); + ~icubFloatingBaseEstimatorV1(); + + /** + * @brief Open the estimator device + * @param[in] config Searchable object of configuration parameters + * @return true/false success/failure + */ + virtual bool open(yarp::os::Searchable& config); + + /** + * @brief Close the estimator device + * @return true/false success/failure + */ + virtual bool close(); + + /** + * @brief Attach other required devices to the estimator. Calls other attach methods. + * @param[in] p List of devices to be attached to the estimator + * @return true/false success/failure + */ + virtual bool attachAll(const yarp::dev::PolyDriverList& p); + + /** + * @brief detach other devices from the estimator device + * + * @return bool + */ + virtual bool detachAll(); + + /** + * @brief the periodic method that is called every update period, + * core of the estimator + * + * @return void + */ + virtual void run(); + + private: + /** + * @brief Load estimator device settings from configuration file + * + * @param[in] config parsed configuration file as a Searchable object + * @return bool + */ + bool loadEstimatorParametersFromConfig(const yarp::os::Searchable& config); + + /** + * @brief Load legged odometry settings from configuration file + * + * @param[in] config parsed configuration file as a Searchable object + * @return bool + */ + bool loadLeggedOdometryParametersFromConfig(const yarp::os::Searchable& config); + + /** + * @brief Load foot contact classifier settings from configuration file + * + * @param[in] config parsed configuration file as a Searchable object + * @return bool + */ + bool loadBipedFootContactClassifierParametersFromConfig(const yarp::os::Searchable& config); + + /** + * @brief Load attitude estimator (Mahony-Hamel filter) settings from configuration file + * + * @param[in] config parsed configuration file as a Searchable object + * @return bool + */ + bool loadIMUAttitudeMahonyEstimatorParametersFromConfig(const yarp::os::Searchable& config); + + /** + * @brief Load attitude estimator (Quaternion EKF) settings from configuration file + * + * @param[in] config parsed configuration file as a Searchable object + * @return bool + */ + bool loadIMUAttitudeQEKFParamtersFromConfig(const yarp::os::Searchable& config); + + /** + * @brief Instantiates the transform broadcaster to publish the world to base transform + * over a ROS topic for RViz visualization + * + * @return bool + */ + bool loadTransformBroadcaster(); + + /** + * @brief function taking in a pointer of readFunc() + * to perform a dry run initial check of sensors + * + * @param[in] verbose verbose argument to be passed to function pointer + * @param[in] func_t pointer of readFunc() + * @return true/false success/failure + */ + bool sensorReadDryRun(bool verbose, bool (icubFloatingBaseEstimatorV1::*func_t)(bool)); + + /** + * @brief read IMU sensors + * + * @param[in] verbose verbose flag + * @return true/false success/failure + */ + bool readIMUSensors(bool verbose); + + /** + * @brief read FT sensors + * + * @param[in] verbose verbose flag + * @return true/false success/failure + */ + bool readFTSensors(bool verbose); + + /** + * @brief read Encoders + * + * @param[in] verbose verbose flag + * @return true/false success/failure + */ + bool readEncoders(bool verbose); + + /** + * @brief parent function calling all read sensors methods + * + * @param[in] verbose verbose flag + * @return true/false success/failure + */ + bool readSensors(bool verbose); + + /** + * @brief Loads SimpleLeggedOdometry, BipedFootContactClassifier and AttitudeMahonyFilter + * also KinDynComputations if necessary + * @return true/false success/failure + */ + bool loadEstimator(); + + /** + * @brief instantiate the legged odometry with loaded config parameters + * + * @return true/false success/failure + */ + bool loadLeggedOdometry(); + + /** + * @brief instantiate the biped foot contact classifier with loaded config parameters + * + * @return true/false success/failure + */ + bool loadBipedFootContactClassifier(); + + /** + * @brief instantiate the Mahony-Hamel filter with loaded config parameters + * + * @return true/false success/failure + */ + bool loadIMUAttitudeMahonyEstimator(); + + /** + * @brief instantiate the Quaternion EKF with loaded config parameters + * + * @return true/false success/failure + */ + bool loadIMUAttitudeQEKF(); + + /** + * @brief Attach all the control board units to the estimator + * @param[in] p List of devices to be attached to the estimator + * @return true/false success/failure + */ + bool attachAllControlBoards(const yarp::dev::PolyDriverList &p); + + /** + * @brief Attach all the inertial measurement units to the estimator + * @param[in] p List of devices to be attached to the estimator + * @return true/false success/failure + */ + bool attachAllInertialMeasurementUnits(const yarp::dev::PolyDriverList &p); + + /** + * @brief Attach all the 6 axis force-torque sensor units to the estimator + * @param[in] p List of devices to be attached to the estimator + * @return true/false success/failure + */ + bool attachAllForceTorqueSensors(const yarp::dev::PolyDriverList &p); + + /** + * @brief Attach the multiple analog sensor interface to the estimator + * @param[in] p List of devices to be attached to the estimator + * @return true/false success/failure + */ + bool attachMultipleAnalogSensors(const yarp::dev::PolyDriverList &p); + + /** + * @brief Allocating data buffers + */ + void resizeBuffers(); + + /** + * @brief Get the list of joints to be loaded by the estimator + * + * @param[in] config parsed configuration file as a Searchable object + * @param[out] joint_list list of joint names obtained from the configuration settings + * @return bool + */ + bool getJointNamesList(const yarp::os::Searchable& config, + std::vector& joint_list); + + /** + * @brief Open ports for publish and service calls + * @return true/false success/failure + */ + bool openComms(); + + /** + * @brief Close the estimator devicee + */ + void closeDevice(); + + /** + * @brief initialize legged odometry + * @return true/false success/failure + */ + bool initializeLeggedOdometry(); + + /** + * @brief initialize biped foot contact classifier + * @return true/false success/failure + */ + bool initializeBipedFootContactClassifier(); + + /** + * @brief initialize Mahony-Hamel filter + * @return true/false success/failure + */ + bool initializeIMUAttitudeEstimator(); + + /** + * @brief initialize quaternion EKF + * @return true/false success/failure + */ + bool initializeIMUAttitudeQEKF(); + + /** + * @brief compute transform of IMU's world frame with estimator world frame + * @return true/false success/failure + */ + bool alignIMUFrames(); + + /** + * @brief Get world to base rotation from attitude estimation using IMU + * @return iDynTree::Rotation return w_R_b computed using IMU rotation estimates + */ + iDynTree::Rotation getBaseOrientationFromIMU(); + + /** + * @brief Get the home transform of the neck to IMU to be used for correcting the + * transform due to neck kinematics. this is useful only when using the head IMU + * @note this method needs to be used only if using the head IMU, not applicable for the waist IMU + * @return iDynTree::Transform return HEADIMU_H_NeckBaseAtHomePosition + */ + iDynTree::Transform getHeadIMU_H_NeckBaseAtZero(); + + /** + * @brief Get the correction transform to be used for correcting the + * transform due to neck kinematics. this is useful only when using the head IMU + * @note this method needs to be used only if using the head IMU, not applicable for the waist IMU + * @return iDynTree::Transform return HEADIMU_H_NeckBase + */ + iDynTree::Transform getHeadIMUCorrectionWithNeckKinematics(); + + /** + * @brief update the fixed frame and kinematic measurements for legged odometry and + * update the world to base transform computed through legged odometery + * @return true/false success/failure + */ + bool updateLeggedOdometry(); + + /** + * @brief propagate the internal state of the Mahony-Hamel filter + * update the Mahony-Hamel filter with IMU measurements and + * @return true/false success/failure + */ + bool updateIMUAttitudeEstimator(); + + /** + * @brief propagate the internal state of the QEKF + * update the QEKF with IMU measurements and + * @return true/false success/failure + */ + bool updateIMUAttitudeQEKF(); + + /** + * @brief correct the orientation estimate of the Head IMU obtained from the + * attitude estimator with the correction transform HEADIMU_H_NeckBase + * @note this method needs to be used only if using the head IMU, not applicable for the waist IMU + * @return true/false success/failure + */ + bool correctHeadIMUWithNeckKinematics(); + + /** + * @brief Fuses the world to base transform + * obtained from legged odometry and IMU attitude estimation + * @return true/false success/failure + */ + bool updateBasePoseWithIMUEstimates(); + + /** + * @brief compute the floating base velocity using + * fixed frame contact Jacobian and holonomic constraint dynamics + * @return true/false success/failure + */ + bool updateBaseVelocity(); + + /** + * @brief computes the floating base velocity using IMU measurements + * linear velocity is obtained through Euler-integration of linear acceleration + * obtained from the proper sensor acceleration of the IMU + * angular velocity is given by the attitude estimator + * @return true/false success/failure + */ + bool updateBaseVelocityWithIMU(); + + /** + * @brief parent method for other publish methods + */ + void publish(); + + /** + * @brief publish internal state of attitude estimator through YARP port + * roll pitch yaw omegax omegay omegaz gyrobiasx gyrobiasy gyrobiasz + */ + void publishIMUAttitudeEstimatorStates(); + + /** + * @brief publish internal state of QEKF through YARP port + * roll pitch yaw + */ + void publishIMUAttitudeQEKFEstimates(); + + /** + * @brief publish floating base state through YARP port + * x y z roll pitch yaw joint_positions + * roll pitch yaw + */ + void publishFloatingBaseState(); + + /** + * @brief publish floating base pose and velocity through YARP port + * x y z roll pitch yaw vx vy vz omegax omegay omegaz + */ + void publishFloatingBasePoseVelocity(); + + /** + * @brief publish feet contact state through YARP port + * fz_lf fz_rf state_lf state_rf fixed_frame + */ + void publishContactState(); + + /** + * @brief publish world to base tranform to /tf ROS topic using IFrameTransform interface + */ + void publishTransform(); + + /** + * @brief configure whole body dynamics device and + * get the output ports to connect to in order + * to get the end-effector contact wrenches + * @return true/false success/failure + */ + bool configureWholeBodyDynamics(const yarp::os::Searchable& config); + + /** + * @brief calibrate the force-torque sensor offsets through a RPC call to wholebodydynamics + * this calibration is necessary for proper estimation of contact wrenches + * the calibration is internally taken care of by the wholebodydynamics device + * @return true/false success/failure + */ + bool calibFTSensorsStanding(); + + /** + * @brief get the foot contact normal forces + */ + void getFeetCartesianWrenches(); + + /** + * @brief read the foot contact wrenches from the output ports of the wholebodydynamics device + * @return true/false success/failure + */ + bool readWholeBodyDynamicsContactWrenches(bool verbose); + + /** + * @brief initialize logger with the necessary variables to be logged + * @return true/false success/failure + */ + bool initializeLogger(); + + /** + * @brief update the logger + * @return true/false success/failure + */ + bool updateLogger(); + + // RPC methods + virtual std::string getEstimationJointsList(); + virtual bool setMahonyKp(const double kp); + virtual bool setMahonyKi(const double ki); + virtual bool setMahonyTimeStep(const double timestep); + virtual bool setContactSchmittThreshold(const double l_fz_break, const double l_fz_make, + const double r_fz_break, const double r_fz_make); + virtual bool setPrimaryFoot(const std::string& primary_foot); + virtual std::string getRefFrameForWorld(); + virtual Pose6D getRefPose6DForWorld(); + virtual bool resetLeggedOdometry(); + virtual bool resetLeggedOdometryWithRefFrame(const std::string& ref_frame, + const double x, const double y, const double z, + const double roll, const double pitch, const double yaw); + virtual bool startFloatingBaseFilter(); + virtual bool useJointVelocityLPF(const bool flag); + virtual bool setJointVelocityLPFCutoffFrequency(const double freq); + + // device options + bool m_verbose{true}; ///< verbose outputs + std::string m_port_prefix{"/base-estimator"}; + + yarp::os::Mutex m_device_mutex; ///< mutex to avoid resource clash + + // status flags + bool m_device_initialized_correctly{false}; + bool m_legged_odometry_update_went_well{false}; + bool m_use_debug_ports{true}; + + // configuration parameters + enum class FilterFSM{IDLE, CONFIGURED, RUNNING}; + FilterFSM m_state{FilterFSM::IDLE}; + double m_device_period_in_s{0.01}; ///< Thread period of the estimator device in seconds + std::string m_model_file_name{"model.urdf"}; ///< URDF file name of the robot + std::string m_robot{"icubSim"}; ///< name of the robot to identify between real and sim + std::vector m_estimation_joint_names; ///< list of joints used for estimation + std::string m_base_link_name{"root_link"}; ///< floating base link + std::string m_initial_fixed_frame; ///< initial fixed frame for legged odometry + std::string m_initial_reference_frame_for_world; ///< frame in which initial world is expressed + iDynTree::Transform m_initial_reference_frame_H_world; ///< pose of the world w.r.t initial reference frame + iDynTree::SchmittParams m_left_foot_contact_schmitt_params, m_right_foot_contact_schmitt_params; ///< contact Schmitt trigger parameters for the feet + std::string m_initial_primary_foot{"left"}; ///< initial primary foot for the contact classifier + iDynTree::AttitudeMahonyFilterParameters m_imu_attitude_observer_params; ///< parameters for the attitude observer + iDynTree::AttitudeQuaternionEKFParameters m_imu_attitude_qekf_params; + std::string m_head_imu_name{"head_imu_acc_1x1"}; + + // robot model and sensors + iDynTree::Model m_model; ///< iDynTree object of loaded robot model + iDynTree::SensorsList m_sensors_list; ///< iDynTree object of loaded sensors list from URDF + + const double m_sensor_timeout_in_seconds{2.0}; ///< Timeout to check for sensor measurements during dry run initial check + const size_t m_nr_of_channels_in_YARP_IMU_sensor{12}; ///< Number of channels available in YARP IMU sensor output port + const size_t m_nr_of_channels_in_YARP_FT_sensor{6}; ///< Number of channels available in YARP FT sensor output port + bool m_use_multiple_analog_sensor_interface{false}; ///< Flag to switch between analog sensor interface or multiple analog sensor interface + + iDynTree::JointPosDoubleArray m_joint_positions; ///< joint positions array + iDynTree::VectorDynSize m_joint_velocities; ///< joint velocities array + + // struct maintaining the IMU measurement serialization + struct IMU_measurements + { + iDynTree::Vector3 linear_proper_acceleration; + iDynTree::Vector3 angular_velocity; + iDynTree::Vector3 angular_acceleration; + bool sensor_status{true}; + std::string sensor_name; + }; + + // struct for the remapped control board interfaces + struct + { + // cannot be a unique_ptr because it is just a reference to a resource owned by someone else + yarp::dev::IEncoders *encs{nullptr}; + }m_remapped_control_board_interfaces; + + iDynTree::SensorsMeasurements m_sensor_measurements; + + std::vector m_whole_body_imu_interface; ///< generic sensor interface for maintaining IMU sensors across the whole body + size_t m_nr_of_IMUs_detected{0}; ///< number of IMUs attached to the estimator device + std::vector m_raw_IMU_measurements; ///< a vector of IMU measurements associated to the vector of IMU sensors + + std::vector m_whole_body_forcetorque_interface; ///< analog sensor interface for maintaining FT seneor across whole body + size_t m_nr_of_forcetorque_sensors_detected{0}; ///< number of FT sensors attached to the estimator device + + // YARP Buffers + std::vector m_imu_meaaurements_from_yarp_server; ///< YARP buffer for IMU measuremnts coming from different IMU sensors + yarp::sig::Vector m_ft_measurements_from_yarp_server; ///< YARP buffer for FT measuremnts coming from different FT sensors + + // Estimation interfaces + std::unique_ptr m_legged_odometry; ///< legged odometry + std::unique_ptr m_biped_foot_contact_classifier; ///< foot contact classifier based on an internal contact force Schmitt Trigger + std::unique_ptr m_imu_attitude_observer; ///< attitude observer to estimate IMU orientation from IMU measurements + std::unique_ptr m_imu_attitude_qekf; + + std::string m_attitude_estimator_type{"qekf"}; + iDynTree::VectorDynSize m_initial_attitude_estimate_as_quaternion; + double m_imu_confidence_roll{0.5}; + double m_imu_confidence_pitch{0.5}; + + yarp::os::BufferedPort m_floating_base_state_port; ///< port to publish floating base pose plus joint positions + yarp::os::BufferedPort m_floating_base_pose_port; ///< port to publish floating base pose + yarp::os::BufferedPort m_contact_state_port; ///< port to publish (fzleft, fzright, left_contact, right_contact, fixedLinkIndex) + yarp::os::BufferedPort m_imu_attitude_observer_estimated_state_port; ///< port to publish rotation as RPY and IMU gyro bias + yarp::os::BufferedPort m_imu_attitude_qekf_estimated_state_port; ///< port to publish rotation as RPY and IMU gyro bias + yarp::os::Port m_estimator_rpc_port; ///< RPC port for service calls + + iDynTree::RPY m_imu_attitude_estimate_as_rpy; + + iDynTree::Rotation m_head_imu_calibration_matrix; + iDynTree::Transform m_imu_H_neck_base_at_zero; + bool m_imu_aligned{false}; + + std::string m_current_fixed_frame; ///< current frame associated to the fixed link in legged odometery + std::string m_previous_fixed_frame; ///< previous frame associated to the fixed link in legged odometery + bool m_no_foot_in_contact{false}; ///< flag to check if contact on both feet is lost + double m_left_foot_contact_normal_force, m_right_foot_contact_normal_force; ///< foot contact force z-direction + + yarp::sig::Vector m_world_pose_base_in_R6; ///< 6D vector pose of floating base frame in world reference frame + yarp::sig::Matrix m_world_H_base; ///< Homogeneous transformation matrix from base to world reference frame + yarp::sig::Vector m_world_velocity_base; ///< 6D vector velocity of floating base frame in the world reference frame + yarp::sig::Vector m_world_velocity_base_from_imu; + + std::string m_left_foot_ft_sensor{"l_foot_ft_sensor"}; + unsigned int m_left_foot_ft_sensor_index, m_right_foot_ft_sensor_index; + std::string m_right_foot_ft_sensor{"r_foot_ft_sensor"}; + std::string m_right_sole{"r_sole"}; + std::string m_left_sole{"l_sole"}; + iDynTree::Rotation m_l_sole_R_l_ft_sensor, m_r_sole_R_r_ft_sensor; + iDynTree::KinDynComputations m_kin_dyn_comp; + + std::string m_left_foot_cartesian_wrench_port_name, m_right_foot_cartesian_wrench_port_name; + yarp::os::BufferedPort m_left_foot_cartesian_wrench_wbd_port, m_right_foot_cartesian_wrench_wbd_port; + yarp::sig::Vector m_left_foot_cartesian_wrench, m_right_foot_cartesian_wrench; + bool m_wbd_is_open{false}; + + yarp::dev::PolyDriver m_transform_broadcaster; + yarp::dev::IFrameTransform *m_transform_interface{nullptr}; + + std::unique_ptr m_logger; + bool m_dump_data{false}; + + std::unique_ptr m_joint_velocities_filter; + double m_joint_vel_filter_cutoff_freq{0.0}; + double m_joint_vel_filter_sample_time_in_s{0.0}; + bool m_use_lpf{false}; + }; + } +} + +#endif diff --git a/ros/fbeViz.launch b/ros/fbeViz.launch new file mode 100644 index 0000000..9d550b3 --- /dev/null +++ b/ros/fbeViz.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/scope/base_scope.xml b/scope/base_scope.xml new file mode 100644 index 0000000..cbe3f29 --- /dev/null +++ b/scope/base_scope.xml @@ -0,0 +1,204 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/scope/base_velocity.xml b/scope/base_velocity.xml new file mode 100644 index 0000000..3c5630e --- /dev/null +++ b/scope/base_velocity.xml @@ -0,0 +1,205 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/scope/contact_scope.xml b/scope/contact_scope.xml new file mode 100644 index 0000000..b99b325 --- /dev/null +++ b/scope/contact_scope.xml @@ -0,0 +1,166 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/scope/waist_imu_scope.xml b/scope/waist_imu_scope.xml new file mode 100644 index 0000000..2ad9311 --- /dev/null +++ b/scope/waist_imu_scope.xml @@ -0,0 +1,97 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Utils.cpp b/src/Utils.cpp new file mode 100644 index 0000000..3b75ba0 --- /dev/null +++ b/src/Utils.cpp @@ -0,0 +1,280 @@ +/** + * @file Utils.cpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +// std +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#include + +// YARP +#include + +// iDynTree +#include +#include + +iDynTree::Matrix3x3 iDynTreeHelper::Rotation::skewSymmetric(const iDynTree::Matrix3x3& input) +{ + iDynTree::Matrix3x3 output; + iDynTree::toEigen(output) = 0.5 * (iDynTree::toEigen(input) - iDynTree::toEigen(input).transpose()); + return output; +} + +void iDynTreeHelper::Triplets::pushTriplets(const iDynTree::Triplets& input, + iDynTree::Triplets& output) +{ + for(auto triplet: input) + output.pushTriplet(triplet); + + return; +} + +void iDynTreeHelper::Triplets::pushTripletsAsSubMatrix(const unsigned& startingRow, + const unsigned& startingColumn, + const iDynTree::Triplets& input, + iDynTree::Triplets& output) +{ + if(startingRow != 0 || startingColumn != 0) + { + iDynTree::Triplets inputShifted = input; + + for(auto triplet: inputShifted) + { + triplet.row += startingRow; + triplet.column += startingColumn; + output.pushTriplet(triplet); + } + } + else + { + iDynTreeHelper::Triplets::pushTriplets(input, output); + } + return; +} + +bool iDynTreeHelper::Triplets::getTripletsFromValues(const yarp::os::Value& input, + const int& matrixDimension, + iDynTree::Triplets& output) +{ + // clear the output + output.clear(); + + if (input.isNull()) + { + yError() << "[getTripletsFromValues] Empty input values."; + return false; + } + + if (!input.isList() || !input.asList()) + { + yError() << "[getSparseMatrixFromTriplets] Unable to read the input as a list."; + return false; + } + + yarp::os::Bottle *tripletsPtr = input.asList(); + + // populate the triplets + for (int i = 0; i < tripletsPtr->size(); ++i) + { + yarp::os::Bottle *tripletPtr = tripletsPtr->get(i).asList(); + + if (tripletPtr->size() != 3) + { + yError() << "[getSparseMatrixFromTriplets] The triplet must have three elements."; + return false; + } + + int row = tripletPtr->get(0).asInt(); + int col = tripletPtr->get(1).asInt(); + + if(col >= matrixDimension || row >= matrixDimension) + { + yError() << "[getSparseMatrixFromTriplets] element position exceeds the matrix dimension."; + return false; + } + output.pushTriplet(iDynTree::Triplet(col, row, tripletPtr->get(2).asDouble())); + } + return true; +} + +iDynSparseMatrix iDynTreeHelper::SparseMatrix::fromEigen(const Eigen::SparseMatrix& eigenSparseMatrix) +{ + iDynTree::Triplets triplets; + // populate the triplets + for (int k=0; k < eigenSparseMatrix.outerSize(); ++k) + for (Eigen::SparseMatrix::InnerIterator it(eigenSparseMatrix, k); it; ++it) + triplets.pushTriplet(iDynTree::Triplet(it.row(), it.col(), it.value())); + + // convert the triplets into a sparse matrix + iDynSparseMatrix iDynTreeSparseMatrix(eigenSparseMatrix.rows(), eigenSparseMatrix.cols()); + iDynTreeSparseMatrix.setFromConstTriplets(triplets); + + return iDynTreeSparseMatrix; +} + +bool YarpHelper::yarpListToiDynTreeVectorDynSize(const yarp::os::Value& input, iDynTree::VectorDynSize& output) +{ + if (input.isNull()) + { + yError() << "[yarpListToiDynTreeVectorDynSize] Empty input value."; + return false; + } + if (!input.isList() || !input.asList()) + { + yError() << "[yarpListToiDynTreeVectorDynSize] Unable to read the input list."; + return false; + } + yarp::os::Bottle *inputPtr = input.asList(); + + if (inputPtr->size() != output.size()) + { + yError() << "[yarpListToiDynTreeVectorDynSize] The size of the iDynTree vector and the size of " + << "the YARP list are not coherent."; + return false; + } + + for (int i = 0; i < inputPtr->size(); i++) + { + if (!inputPtr->get(i).isDouble() && !inputPtr->get(i).isInt()) + { + yError() << "[yarpListToiDynTreeVectorDynSize] The input is expected to be a double or a int"; + return false; + } + output(i) = inputPtr->get(i).asDouble(); + } + return true; +} + +bool YarpHelper::addVectorOfStringToProperty(yarp::os::Property& prop, const std::string& key, + const std::vector& list) +{ + // check if the key already exists + if(prop.check(key)) + { + yError() << "[addVectorOfStringToProperty] The property already exist."; + return false; + } + + prop.addGroup(key); + yarp::os::Bottle& bot = prop.findGroup(key).addList(); + for(size_t i=0; i < list.size(); i++) + bot.addString(list[i].c_str()); + + return true; +} + +bool YarpHelper::yarpListToStringVector(yarp::os::Value*& input, std::vector& output) +{ + // clear the std::vector + output.clear(); + + // check if the yarp value is a list + if(!input->isList()) + { + yError() << "[yarpListToStringVector] The input is not a list."; + return false; + } + + yarp::os::Bottle *bottle = input->asList(); + for(int i = 0; i < bottle->size(); i++) + { + // check if the elements of the bottle are strings + if(!bottle->get(i).isString()) + { + yError() << "[yarpListToStringVector] There is a field that is not a string."; + return false; + } + output.push_back(bottle->get(i).asString()); + } + return true; +} + +bool YarpHelper::getStringFromSearchable(const yarp::os::Searchable& config, const std::string& key, + std::string& string) +{ + yarp::os::Value* value; + if(!config.check(key, value)) + { + yError() << "[getStringFromSearchable] Missing field "<< key; + return false; + } + + if(!value->isString()) + { + yError() << "[getStringFromSearchable] the value is not a string."; + return false; + } + + string = value->asString(); + return true; +} + +bool YarpHelper::getNumberFromSearchable(const yarp::os::Searchable& config, const std::string& key, + double& number) +{ + yarp::os::Value* value; + if(!config.check(key, value)) + { + yError() << "[getNumberFromSearchable] Missing field "<< key; + return false; + } + + if(!value->isDouble()) + { + yError() << "[getNumberFromSearchable] the value is not a double."; + return false; + } + + number = value->asDouble(); + return true; +} + +bool YarpHelper::getNumberFromSearchable(const yarp::os::Searchable& config, const std::string& key, + int& number) +{ + yarp::os::Value* value; + if(!config.check(key, value)) + { + yError() << "[getNumberFromSearchable] Missing field "<< key; + return false; + } + + if(!value->isInt()) + { + yError() << "[getNumberFromSearchable] the value is not an integer."; + return false; + } + + number = value->asInt(); + return true; +} + +void YarpHelper::populateBottleWithStrings(yarp::os::Bottle& bottle, const std::initializer_list& strings) +{ + for(const auto& string : strings) + bottle.addString(string); +} + +double normalizeAnglePositive(const double& angle) +{ + return fmod(fmod(angle, 2.0 * M_PI) + 2.0 * M_PI, 2.0 * M_PI); +} + +double normalizeAngle(const double& angle) +{ + double a = normalizeAnglePositive(angle); + if (a > M_PI) + a -= 2.0 *M_PI; + return a; +} + +double iDynTreeHelper::shortestAngularDistance(const double& fromRad, const double& toRad) +{ + return normalizeAngle(toRad - fromRad); +} diff --git a/src/WalkingLogger.cpp b/src/WalkingLogger.cpp new file mode 100644 index 0000000..3848fa4 --- /dev/null +++ b/src/WalkingLogger.cpp @@ -0,0 +1,91 @@ +/** + * @file WalkingLogger.cpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +// YARP +#include + +#include +#include + +bool WalkingLogger::configure(const yarp::os::Searchable& config, const std::string& name) +{ + std::string portInput, portOutput; + + // check if the config file is empty + if(config.isNull()) + { + yError() << "[configureLogger] Empty configuration for the force torque sensors."; + return false; + } + + // open the connect the data logger port + if(!YarpHelper::getStringFromSearchable(config, "dataLoggerOutputPort_name", portOutput)) + { + yError() << "[configureLogger] Unable to get the string from searchable."; + return false; + } + if(!YarpHelper::getStringFromSearchable(config, "dataLoggerInputPort_name", portInput)) + { + yError() << "[configureLogger] Unable to get the string from searchable."; + return false; + } + m_dataPort.open("/" + name + portOutput); + if(!yarp::os::Network::connect("/" + name + portOutput, portInput)) + { + yError() << "Unable to connect to port " << "/" + name + portOutput; + return false; + } + + // open the connect the rpc logger port + if(!YarpHelper::getStringFromSearchable(config, "dataLoggerRpcOutputPort_name", portOutput)) + { + yError() << "[configureLogger] Unable to get the string from searchable."; + return false; + } + if(!YarpHelper::getStringFromSearchable(config, "dataLoggerRpcInputPort_name", portInput)) + { + yError() << "[configureLogger] Unable to get the string from searchable."; + return false; + } + m_rpcPort.open("/" + name + portOutput); + if(!yarp::os::Network::connect("/" + name + portOutput, portInput)) + { + yError() << "Unable to connect to port " << "/" + name + portOutput; + return false; + } + return true; +} + +bool WalkingLogger::startRecord(const std::initializer_list& strings) +{ + yarp::os::Bottle cmd, outcome; + + YarpHelper::populateBottleWithStrings(cmd, strings); + + m_rpcPort.write(cmd, outcome); + if(outcome.get(0).asInt() != 1) + { + yError() << "[startWalking] Unable to store data"; + return false; + } + return true; +} + +void WalkingLogger::quit() +{ + // stop recording + yarp::os::Bottle cmd, outcome; + cmd.addString("quit"); + m_rpcPort.write(cmd, outcome); + if(outcome.get(0).asInt() != 1) + yInfo() << "[close] Unable to close the stream."; + + // close ports + m_dataPort.close(); + m_rpcPort.close(); +} diff --git a/src/configureEstimator.cpp b/src/configureEstimator.cpp new file mode 100644 index 0000000..ab2d73d --- /dev/null +++ b/src/configureEstimator.cpp @@ -0,0 +1,504 @@ +/* +################################################################################ +# # +# Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia (IIT) # +# All Rights Reserved. # +# # +################################################################################ + +# @authors: Prashanth Ramadoss +# Giulio Romualdi +# Silvio Traversaro +# Daniele Pucci +*/ + +#include + +bool yarp::dev::icubFloatingBaseEstimatorV1::loadEstimatorParametersFromConfig(const yarp::os::Searchable& config) +{ + if (config.check("model_file") && config.find("model_file").isString()) + { + m_model_file_name = config.find("model_file").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"model_file\" parameter in configuration file." << + " Loading default file with name " << m_model_file_name; + } + + if (config.check("robot") && config.find("robot").isString()) + { + m_robot = config.find("robot").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"robot\" parameter in configuration file." << + " Loading default robot name " << m_robot; + } + + if (config.check("device_period_in_seconds") && config.find("device_period_in_seconds").isDouble()) + { + m_device_period_in_s = config.find("device_period_in_seconds").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"device_period_in_seconds\" parameter in configuration file." << + " Loading default device period " << m_device_period_in_s; + } + + if (!getJointNamesList(config, m_estimation_joint_names)) + { + return false; + } + + if (config.check("base_link") && config.find("base_link").isString()) + { + m_base_link_name = config.find("base_link").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"base_link\" parameter in configuration file." << + " Loading default robot name " << m_base_link_name; + } + + if (config.check("left_foot_ft_sensor") && config.find("left_foot_ft_sensor").isString()) + { + m_left_foot_ft_sensor = config.find("left_foot_ft_sensor").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"left_foot_ft_sensor\" parameter in configuration file." << + " Loading default FT sensor name " << m_left_foot_ft_sensor; + } + + if (config.check("right_foot_ft_sensor") && config.find("right_foot_ft_sensor").isString()) + { + m_right_foot_ft_sensor = config.find("right_foot_ft_sensor").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"right_foot_ft_sensor\" parameter in configuration file." << + " Loading default FT sensor name " << m_right_foot_ft_sensor; + } + + if (config.check("attitude_filter_type") && config.find("attitude_filter_type").isString()) + { + m_attitude_estimator_type = config.find("attitude_filter_type").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"attitude_filter_type\" parameter in configuration file." << + " Loading default attitude estimator type " << m_attitude_estimator_type; + } + + if (config.check("initial_attitude_estimate_as_quaternion") && config.find("initial_attitude_estimate_as_quaternion").isList()) + { + yarp::os::Bottle *attitude = config.find("initial_attitude_estimate_as_quaternion").asList(); + if (!attitude || attitude->size() != 4) + { + yWarning() << "floatingBaseEstimatorV1: " << "please mention \"initial_attitude_estimate_as_quaternion\" parameter as a list of 4 doubles."; + yWarning() << "floatingBaseEstimatorV1: " << "uing default value for initial attitude estimate: " << m_initial_reference_frame_for_world; + } + + m_initial_attitude_estimate_as_quaternion.resize(4); + + for (size_t i =0; i < 4; i++) + { + m_initial_attitude_estimate_as_quaternion(i) = attitude->get(i).asDouble(); + } + } + else + { + m_initial_attitude_estimate_as_quaternion.resize(4); + m_initial_attitude_estimate_as_quaternion.zero(); + m_initial_attitude_estimate_as_quaternion(0) = 1.0; + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"initial_attitude_estimate_as_quaternion\" parameter in configuration file." << + "uing default value for initial attitude estimate: " << m_initial_attitude_estimate_as_quaternion.toString(); + } + + if (config.check("imu_confidence_roll") && config.find("imu_confidence_roll").isDouble()) + { + m_imu_confidence_roll = config.find("imu_confidence_roll").asDouble(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"imu_confidence_roll\" parameter in configuration file." << + " using default value " << m_imu_confidence_roll ; + } + + if (config.check("imu_confidence_pitch") && config.find("imu_confidence_pitch").isDouble()) + { + m_imu_confidence_pitch = config.find("imu_confidence_pitch").asDouble(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"imu_confidence_pitch\" parameter in configuration file." << + " using default value " << m_imu_confidence_pitch ; + } + + if (config.check("publish_debug_ports") && config.find("publish_debug_ports").isBool()) + { + m_use_debug_ports = config.find("publish_debug_ports").asBool(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "debug mode set to set to " << m_use_debug_ports; + } + + bool ok = loadLeggedOdometryParametersFromConfig(config); + ok = loadBipedFootContactClassifierParametersFromConfig(config) && ok; + + if (config.check("imu_name") && config.find("imu_name").isString()) + { + m_head_imu_name = config.find("imu_name").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will get IMU measurements from default IMU with name: " << m_head_imu_name ; + } + + if (m_attitude_estimator_type == "mahony") + { + ok = loadIMUAttitudeMahonyEstimatorParametersFromConfig(config) && ok; + } + else if (m_attitude_estimator_type == "qekf") + { + ok = loadIMUAttitudeQEKFParamtersFromConfig(config) && ok; + } + + m_dump_data = config.check("dump_data", yarp::os::Value(false)).asBool(); + + m_use_lpf = config.check("use_low_pass_filters", yarp::os::Value(false)).asBool(); + + if (m_use_lpf) + { + m_joint_vel_filter_cutoff_freq = config.check("joint_vel_lpf_cutoff_freq", yarp::os::Value(0.0)).asDouble(); + m_joint_vel_filter_sample_time_in_s = m_device_period_in_s; + } + + return ok; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::loadLeggedOdometryParametersFromConfig(const yarp::os::Searchable& config) +{ + if (config.check("initial_fixed_frame") && config.find("initial_fixed_frame").isString()) + { + m_initial_fixed_frame = config.find("initial_fixed_frame").asString(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"initial_fixed_frame\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + if (config.check("initial_reference_frame_for_world") && config.find("initial_reference_frame_for_world").isString()) + { + m_initial_reference_frame_for_world = config.find("initial_reference_frame_for_world").asString(); + } + else + { + m_initial_reference_frame_for_world = m_initial_fixed_frame; + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"initial_reference_frame_for_world\" parameter in configuration file." << + "Setting same value as initial fixed frame, " << m_initial_reference_frame_for_world; + m_initial_reference_frame_H_world.Identity(); + } + + if (config.check("initial_reference_frame_xyzrpy_pose_world") && config.find("initial_reference_frame_xyzrpy_pose_world").isList()) + { + yarp::os::Bottle *pose = config.find("initial_reference_frame_xyzrpy_pose_world").asList(); + if (!pose || pose->size() != 6) + { + yWarning() << "floatingBaseEstimatorV1: " << "please mention \"initial_reference_frame_xyzrpy_pose_world\" parameter as a list of 6 doubles."; + return false; + } + + iDynTree::Position initial_reference_frame_p_world; + initial_reference_frame_p_world(0) = pose->get(0).asDouble(); + initial_reference_frame_p_world(1) = pose->get(1).asDouble(); + initial_reference_frame_p_world(2) = pose->get(2).asDouble(); + + iDynTree::Rotation initial_reference_frame_R_world = iDynTree::Rotation::RPY(pose->get(3).asDouble(), + pose->get(4).asDouble(), + pose->get(5).asDouble()); + + m_initial_reference_frame_H_world = iDynTree::Transform(initial_reference_frame_R_world, initial_reference_frame_p_world); + } + else + { + m_initial_reference_frame_for_world = m_initial_fixed_frame; + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"initial_reference_frame_for_world\" parameter in configuration file." << + " Setting same value as initial fixed frame, " << m_initial_reference_frame_for_world; + m_initial_reference_frame_H_world.Identity(); + } + + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::loadBipedFootContactClassifierParametersFromConfig(const yarp::os::Searchable& config) +{ + if (config.check("initial_primary_foot") && config.find("initial_primary_foot").isString()) + { + m_initial_primary_foot = config.find("initial_primary_foot").asString(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Could not find \"initial_primary_foot\" parameter in configuration file." << + " Loading default initial primary foot " << m_initial_primary_foot; + } + + if (config.check("schmitt_stable_contact_make_time") && config.find("schmitt_stable_contact_make_time").isDouble()) + { + m_left_foot_contact_schmitt_params.stableTimeContactMake = config.find("schmitt_stable_contact_make_time").asDouble(); + m_right_foot_contact_schmitt_params.stableTimeContactMake = m_left_foot_contact_schmitt_params.stableTimeContactMake; + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"schmitt_stable_contact_make_time\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + if (config.check("schmitt_stable_contact_break_time") && config.find("schmitt_stable_contact_break_time").isDouble()) + { + m_left_foot_contact_schmitt_params.stableTimeContactBreak = config.find("schmitt_stable_contact_break_time").asDouble(); + m_right_foot_contact_schmitt_params.stableTimeContactBreak = m_left_foot_contact_schmitt_params.stableTimeContactBreak; + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"schmitt_stable_contact_break_time\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + if (config.check("left_schmitt_contact_make_force_threshold") && config.find("left_schmitt_contact_make_force_threshold").isDouble()) + { + m_left_foot_contact_schmitt_params.contactMakeForceThreshold = config.find("left_schmitt_contact_make_force_threshold").asDouble(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"left_schmitt_contact_make_force_threshold\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + if (config.check("left_schmitt_contact_break_force_threshold") && config.find("left_schmitt_contact_break_force_threshold").isDouble()) + { + m_left_foot_contact_schmitt_params.contactBreakForceThreshold = config.find("left_schmitt_contact_break_force_threshold").asDouble(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"left_schmitt_contact_break_force_threshold\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + if (config.check("right_schmitt_contact_make_force_threshold") && config.find("right_schmitt_contact_make_force_threshold").isDouble()) + { + m_right_foot_contact_schmitt_params.contactMakeForceThreshold = config.find("right_schmitt_contact_make_force_threshold").asDouble(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"right_schmitt_contact_make_force_threshold\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + if (config.check("right_schmitt_contact_break_force_threshold") && config.find("right_schmitt_contact_break_force_threshold").isDouble()) + { + m_right_foot_contact_schmitt_params.contactBreakForceThreshold = config.find("right_schmitt_contact_break_force_threshold").asDouble(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"right_schmitt_contact_break_force_threshold\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::loadIMUAttitudeMahonyEstimatorParametersFromConfig(const yarp::os::Searchable& config) +{ + if (config.check("mahony_kp") && config.find("mahony_kp").isDouble()) + { + m_imu_attitude_observer_params.kp = config.find("mahony_kp").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default gain kp: " << m_imu_attitude_observer_params.kp ; + } + + if (config.check("mahony_ki") && config.find("mahony_ki").isDouble()) + { + m_imu_attitude_observer_params.ki = config.find("mahony_ki").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default gain ki: " << m_imu_attitude_observer_params.ki ; + } + + if (config.check("mahony_use_magnetometer") && config.find("mahony_use_magnetometer").isBool()) + { + m_imu_attitude_observer_params.use_magnetometer_measurements = config.find("mahony_use_magnetometer").asBool(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "use magnetometer flag set to " << m_imu_attitude_observer_params.use_magnetometer_measurements; + } + + if (config.check("mahony_discretization_time_step_in_seconds") && config.find("mahony_discretization_time_step_in_seconds").isDouble()) + { + m_imu_attitude_observer_params.time_step_in_seconds = config.find("mahony_discretization_time_step_in_seconds").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default discretization time step: " << m_imu_attitude_observer_params.time_step_in_seconds ; + } + + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::loadIMUAttitudeQEKFParamtersFromConfig(const yarp::os::Searchable& config) +{ + if (config.check("qekf_discretization_time_step_in_seconds") && config.find("qekf_discretization_time_step_in_seconds").isDouble()) + { + m_imu_attitude_qekf_params.time_step_in_seconds = config.find("qekf_discretization_time_step_in_seconds").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default discretization time step: " << m_imu_attitude_qekf_params.time_step_in_seconds ; + } + + if (config.check("qekf_accelerometer_noise_variance") && config.find("qekf_accelerometer_noise_variance").isDouble()) + { + m_imu_attitude_qekf_params.accelerometer_noise_variance = config.find("qekf_accelerometer_noise_variance").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default accelerometer noise variance: " << m_imu_attitude_qekf_params.accelerometer_noise_variance ; + } + + if (config.check("qekf_magnetometer_noise_variance") && config.find("qekf_magnetometer_noise_variance").isDouble()) + { + m_imu_attitude_qekf_params.magnetometer_noise_variance = config.find("qekf_magnetometer_noise_variance").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default magnetometer noise variance: " << m_imu_attitude_qekf_params.magnetometer_noise_variance ; + } + + if (config.check("qekf_gyroscope_noise_variance") && config.find("qekf_gyroscope_noise_variance").isDouble()) + { + m_imu_attitude_qekf_params.gyroscope_noise_variance = config.find("qekf_gyroscope_noise_variance").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default gyroscope noise variance: " << m_imu_attitude_qekf_params.gyroscope_noise_variance ; + } + + if (config.check("qekf_gyro_bias_noise_variance") && config.find("qekf_gyro_bias_noise_variance").isDouble()) + { + m_imu_attitude_qekf_params.gyro_bias_noise_variance = config.find("qekf_gyro_bias_noise_variance").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default gyro bias noise variance: " << m_imu_attitude_qekf_params.gyro_bias_noise_variance ; + } + + if (config.check("qekf_initial_orientation_error_variance") && config.find("qekf_initial_orientation_error_variance").isDouble()) + { + m_imu_attitude_qekf_params.initial_orientation_error_variance = config.find("qekf_initial_orientation_error_variance").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default initial state orientation variance: " << m_imu_attitude_qekf_params.initial_orientation_error_variance ; + } + + if (config.check("qekf_initial_ang_vel_error_variance") && config.find("qekf_initial_ang_vel_error_variance").isDouble()) + { + m_imu_attitude_qekf_params.initial_ang_vel_error_variance = config.find("qekf_initial_ang_vel_error_variance").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default initial state angular velocity variance: " << m_imu_attitude_qekf_params.initial_ang_vel_error_variance ; + } + + if (config.check("qekf_initial_gyro_bias_error_variance") && config.find("qekf_initial_gyro_bias_error_variance").isDouble()) + { + m_imu_attitude_qekf_params.initial_gyro_bias_error_variance = config.find("qekf_initial_gyro_bias_error_variance").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default initial state gyro bias variance: " << m_imu_attitude_qekf_params.initial_gyro_bias_error_variance ; + } + + if (config.check("qekf_bias_correlation_time_factor") && config.find("qekf_bias_correlation_time_factor").isDouble()) + { + m_imu_attitude_qekf_params.bias_correlation_time_factor = config.find("qekf_bias_correlation_time_factor").asDouble(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "Attitude estimator will use default bias forgetting factor: " << m_imu_attitude_qekf_params.bias_correlation_time_factor ; + } + + if (config.check("qekf_use_magnetometer_measurements") && config.find("qekf_use_magnetometer_measurements").isBool()) + { + m_imu_attitude_qekf_params.use_magnetometer_measurements = config.find("qekf_use_magnetometer_measurements").asBool(); + } + else + { + yWarning() << "floatingBaseEstimatorV1: " << "qekf use magnetometer flag set to " << m_imu_attitude_qekf_params.use_magnetometer_measurements; + } + + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::openComms() +{ + bool ok{false}; + ok = m_imu_attitude_observer_estimated_state_port.open(m_port_prefix + "/mahony_state/state:o"); + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << "could not open port " << m_port_prefix + "/mahony_state/state:o"; + return false; + } + + ok = m_imu_attitude_qekf_estimated_state_port.open(m_port_prefix + "/qekf/state:o"); + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << "could not open port " << m_port_prefix + "/qekf/state:o"; + return false; + } + + ok = m_floating_base_state_port.open(m_port_prefix + "/floating_base/configuration:o"); + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << "could not open port " << m_port_prefix + "/floating_base/configuration:o"; + return false; + } + + ok = m_floating_base_pose_port.open(m_port_prefix + "/floating_base/state:o"); + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << "could not open port " << m_port_prefix + "/floating_base/state:o"; + return false; + } + + ok = m_contact_state_port.open(m_port_prefix + "/feet_contact/state:o"); + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << "could not open port " << m_port_prefix + "/feet_contact/state:o"; + return false; + } + + floatingBaseEstimationRPC::yarp().attachAsServer(m_estimator_rpc_port); + ok = m_estimator_rpc_port.open(m_port_prefix + "/rpc"); + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << "could not open port " << m_port_prefix + "rpc"; + return false; + } + + return true; +} diff --git a/src/fbeRobotInterface.cpp b/src/fbeRobotInterface.cpp new file mode 100644 index 0000000..0d56af1 --- /dev/null +++ b/src/fbeRobotInterface.cpp @@ -0,0 +1,559 @@ +/* +################################################################################ +# # +# Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT) # +# All Rights Reserved. # +# # +################################################################################ + +# @authors: Prashanth Ramadoss +# Giulio Romualdi +# Silvio Traversaro +# Daniele Pucci +*/ + +#include + + +bool yarp::dev::icubFloatingBaseEstimatorV1::sensorReadDryRun(bool verbose, bool (yarp::dev::icubFloatingBaseEstimatorV1::*func_t)(bool)) +{ + double tic{yarp::os::Time::now()}; + double time_elapsed_trying_to_read_sensors{0.0}; + bool read_success{false}; + + while ((time_elapsed_trying_to_read_sensors < m_sensor_timeout_in_seconds) && !read_success) + { + read_success = (this->*func_t)(verbose); + time_elapsed_trying_to_read_sensors = (yarp::os::Time::now() - tic); + } + + if (!read_success) + { + yError() << "floatingBaseEstimatorV1: " << "unable to read from sensor correctly for " << m_sensor_timeout_in_seconds << " seconds.. exiting."; + } + return read_success; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::attachAll(const yarp::dev::PolyDriverList& p) +{ + yarp::os::LockGuard guard(m_device_mutex); + if (!attachAllControlBoards(p)) + { + yError() << "icubFloatingBaseEstimatorV1: " << "Could not attach the control boards"; + return false; + } + + if (!loadEstimator()) + { + yError() << "floatingBaseEstimatorV1: " << "Could not load estimator"; + return false; + } + + if (m_use_multiple_analog_sensor_interface) + { + if (!attachMultipleAnalogSensors(p)) + { + yError() << "floatingBaseEstimatorV1: " << "Could not attach the multiple analog sensor interface"; + return false; + } + } + else + { + if (!attachAllInertialMeasurementUnits(p)) + { + yError() << "floatingBaseEstimatorV1: " << "Could not attach the inertial measurement units"; + return false; + } + + if (!attachAllForceTorqueSensors(p)) + { + yError() << "floatingBaseEstimatorV1: " << "Could not attach the force-torque sensors"; + return false; + } + } + + start(); + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::attachAllControlBoards(const yarp::dev::PolyDriverList& p) +{ + bool ok{false}; + for (size_t dev_idx = 0; dev_idx < (size_t)p.size(); dev_idx++) + { + ok = p[dev_idx]->poly->view(m_remapped_control_board_interfaces.encs); + if (ok) + { + break; + } + } + + if (!ok) + { + return false; + } + + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::loadEstimator() +{ + yarp::os::ResourceFinder &rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + std::string model_file_path = rf.findFileByName(m_model_file_name); + yInfo() << "floatingBaseEstimatorV1: " << "Loading model from " + model_file_path; + + iDynTree::ModelLoader model_loader; ///< iDynTree object to load robot model + bool ok = model_loader.loadReducedModelFromFile(model_file_path, m_estimation_joint_names); + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << "Could not load model from specified path."; + return false; + } + + m_model = model_loader.model(); + m_sensors_list = model_loader.sensors(); + m_sensor_measurements.resize(m_sensors_list); + + m_kin_dyn_comp.loadRobotModel(m_model); + + resizeBuffers(); + setPeriod(m_device_period_in_s); + + ok = m_sensors_list.getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE, m_left_foot_ft_sensor, m_left_foot_ft_sensor_index) && ok; + ok = m_sensors_list.getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE, m_right_foot_ft_sensor, m_right_foot_ft_sensor_index) && ok; + + m_r_sole_R_r_ft_sensor = m_kin_dyn_comp.getRelativeTransform(m_model.getFrameIndex(m_right_sole), m_right_foot_ft_sensor_index).getRotation(); + m_l_sole_R_l_ft_sensor = m_kin_dyn_comp.getRelativeTransform(m_model.getFrameIndex(m_left_sole), m_left_foot_ft_sensor_index).getRotation(); + + ok = loadLeggedOdometry(); + ok = loadBipedFootContactClassifier() && ok; + if (m_attitude_estimator_type == "mahony") + { + ok = loadIMUAttitudeMahonyEstimator() && ok; + } + else if (m_attitude_estimator_type == "qekf") + { + ok = loadIMUAttitudeQEKF() && ok; + } + ok = loadTransformBroadcaster() && ok; + + if (!ok) + { + return false; + } + + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::attachMultipleAnalogSensors(const yarp::dev::PolyDriverList& p) +{ + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::attachAllForceTorqueSensors(const yarp::dev::PolyDriverList& p) +{ + std::vector ft_sensor_list; + std::vector ft_sensor_name; + for (size_t dev_idx = 0; dev_idx < (size_t)p.size(); dev_idx++) + { + // check if an analog sensor has 6 channels implying it is a forcetorque sensor + yarp::dev::IAnalogSensor* p_forcetorque = 0; + if (p[dev_idx]->poly->view(p_forcetorque)) + { + if (p_forcetorque->getChannels() == (int)m_nr_of_channels_in_YARP_FT_sensor) + { + ft_sensor_list.push_back(p_forcetorque); + ft_sensor_name.push_back(p[dev_idx]->key); + } + } + } + + if (ft_sensor_list.size() != m_sensors_list.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE)) + { + yError() << "floatingBaseEstimatorV1: " << "Obtained " << m_sensors_list.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE) << "from the model, but trying to attach " << (int)ft_sensor_list.size() << " FT sensors in the attach list."; + return false; + } + + m_whole_body_forcetorque_interface.resize(ft_sensor_list.size()); + for (size_t iDyn_sensor_idx = 0; iDyn_sensor_idx < m_whole_body_forcetorque_interface.size(); iDyn_sensor_idx++) + { + std::string sensor_name = m_sensors_list.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, iDyn_sensor_idx)->getName(); + // search in sensors list for ft sensor with same name as attach list + int idx_of_device_with_same_name{-1}; + for (size_t dev_idx = 0; dev_idx < ft_sensor_list.size(); dev_idx++) + { + if (ft_sensor_name[dev_idx] == sensor_name) + { + idx_of_device_with_same_name = dev_idx; + break; + } + } + + if (idx_of_device_with_same_name == -1) + { + yError() << "floatingBaseEstimatorV1: " << "was expecting a FT sensor with name " << sensor_name; + return false; + } + m_whole_body_forcetorque_interface[iDyn_sensor_idx] = ft_sensor_list[idx_of_device_with_same_name]; + } + + m_nr_of_forcetorque_sensors_detected = m_whole_body_forcetorque_interface.size(); + // dry run of readFTSensors() to check if all FTs work and to initialize buffers + m_ft_measurements_from_yarp_server.resize(m_nr_of_channels_in_YARP_FT_sensor); + bool verbose{false}; + if (!sensorReadDryRun(verbose, &yarp::dev::icubFloatingBaseEstimatorV1::readFTSensors)) + { + return false; + } + + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::attachAllInertialMeasurementUnits(const yarp::dev::PolyDriverList& p) +{ + std::vector imu_sensor_list; + std::vector imu_sensor_name; + for (size_t dev_idx = 0; dev_idx < (size_t)p.size(); dev_idx++) + { + // check if a generic sensor has 12 channels implying it is a IMU sensor + yarp::dev::IGenericSensor* p_IMU = 0; + if (p[dev_idx]->poly->view(p_IMU)) + { + int nr_of_channels{0}; + p_IMU->getChannels(&nr_of_channels); + if (nr_of_channels == (int)m_nr_of_channels_in_YARP_IMU_sensor) + { + imu_sensor_list.push_back(p_IMU); + imu_sensor_name.push_back(p[dev_idx]->key); + } + } + } + + m_whole_body_imu_interface = imu_sensor_list; + m_nr_of_IMUs_detected = m_whole_body_imu_interface.size(); + + if (m_nr_of_IMUs_detected == 0) + { + yError() << "floatingBaseEstimatorV1: " << "Expecting atleast one IMU."; + return false; + } + + + m_raw_IMU_measurements.resize(m_nr_of_IMUs_detected); + // check this so that we can get sensor transforms from idyntree + // note this is a reverse check (different from the check on FT sensors) + for (size_t imu = 0; imu < m_nr_of_IMUs_detected; imu++) + { + bool found_imu{false}; + for (size_t iDyn_sensor_idx = 0; iDyn_sensor_idx < m_sensors_list.getNrOfSensors(iDynTree::ACCELEROMETER); iDyn_sensor_idx++) + { + std::string imu_name = m_sensors_list.getSensor(iDynTree::ACCELEROMETER, iDyn_sensor_idx)->getName(); + if (imu_sensor_name[imu] == imu_name) + { + found_imu = true; + break; + } + } + + if (!found_imu) + { + yError() << "floatingBaseEstimatorV1: " << "was expecting IMU with name from " << imu_sensor_name[imu] << " iDyntree Model"; + return false; + } + m_raw_IMU_measurements[imu].sensor_name = imu_sensor_name[imu]; + } + + // dry run of readIMUSensors() to check if all IMUs work and to initialize buffers + m_imu_meaaurements_from_yarp_server.resize(m_whole_body_imu_interface.size()); + for (size_t imu; imu < (size_t)m_whole_body_imu_interface.size(); imu++) + { + m_imu_meaaurements_from_yarp_server[imu].resize(m_nr_of_channels_in_YARP_IMU_sensor); + } + bool verbose{false}; + if (!sensorReadDryRun(verbose, &yarp::dev::icubFloatingBaseEstimatorV1::readIMUSensors)) + { + return false; + } + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::readIMUSensors(bool verbose) +{ + bool all_IMUs_read_correctly{true}; + for (size_t imu = 0; imu < m_nr_of_IMUs_detected; imu++) + { + // TODO: get sensor name and associated transformation matrix + bool ok{true}; + m_raw_IMU_measurements[imu].angular_acceleration.zero(); + m_raw_IMU_measurements[imu].angular_velocity.zero(); + m_raw_IMU_measurements[imu].linear_proper_acceleration.zero(); + + ok = m_whole_body_imu_interface[imu]->read(m_imu_meaaurements_from_yarp_server[imu]); + m_raw_IMU_measurements[imu].sensor_status = ok; + if (!ok && verbose) + { + yWarning() << "floatingBaseEstimatorV1: " << "unable to read from IMU sensor " << m_raw_IMU_measurements[imu].sensor_name << " correctly. using old measurements."; + } + + if (ok) + { + m_raw_IMU_measurements[imu].angular_velocity(0) = deg2rad(m_imu_meaaurements_from_yarp_server[imu][6]); + m_raw_IMU_measurements[imu].angular_velocity(1) = deg2rad(m_imu_meaaurements_from_yarp_server[imu][7]); + m_raw_IMU_measurements[imu].angular_velocity(2) = deg2rad(m_imu_meaaurements_from_yarp_server[imu][8]); + + m_raw_IMU_measurements[imu].linear_proper_acceleration(0) = m_imu_meaaurements_from_yarp_server[imu][3]; + m_raw_IMU_measurements[imu].linear_proper_acceleration(1) = m_imu_meaaurements_from_yarp_server[imu][4]; + m_raw_IMU_measurements[imu].linear_proper_acceleration(2) = m_imu_meaaurements_from_yarp_server[imu][5]; + } + + all_IMUs_read_correctly = all_IMUs_read_correctly && ok; + } + + return all_IMUs_read_correctly; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::readFTSensors(bool verbose) +{ + bool ft_sensors_read_correctly{true}; + for (size_t ft = 0; ft < m_nr_of_forcetorque_sensors_detected; ft++) + { + iDynTree::Wrench buffer_wrench; + int ft_ret_value = m_whole_body_forcetorque_interface[ft]->read(m_ft_measurements_from_yarp_server); + bool ok = (ft_ret_value == yarp::dev::IAnalogSensor::AS_OK); + ft_sensors_read_correctly = ft_sensors_read_correctly && ok; + + if (!ok && verbose) + { + yWarning() << "floatingBaseEstimatorV1: " << "unable to read from FT sensor " << m_sensors_list.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, ft)->getName() << " correctly. using old measurements."; + } + + bool is_NaN = false; + for (size_t i = 0; i < m_ft_measurements_from_yarp_server.size(); i++) + { + if (std::isnan(m_ft_measurements_from_yarp_server[i])) + { + is_NaN = true; + break; + } + } + + if (is_NaN) + { + yError() << "floatingBaseEstimatorV1: " << "FT sensor " << m_sensors_list.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, ft)->getName() << " contains nan: . using old measurements."<< m_ft_measurements_from_yarp_server.toString(); + return false; + } + + if (ok) + { + iDynTree::toiDynTree(m_ft_measurements_from_yarp_server, buffer_wrench); + m_sensor_measurements.setMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE, ft, buffer_wrench); + } + } + return ft_sensors_read_correctly; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::readEncoders(bool verbose) +{ + int ax; m_remapped_control_board_interfaces.encs->getAxes(&ax); + bool encoders_read_correctly = m_remapped_control_board_interfaces.encs->getEncoders(m_joint_positions.data()); + + encoders_read_correctly = m_remapped_control_board_interfaces.encs->getEncoderSpeeds(m_joint_velocities.data()) && encoders_read_correctly; + if (!encoders_read_correctly && verbose) + { + yWarning() << "floatingBaseEstimatorV1: " << "unable to read from encoders interface properly"; + } + if (m_use_lpf) + { + if (!m_device_initialized_correctly) + { + m_joint_velocities_filter = std::make_unique(m_joint_vel_filter_cutoff_freq, m_device_period_in_s); + m_joint_velocities_filter->setSampleTime(m_device_period_in_s); + m_joint_velocities_filter->setCutFrequency(m_joint_vel_filter_cutoff_freq); + yarp::sig::Vector init_vel; + init_vel.resize(m_joint_velocities.size()); + iDynTree::toYarp(m_joint_velocities, init_vel); + m_joint_velocities_filter->init(init_vel); + } + else + { + yarp::sig::Vector unfilt_vel(m_joint_velocities.size()); + iDynTree::toYarp(m_joint_velocities, unfilt_vel); + const yarp::sig::Vector& filtered_velocities = m_joint_velocities_filter->filt(unfilt_vel); + iDynTree::toiDynTree(filtered_velocities, m_joint_velocities); + } + } + convertVectorFromDegreesToRadians(m_joint_positions); + convertVectorFromDegreesToRadians(m_joint_velocities); + return encoders_read_correctly; +} + + +bool yarp::dev::icubFloatingBaseEstimatorV1::configureWholeBodyDynamics(const yarp::os::Searchable& config) +{ + if (config.check("left_foot_cartesian_wrench_port") && config.find("left_foot_cartesian_wrench_port").isString()) + { + m_left_foot_cartesian_wrench_port_name = config.find("left_foot_cartesian_wrench_port").asString(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"left_foot_cartesian_wrench_port\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + if (config.check("right_foot_cartesian_wrench_port") && config.find("right_foot_cartesian_wrench_port").isString()) + { + m_right_foot_cartesian_wrench_port_name = config.find("right_foot_cartesian_wrench_port").asString(); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"right_foot_cartesian_wrench_port\" parameter in configuration file." << + " Exiting..." ; + return false; + } + + bool ok = m_left_foot_cartesian_wrench_wbd_port.open(m_port_prefix + "/left_foot_cartesian_wrench:i"); + ok = yarp::os::Network::connect(m_left_foot_cartesian_wrench_port_name, m_port_prefix + "/left_foot_cartesian_wrench:i") && ok; + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << " could not connect to " << m_left_foot_cartesian_wrench_port_name; + return false; + } + + ok = m_right_foot_cartesian_wrench_wbd_port.open(m_port_prefix + "/right_foot_cartesian_wrench:i"); + ok = yarp::os::Network::connect(m_right_foot_cartesian_wrench_port_name, m_port_prefix + "/right_foot_cartesian_wrench:i") && ok; + if (!ok) + { + yError() << "floatingBaseEstimatorV1: " << " could not connect to " << m_right_foot_cartesian_wrench_port_name; + return false; + } + + m_right_foot_cartesian_wrench.resize(6); + m_left_foot_cartesian_wrench.resize(6); + + bool verbose{false}; + if (!sensorReadDryRun(verbose, &yarp::dev::icubFloatingBaseEstimatorV1::readWholeBodyDynamicsContactWrenches)) + { + return false; + } + + m_wbd_is_open = true; + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::calibFTSensorsStanding() +{ + yarp::os::RpcClient wbd_rpc_port; + wbd_rpc_port.open("/wholeBodyDynamics/local/rpc"); + yarp::os::Network::connect("/wholeBodyDynamics/local/rpc", "/wholeBodyDynamics/rpc"); + yarp::os::Bottle calib_command; + calib_command.addString("calibStanding"); + calib_command.addString("all"); + yarp::os::Bottle calib_response; + + // writing five times for sanity check + for (int i = 0; i < 5; i++) + { + yarp::os::Time::delay(0.01); + wbd_rpc_port.write(calib_command, calib_response); + } + + if (calib_response.toString() != "[ok]") + { + return false; + } + return true; +} + +bool readCartesianWrenchesFromPorts(yarp::os::BufferedPort& port, yarp::sig::Vector& wrench, bool verbose) +{ + yarp::sig::Vector* raw_wrench; + + raw_wrench = port.read(false); + if (raw_wrench != nullptr) + { + wrench = *raw_wrench; + if (wrench.size() !=6 && verbose) + { + yError() << "floatingBaseEstimatorV1: " << "wrench size mismatch in left foot port."; + } + + bool is_NaN = false; + for (size_t i = 0; i < wrench.size(); i++) + { + if (std::isnan(wrench[i])) + { + is_NaN = true; + break; + } + } + + if (is_NaN) + { + yError() << "floatingBaseEstimatorV1: " << "foot cartesian wrench contains nan: . using old measurements." << wrench.toString(); + return false; + } + } + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::readWholeBodyDynamicsContactWrenches(bool verbose) +{ + bool ok = readCartesianWrenchesFromPorts(m_left_foot_cartesian_wrench_wbd_port, m_left_foot_cartesian_wrench, m_verbose); + ok = readCartesianWrenchesFromPorts(m_right_foot_cartesian_wrench_wbd_port, m_right_foot_cartesian_wrench, m_verbose) && ok; + + return ok; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::readSensors(bool verbose) +{ + bool all_sensors_read_correctly{true}; + all_sensors_read_correctly = readEncoders(m_verbose) && all_sensors_read_correctly; + all_sensors_read_correctly = readFTSensors(m_verbose) && all_sensors_read_correctly; + all_sensors_read_correctly = readIMUSensors(m_verbose) && all_sensors_read_correctly; + all_sensors_read_correctly = readWholeBodyDynamicsContactWrenches(m_verbose) && all_sensors_read_correctly; + + return all_sensors_read_correctly; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::loadTransformBroadcaster() +{ + yarp::os::Property tf_broadcaster_settings; + tf_broadcaster_settings.put("device", "transformClient"); + tf_broadcaster_settings.put("remote", "/transformServer"); + tf_broadcaster_settings.put("local", m_port_prefix + "/transformClient"); + + tf_broadcaster_settings.addGroup("axesNames"); + yarp::os::Bottle& axes_bottle = tf_broadcaster_settings.findGroup("axesNames").addList(); + + + for (size_t i = 0; i < m_estimation_joint_names.size(); i++) + { + axes_bottle.addString(m_estimation_joint_names[i]); + } + + if (!m_transform_broadcaster.open(tf_broadcaster_settings)) + { + yError() << "floatingBaseEstimatorV1: " << "could not open transform broadcaster."; + return false; + } + + if (!m_transform_broadcaster.view(m_transform_interface)) + { + yError() << "floatingBaseEstimatorV1: " << "could not access transform interface"; + return false; + } + + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::detachAll() +{ + yarp::os::LockGuard guard(m_device_mutex); + m_device_initialized_correctly = false; + if (isRunning()) + { + stop(); + } + return true; +} diff --git a/src/icubFloatingBaseEstimatorV1.cpp b/src/icubFloatingBaseEstimatorV1.cpp new file mode 100644 index 0000000..45c0d33 --- /dev/null +++ b/src/icubFloatingBaseEstimatorV1.cpp @@ -0,0 +1,996 @@ +/* +################################################################################ +# # +# Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT) # +# All Rights Reserved. # +# # +################################################################################ + +# @authors: Prashanth Ramadoss +# Giulio Romualdi +# Silvio Traversaro +# Daniele Pucci +*/ + +#include + + +bool yarp::dev::icubFloatingBaseEstimatorV1::getJointNamesList(const yarp::os::Searchable& config, std::vector< std::__cxx11::string >& joint_list) +{ + yarp::os::Property property; + property.fromString(config.toString().c_str()); + + yarp::os::Bottle *property_joints_list = property.find("joints_list").asList(); + if (property_joints_list == 0) + { + yError() << "floatingBaseEstimatorV1: " << "Could not find \"joints_list\" parameter in configuration file."; + return false; + } + + joint_list.resize(property_joints_list->size()); + for (int joint_idx = 0; joint_idx < property_joints_list->size(); joint_idx++) + { + joint_list[joint_idx] = property_joints_list->get(joint_idx).asString().c_str(); + } + return true; +} + +void yarp::dev::icubFloatingBaseEstimatorV1::resizeBuffers() +{ + m_joint_positions.resize(m_model); + m_joint_velocities.resize(m_joint_positions.size()); + m_joint_velocities.zero(); + + m_initial_attitude_estimate_as_quaternion.resize(4); + m_initial_attitude_estimate_as_quaternion.zero(); + m_initial_attitude_estimate_as_quaternion(0) = 1.0; + + m_world_pose_base_in_R6.resize(6); + m_world_velocity_base.resize(6); + m_world_velocity_base_from_imu.resize(6); + m_world_H_base.resize(4, 4); + m_left_foot_cartesian_wrench.resize(6); + m_right_foot_cartesian_wrench.resize(6); + + // wbd contact wrenches, ft sensors and imu measurement buffers are resized in the respective attach methods. +} + +yarp::dev::icubFloatingBaseEstimatorV1::icubFloatingBaseEstimatorV1(double period, yarp::os::ShouldUseSystemClock useSystemClock): PeriodicThread(period, useSystemClock) +{ +} + +yarp::dev::icubFloatingBaseEstimatorV1::icubFloatingBaseEstimatorV1(): PeriodicThread(0.01, yarp::os::ShouldUseSystemClock::No) +{ +} + + +bool yarp::dev::icubFloatingBaseEstimatorV1::open(yarp::os::Searchable& config) +{ + yarp::os::LockGuard guard(m_device_mutex); + if (!configureWholeBodyDynamics(config)) + { + yError() << "floatingBaseEstimatorV1: " << "Could not connect to wholebodydynamics"; + return false; + } + + if (!loadEstimatorParametersFromConfig(config)) + { + yError() << "floatingBaseEstimatorV1: " << "Failed to load settings from configuration file"; + return false; + } + + if (!openComms()) + { + return false; + } + + if (m_dump_data) + { + m_logger = std::make_unique(); + yarp::os::Bottle& loggerOptions = config.findGroup("LOGGER"); + if (!m_logger->configure(loggerOptions, m_port_prefix)) + { + yError() << "[configure] Unable to configure the logger"; + return false; + } + } + + m_state = FilterFSM::IDLE; + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::loadLeggedOdometry() +{ + if (!m_model.setDefaultBaseLink(m_model.getFrameIndex(m_base_link_name))) + { + yWarning() << "floatingBaseEstimatorV1: " << "could not set default base link to " << m_base_link_name << + ". using link " << m_model.getDefaultBaseLink() << " instead."; + } + + m_legged_odometry = std::make_unique(); + if (!m_legged_odometry->setModel(m_model)) + { + yError() << "floatingBaseEstimatorV1: " << "failed to set model for legged odometry"; + return false; + } + + int axes{0}; + m_remapped_control_board_interfaces.encs->getAxes(&axes); + if (axes != (int)m_legged_odometry->model().getNrOfDOFs()) + { + yError() << "floatingBaseEstimatorV1: " << "estimator model and remapped control board interface has inconsistent number of joints"; + return false; + } + + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::loadBipedFootContactClassifier() +{ + m_biped_foot_contact_classifier = std::make_unique(m_left_foot_contact_schmitt_params, m_right_foot_contact_schmitt_params); + m_biped_foot_contact_classifier->setContactSwitchingPattern(iDynTree::ALTERNATE_CONTACT); + + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::loadIMUAttitudeMahonyEstimator() +{ + m_imu_attitude_observer = std::make_unique(); + m_imu_attitude_observer->setGainkp(m_imu_attitude_observer_params.kp); + m_imu_attitude_observer->setGainki(m_imu_attitude_observer_params.ki); + m_imu_attitude_observer->setTimeStepInSeconds(m_imu_attitude_observer_params.time_step_in_seconds); + m_imu_attitude_observer->useMagnetoMeterMeasurements(m_imu_attitude_observer_params.use_magnetometer_measurements); + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::loadIMUAttitudeQEKF() +{ + m_imu_attitude_qekf = std::make_unique(); + m_imu_attitude_qekf->setParameters(m_imu_attitude_qekf_params); + return true; +} + + +bool yarp::dev::icubFloatingBaseEstimatorV1::initializeLeggedOdometry() +{ + bool ok = m_legged_odometry->updateKinematics(m_joint_positions); + ok = ok && m_legged_odometry->init(m_initial_fixed_frame, m_initial_reference_frame_for_world, m_initial_reference_frame_H_world); + yInfo() << "Base link set to: " << m_legged_odometry->model().getLinkName(m_legged_odometry->model().getDefaultBaseLink()); + yInfo() << "Initial world to base transform \n" << m_legged_odometry->getWorldLinkTransform(m_legged_odometry->model().getDefaultBaseLink()).toString(); + + return ok; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::initializeBipedFootContactClassifier() +{ + if (m_initial_primary_foot == "left") + { + m_biped_foot_contact_classifier->setPrimaryFoot(iDynTree::BipedFootContactClassifier::LEFT_FOOT); + } + else if (m_initial_primary_foot == "right") + { + m_biped_foot_contact_classifier->setPrimaryFoot(iDynTree::BipedFootContactClassifier::RIGHT_FOOT); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "initial primary foot not set."; + } + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::initializeIMUAttitudeEstimator() +{ + iDynTree::VectorDynSize state; + state.resize((int)m_imu_attitude_observer->getInternalStateSize()); + state.zero(); + + state(0) = m_initial_attitude_estimate_as_quaternion(0); + state(1) = m_initial_attitude_estimate_as_quaternion(1); + state(2) = m_initial_attitude_estimate_as_quaternion(2); + state(3) = m_initial_attitude_estimate_as_quaternion(3); + + iDynTree::Span stateBuf(state.data(), state.size()); + m_imu_attitude_observer->setInternalState(stateBuf); + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::initializeIMUAttitudeQEKF() +{ + if (!m_imu_attitude_qekf->initializeFilter()) + { + yInfo() << "floatingBaseEstimatorV1: " << "Could not initialize Qekf"; + return false; + } + + iDynTree::Vector10 state; + state.zero(); + + state(0) = m_initial_attitude_estimate_as_quaternion(0); + state(1) = m_initial_attitude_estimate_as_quaternion(1); + state(2) = m_initial_attitude_estimate_as_quaternion(2); + state(3) = m_initial_attitude_estimate_as_quaternion(3); + + iDynTree::Span state_span(state.data(), state.size()); + m_imu_attitude_qekf->setInternalState(state_span); + // Initial state covariance set during loadAttitudeQEKF + + return true; +} + + +void yarp::dev::icubFloatingBaseEstimatorV1::getFeetCartesianWrenches() +{ + // get these wrenches from whole body dynamics to avoid errors due to calibration offsets + m_left_foot_contact_normal_force = m_left_foot_cartesian_wrench(2); + m_right_foot_contact_normal_force = m_right_foot_cartesian_wrench(2); +} + + +bool yarp::dev::icubFloatingBaseEstimatorV1::updateLeggedOdometry() +{ + m_no_foot_in_contact = false; + m_biped_foot_contact_classifier->updateFootContactState(yarp::os::Time::now(), m_left_foot_contact_normal_force, m_right_foot_contact_normal_force); + switch (m_biped_foot_contact_classifier->getPrimaryFoot()) + { + case iDynTree::BipedFootContactClassifier::contactFoot::LEFT_FOOT : + { + m_current_fixed_frame = "l_sole"; + break; + } + case iDynTree::BipedFootContactClassifier::contactFoot::RIGHT_FOOT : + { + m_current_fixed_frame = "r_sole"; + break; + } + case iDynTree::BipedFootContactClassifier::contactFoot::UNKNOWN_FOOT : + { + m_current_fixed_frame = "unknown"; + m_no_foot_in_contact = true; + if (m_previous_fixed_frame != "unknown") + { + yError() << "floatingBaseEstimatorV1: " << "no foot in contact, failed to set fixed link"; + } + return false; + } + } + + if (m_previous_fixed_frame != m_current_fixed_frame) + { + m_legged_odometry_update_went_well = m_legged_odometry->updateKinematics(m_joint_positions); + iDynTree::Transform w_H_fixedFrame = m_legged_odometry->getWorldFrameTransform(m_legged_odometry->model().getFrameIndex(m_current_fixed_frame)); + iDynTree::Position w_H_fixedFrame_position = w_H_fixedFrame.getPosition(); + + // note flat terrain assumption + w_H_fixedFrame_position(2) = 0; + w_H_fixedFrame.setPosition(w_H_fixedFrame_position); + + if (m_legged_odometry->changeFixedFrame(m_current_fixed_frame, w_H_fixedFrame)) + { + yInfo() << "floatingBaseEstimatorV1: " << "legged odometry changed fixed frame to " << m_current_fixed_frame; + } + else + { + m_legged_odometry_update_went_well = false; + } + } + + m_legged_odometry_update_went_well = m_legged_odometry->updateKinematics(m_joint_positions); + + iDynTree::Transform w_H_b = m_legged_odometry->getWorldLinkTransform(m_legged_odometry->model().getDefaultBaseLink()); + //yInfo() << "World Pos: " << w_H_b.getPosition().toString(); + yarp::eigen::toEigen(m_world_pose_base_in_R6).block<3,1>(0,0) = iDynTree::toEigen(w_H_b.getPosition()); + yarp::eigen::toEigen(m_world_pose_base_in_R6).block<3,1>(3,0) = iDynTree::toEigen(w_H_b.getRotation().asRPY()); + iDynTree::toYarp(w_H_b.asHomogeneousTransform(), m_world_H_base); + + return m_legged_odometry_update_went_well; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::updateIMUAttitudeEstimator() +{ + for (size_t imu = 0; imu < (size_t)m_whole_body_imu_interface.size(); imu++) + { + if (m_raw_IMU_measurements[imu].sensor_name == m_head_imu_name) + { + m_imu_attitude_observer->updateFilterWithMeasurements(m_raw_IMU_measurements[imu].linear_proper_acceleration, + m_raw_IMU_measurements[imu].angular_velocity); + + break; + } + } + + m_imu_attitude_observer->propagateStates(); + m_imu_attitude_observer->getOrientationEstimateAsRPY(m_imu_attitude_estimate_as_rpy); + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::updateIMUAttitudeQEKF() +{ + m_imu_attitude_qekf->propagateStates(); + for (size_t imu = 0; imu < (size_t)m_whole_body_imu_interface.size(); imu++) + { + if (m_raw_IMU_measurements[imu].sensor_name == m_head_imu_name) + { + m_imu_attitude_qekf->updateFilterWithMeasurements(m_raw_IMU_measurements[imu].linear_proper_acceleration, + m_raw_IMU_measurements[imu].angular_velocity); + + break; + } + } + return true; +} + +iDynTree::Transform yarp::dev::icubFloatingBaseEstimatorV1::getHeadIMU_H_NeckBaseAtZero() +{ + iDynTree::KinDynComputations temp_kin_comp; + temp_kin_comp.loadRobotModel(m_model); + iDynTree::JointPosDoubleArray initial_positions{m_joint_positions}, initial_velocities{m_joint_positions}; + initial_velocities.zero(); + iDynTree::Vector3 gravity; + gravity.zero(); + gravity(2) = -9.81; + // set neck pitch, roll and yaw to zero + initial_positions(0) = initial_positions(1) = initial_positions(2) = 0.0; + temp_kin_comp.setRobotState(initial_positions, initial_velocities, gravity); + + return temp_kin_comp.getRelativeTransform(m_head_imu_name, "head"); +} + +iDynTree::Transform yarp::dev::icubFloatingBaseEstimatorV1::getHeadIMUCorrectionWithNeckKinematics() +{ + // this funciton returns imu_H_imuAssumingNeckBaseToZero + if (!m_imu_aligned) + { + m_imu_H_neck_base_at_zero = getHeadIMU_H_NeckBaseAtZero(); + m_imu_aligned = true; + } + iDynTree::Transform imu_H_neck_base = m_kin_dyn_comp.getRelativeTransform(m_head_imu_name, "head"); + return imu_H_neck_base*(m_imu_H_neck_base_at_zero.inverse()); +} + + +bool yarp::dev::icubFloatingBaseEstimatorV1::alignIMUFrames() +{ + iDynTree::Rotation b_R_head_imu = m_kin_dyn_comp.getRelativeTransform(m_base_link_name, m_head_imu_name).getRotation(); + iDynTree::Rotation wIMU_R_IMU_0; + if (m_attitude_estimator_type == "qekf") + { + m_imu_attitude_qekf->getOrientationEstimateAsRotationMatrix(wIMU_R_IMU_0); + } + else if (m_attitude_estimator_type == "mahony") + { + m_imu_attitude_observer->getOrientationEstimateAsRotationMatrix(wIMU_R_IMU_0); + } + else + { + yError() << "floatingBaseEstimatorV1: " << "Not using any attitude estimator, cannot align IMU frames"; + } + iDynTree::Rotation w_R_b = iDynTree::Rotation::RPY(m_world_pose_base_in_R6(3), m_world_pose_base_in_R6(4), m_world_pose_base_in_R6(5)); + m_head_imu_calibration_matrix = w_R_b*b_R_head_imu*wIMU_R_IMU_0.inverse(); + return true; +} + +iDynTree::Rotation yarp::dev::icubFloatingBaseEstimatorV1::getBaseOrientationFromIMU() +{ + iDynTree::Rotation wIMU_R_IMU; + if (m_attitude_estimator_type == "mahony") + { + m_imu_attitude_observer->getOrientationEstimateAsRotationMatrix(wIMU_R_IMU); + } + else if (m_attitude_estimator_type == "qekf") + { + m_imu_attitude_qekf->getOrientationEstimateAsRotationMatrix(wIMU_R_IMU); + } + +// iDynTree::Rotation wIMU_R_wIMUNeckAtZero + // = getHeadIMUCorrectionWithNeckKinematics().getRotation(); + iDynTree::Rotation IMU_R_b = m_kin_dyn_comp.getRelativeTransform(m_head_imu_name, m_base_link_name).getRotation(); + + // correct IMU with neck kinematics +// iDynTree::Rotation wIMU_R_b = wIMU_R_wIMUNeckAtZero*wIMU_R_IMU*IMU_R_b; + iDynTree::Rotation wIMU_R_b = wIMU_R_IMU*IMU_R_b; + return (m_head_imu_calibration_matrix * wIMU_R_b); +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::updateBasePoseWithIMUEstimates() +{ + double updated_roll; + double updated_pitch; + iDynTree::Rotation w_R_b_imu = getBaseOrientationFromIMU(); + double weight_imu_on_roll{m_imu_confidence_roll}, weight_imu_on_pitch{m_imu_confidence_roll}; + std::string fixed_link; + + if (m_no_foot_in_contact) + { + weight_imu_on_pitch = 1.0; + weight_imu_on_roll = 1.0; + fixed_link = "r_foot"; + } + else + { + fixed_link = m_current_fixed_frame; + } + + // update base rotation + updated_roll = weight_imu_on_roll*w_R_b_imu.asRPY()(0) + (1 - weight_imu_on_roll) * m_world_pose_base_in_R6(3); + updated_pitch = weight_imu_on_pitch*w_R_b_imu.asRPY()(1) + (1 - weight_imu_on_pitch)* m_world_pose_base_in_R6(4); + + iDynTree::Rotation w_R_b = iDynTree::Rotation::RPY(updated_roll, updated_pitch, m_world_pose_base_in_R6(5)); + + // update base position + iDynTree::Transform w_H_b; + iDynTree::toiDynTree(m_world_H_base, w_H_b); + iDynTree::Transform b_H_fl = m_kin_dyn_comp.getRelativeTransform(m_base_link_name, fixed_link); + iDynTree::Position w_p_fl = (w_H_b*b_H_fl).getPosition(); + iDynTree::Position b_p_fl = b_H_fl.getPosition(); + iDynTree::Position w_p_b = w_p_fl - (w_R_b*b_p_fl); + + w_H_b.setRotation(w_R_b); + //w_H_b.setPosition(w_p_b); + + yarp::eigen::toEigen(m_world_pose_base_in_R6).block<3,1>(0,0) = iDynTree::toEigen(w_H_b.getPosition()); + yarp::eigen::toEigen(m_world_pose_base_in_R6).block<3,1>(3,0) = iDynTree::toEigen(w_H_b.getRotation().asRPY()); + iDynTree::toYarp(w_H_b.asHomogeneousTransform(), m_world_H_base); + + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::updateBaseVelocity() +{ + using iDynTree::toiDynTree; + using iDynTree::toEigen; + using iDynTree::toYarp; + + if (!m_model.getFrameIndex(m_initial_fixed_frame)) + { + return false; + } + iDynTree::Transform w_H_b_estimate; + toiDynTree(m_world_H_base, w_H_b_estimate); + + iDynTree::Vector3 gravity; + gravity(2) = -9.8; + + // we assume here the base velocity is zero, since the following computation + // gives us the floating jacobian which is dependent only on the joint positions + // and floating base pose due to mixed velocity representation + iDynTree::Twist base_velocity; + base_velocity.zero(); + m_kin_dyn_comp.setRobotState(w_H_b_estimate, m_joint_positions, base_velocity, m_joint_velocities, gravity); + + size_t n_joints = m_joint_velocities.size(); + iDynTree::MatrixDynSize contact_jacobian(6, (n_joints + 6)); + iDynTree::MatrixDynSize contact_jacobian_base(6, 6); + iDynTree::MatrixDynSize contact_jacobian_shape(6, n_joints); + + + if (!m_kin_dyn_comp.getFrameFreeFloatingJacobian(m_current_fixed_frame, contact_jacobian)) + { + yWarning() << "floatingBaseEstimatorV1: Could not compute the contact jacobian"; + return false; + } + + toEigen(contact_jacobian_base) = toEigen(contact_jacobian).block(0, 0, 6, 6); + toEigen(contact_jacobian_shape) = toEigen(contact_jacobian).block(0, 6, 6, n_joints); + + auto contact_jacobian_base_inverse{toEigen(contact_jacobian_base).inverse()}; + iDynTree::Vector6 floating_base_velocity; + toEigen(floating_base_velocity) = -contact_jacobian_base_inverse * toEigen(contact_jacobian_shape) * toEigen(m_joint_velocities); + toYarp(floating_base_velocity, m_world_velocity_base); + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::updateBaseVelocityWithIMU() +{ + using iDynTree::toEigen; + iDynTree::Vector3 y_acc; + // works only for waist IMU, waist IMU name is stored in the variable m_head_imu_name + for (size_t imu = 0; imu < (size_t)m_whole_body_imu_interface.size(); imu++) + { + if (m_raw_IMU_measurements[imu].sensor_name == m_head_imu_name) // change my name + { + y_acc = m_raw_IMU_measurements[imu].linear_proper_acceleration; + + break; + } + } + + // w_R_b*b_R_w*w_R_wimu(imu_ang_vel) + iDynTree::Rotation wIMU_R_IMU; + iDynTree::Vector10 attitude_estimator_state; + iDynTree::Span state_buffer(attitude_estimator_state.data(), attitude_estimator_state.size()); + if (m_attitude_estimator_type == "mahony") + { + m_imu_attitude_observer->getOrientationEstimateAsRotationMatrix(wIMU_R_IMU); + m_imu_attitude_observer->getInternalState(state_buffer); + } + else if (m_attitude_estimator_type == "qekf") + { + m_imu_attitude_qekf->getOrientationEstimateAsRotationMatrix(wIMU_R_IMU); + m_imu_attitude_qekf->getInternalState(state_buffer); + } + + iDynTree::Rotation w_R_IMU = m_head_imu_calibration_matrix*wIMU_R_IMU; + iDynTree::Vector3 gravity; + gravity.zero(); + gravity(2) = -9.8; + + iDynTree::Vector6 base_vel; + // compute left trivialized base velocity + auto imu_lin_vel{toEigen(base_vel).block<3, 1>(0, 0)}; + imu_lin_vel = imu_lin_vel + m_device_period_in_s*((toEigen(w_R_IMU)*toEigen(y_acc)) + toEigen(gravity)); + + iDynTree::Vector3 imu_ang_vel; + imu_ang_vel(0) = attitude_estimator_state(4); + imu_ang_vel(1) = attitude_estimator_state(5); + imu_ang_vel(2) = attitude_estimator_state(6); + toEigen(base_vel).block<3, 1>(3, 0) = (toEigen(w_R_IMU)*toEigen(imu_ang_vel)); + + iDynTree::toYarp(base_vel, m_world_velocity_base_from_imu); + return true; +} + + +void yarp::dev::icubFloatingBaseEstimatorV1::publishFloatingBaseState() +{ + yarp::os::Bottle &state_bottle = m_floating_base_state_port.prepare(); + state_bottle.clear(); + for (unsigned int i = 0; i < m_world_pose_base_in_R6.size(); i++) + { + state_bottle.addDouble(m_world_pose_base_in_R6[i]); + } + + for (unsigned int i = 0; i < m_joint_positions.size(); i++) + { + state_bottle.addDouble(m_joint_positions(i)); + } + + m_floating_base_state_port.write(); +} + +void yarp::dev::icubFloatingBaseEstimatorV1::publishFloatingBasePoseVelocity() +{ + yarp::os::Bottle &state_bottle = m_floating_base_pose_port.prepare(); + state_bottle.clear(); + for (unsigned int i = 0; i < m_world_pose_base_in_R6.size(); i++) + { + state_bottle.addDouble(m_world_pose_base_in_R6[i]); + } + + for (unsigned int i = 0; i < m_world_velocity_base.size(); i++) + { + state_bottle.addDouble(m_world_velocity_base[i]); + } + + m_floating_base_pose_port.write(); +} + +void yarp::dev::icubFloatingBaseEstimatorV1::publishContactState() +{ + yarp::os::Bottle &state_bottle = m_contact_state_port.prepare(); + state_bottle.clear(); + state_bottle.addDouble(m_left_foot_contact_normal_force); + state_bottle.addDouble(m_right_foot_contact_normal_force); + state_bottle.addInt(m_biped_foot_contact_classifier->getLeftFootContactState()); + state_bottle.addInt(m_biped_foot_contact_classifier->getRightFootContactState()); + state_bottle.addString(m_current_fixed_frame); + m_contact_state_port.write(); +} + + +void yarp::dev::icubFloatingBaseEstimatorV1::publishIMUAttitudeEstimatorStates() +{ + iDynTree::VectorDynSize attitude_observer_state; + if (m_attitude_estimator_type == "mahony") + { + attitude_observer_state.resize(m_imu_attitude_observer->getInternalStateSize()); + } + else if (m_attitude_estimator_type == "qekf") + { + attitude_observer_state.resize(m_imu_attitude_qekf->getInternalStateSize()); + } + + iDynTree::Span stateBuffer(attitude_observer_state.data(), attitude_observer_state.size()); + + if (m_attitude_estimator_type == "mahony") + { + m_imu_attitude_observer->getInternalState(stateBuffer); + } + else if (m_attitude_estimator_type == "qekf") + { + m_imu_attitude_qekf->getInternalState(stateBuffer); + } + + yarp::os::Bottle &state_bottle = m_imu_attitude_observer_estimated_state_port.prepare(); + state_bottle.clear(); + state_bottle.addDouble(rad2deg(m_imu_attitude_estimate_as_rpy(0))); // orientation roll + state_bottle.addDouble(rad2deg(m_imu_attitude_estimate_as_rpy(1))); // orientation pitch + state_bottle.addDouble(rad2deg(m_imu_attitude_estimate_as_rpy(2))); // orientation yaw + state_bottle.addDouble(rad2deg(attitude_observer_state(4))); // ang vel about x + state_bottle.addDouble(rad2deg(attitude_observer_state(5))); // ang vel about y + state_bottle.addDouble(rad2deg(attitude_observer_state(6))); // ang vel about z + state_bottle.addDouble(rad2deg(attitude_observer_state(7))); // gyro bias about x + state_bottle.addDouble(rad2deg(attitude_observer_state(8))); // gyro bias about y + state_bottle.addDouble(rad2deg(attitude_observer_state(9))); // gyro bias about z + + m_imu_attitude_observer_estimated_state_port.write(); +} + +void yarp::dev::icubFloatingBaseEstimatorV1::publishIMUAttitudeQEKFEstimates() +{ + iDynTree::RPY rpy; + m_imu_attitude_qekf->getOrientationEstimateAsRPY(rpy); + + yarp::os::Bottle &state_bottle = m_imu_attitude_qekf_estimated_state_port.prepare(); + state_bottle.clear(); + state_bottle.addDouble(rad2deg(rpy(0))); + state_bottle.addDouble(rad2deg(rpy(1))); + state_bottle.addDouble(rad2deg(rpy(2))); + + m_imu_attitude_qekf_estimated_state_port.write(); +} + + +void yarp::dev::icubFloatingBaseEstimatorV1::publishTransform() +{ + m_transform_interface->setTransform(m_base_link_name, "world", m_world_H_base); +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::initializeLogger() +{ + m_logger->startRecord({"record","fbe_x", "fbe_y", "fbe_z", + "fbe_roll", "fbe_pitch", "fbe_yaw", + "fbe_v_x", "fbe_v_y", "fbe_v_z", + "fbe_omega_x", "fbe_omega_y", "fbe_omega_z", + "fbe_lf_contact", "fbe_rf_contact", + "fbe_lf_fz", "fbe_rf_fz", + "attEst_roll", "attEst_pitch", "attEst_yaw", + "fbe_v_x_imu", "fbe_v_y_imu", "fbe_v_z_imu"}); + + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::updateLogger() +{ + yarp::sig::Vector feet_contact_state; + yarp::sig::Vector feet_contact_normal_forces; + feet_contact_state.resize(2); + feet_contact_normal_forces.resize(2); + feet_contact_state(0) = m_biped_foot_contact_classifier->getLeftFootContactState(); + feet_contact_state(1) = m_biped_foot_contact_classifier->getRightFootContactState(); + feet_contact_normal_forces(0) = m_left_foot_contact_normal_force; + feet_contact_normal_forces(1) = m_right_foot_contact_normal_force; + m_logger->sendData(m_world_pose_base_in_R6, m_world_velocity_base, feet_contact_state, + feet_contact_normal_forces, + m_imu_attitude_estimate_as_rpy, m_world_velocity_base_from_imu); + return true; +} + + +void yarp::dev::icubFloatingBaseEstimatorV1::publish() +{ + publishFloatingBasePoseVelocity(); + publishContactState(); + + if (m_use_debug_ports) + { + publishFloatingBaseState(); + publishIMUAttitudeEstimatorStates(); + if (m_attitude_estimator_type == "qekf") + { + publishIMUAttitudeQEKFEstimates(); + } + } + publishTransform(); +} + +void yarp::dev::icubFloatingBaseEstimatorV1::run() +{ + yarp::os::LockGuard guard(m_device_mutex); + + if(m_state != FilterFSM::RUNNING) + return; + + // if (!m_device_initialized_correctly) + // { + // calibFTSensorsStanding(); + // } + + if (readSensors(m_verbose)) + { + getFeetCartesianWrenches(); + + if (!m_device_initialized_correctly) + { + bool ok{false}; + ok = initializeLeggedOdometry(); + ok = initializeBipedFootContactClassifier(); + updateLeggedOdometry(); + if (m_attitude_estimator_type == "mahony") + { + ok = initializeIMUAttitudeEstimator(); + } + else if (m_attitude_estimator_type == "qekf") + { + ok = initializeIMUAttitudeQEKF(); + } + ok = alignIMUFrames(); + publish(); + if (m_dump_data) + { + initializeLogger(); + } + m_previous_fixed_frame = m_current_fixed_frame; + m_device_initialized_correctly = ok; + } + else + { + updateLeggedOdometry(); + if (m_attitude_estimator_type == "mahony") + { + updateIMUAttitudeEstimator(); + } + else if (m_attitude_estimator_type == "qekf") + { + updateIMUAttitudeQEKF(); + } + updateBasePoseWithIMUEstimates(); + updateBaseVelocity(); + //updateBaseVelocityWithIMU(); + publish(); + if (m_dump_data) + { + updateLogger(); + } + m_previous_fixed_frame = m_current_fixed_frame; + } + } + else + { + yError() << "floatingBaseEstimatorV1: " << " estimator will not return meaningful estimates, reading sensors failed"; + } + +} + +void yarp::dev::icubFloatingBaseEstimatorV1::closeDevice() +{ + if (!m_imu_attitude_observer_estimated_state_port.isClosed()) + { + m_imu_attitude_observer_estimated_state_port.close(); + } + + if (!m_imu_attitude_qekf_estimated_state_port.isClosed()) + { + m_imu_attitude_qekf_estimated_state_port.close(); + } + + if (!m_floating_base_state_port.isClosed()) + { + m_floating_base_state_port.close(); + } + + if (!m_floating_base_pose_port.isClosed()) + { + m_floating_base_pose_port.close(); + } + + if (!m_contact_state_port.isClosed()) + { + m_contact_state_port.close(); + } + + if (m_estimator_rpc_port.isOpen()) + { + m_estimator_rpc_port.close(); + } + + m_device_initialized_correctly = false; + m_remapped_control_board_interfaces.encs = nullptr; + + if (m_transform_broadcaster.isValid()) + { + m_transform_broadcaster.close(); + } + + m_transform_interface = nullptr; + + if (m_wbd_is_open) + { + if (yarp::os::Network::isConnected(m_left_foot_cartesian_wrench_port_name, m_port_prefix + "/left_foot_cartesian_wrench:i")) + { + yarp::os::Network::disconnect(m_left_foot_cartesian_wrench_port_name, m_port_prefix + "/left_foot_cartesian_wrench:i"); + } + + if (yarp::os::Network::isConnected(m_right_foot_cartesian_wrench_port_name, m_port_prefix + "/right_foot_cartesian_wrench:i")) + { + yarp::os::Network::disconnect(m_right_foot_cartesian_wrench_port_name, m_port_prefix + "/right_foot_cartesian_wrench:i"); + } + } + if (!m_left_foot_cartesian_wrench_wbd_port.isClosed()) + { + m_left_foot_cartesian_wrench_wbd_port.close(); + } + + if (!m_right_foot_cartesian_wrench_wbd_port.isClosed()) + { + m_right_foot_cartesian_wrench_wbd_port.close(); + } + + if (m_dump_data) + { + m_logger->quit(); + } +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::close() +{ + yarp::os::LockGuard guard(m_device_mutex); + closeDevice(); + + return true; +} + +yarp::dev::icubFloatingBaseEstimatorV1::~icubFloatingBaseEstimatorV1() +{ + +} + +/// RPC methods + +bool yarp::dev::icubFloatingBaseEstimatorV1::setMahonyKp(const double kp) +{ + yarp::os::LockGuard guard(m_device_mutex); + m_imu_attitude_observer->setGainkp(kp); + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::setMahonyKi(const double ki) +{ + yarp::os::LockGuard guard(m_device_mutex); + m_imu_attitude_observer->setGainki(ki); + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::setMahonyTimeStep(const double timestep) +{ + yarp::os::LockGuard guard(m_device_mutex); + m_imu_attitude_observer->setTimeStepInSeconds(timestep); + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::setContactSchmittThreshold(const double l_fz_break, const double l_fz_make, + const double r_fz_break, const double r_fz_make) +{ + yarp::os::LockGuard guard(m_device_mutex); + m_biped_foot_contact_classifier->m_leftFootContactClassifier->m_contactSchmitt->setLowValueThreshold(l_fz_break); + m_biped_foot_contact_classifier->m_leftFootContactClassifier->m_contactSchmitt->setHighValueThreshold(l_fz_make); + m_biped_foot_contact_classifier->m_rightFootContactClassifier->m_contactSchmitt->setLowValueThreshold(r_fz_break); + m_biped_foot_contact_classifier->m_rightFootContactClassifier->m_contactSchmitt->setHighValueThreshold(r_fz_make); + + m_biped_foot_contact_classifier->updateFootContactState(yarp::os::Time::now(), m_left_foot_contact_normal_force, + m_right_foot_contact_normal_force); + return true; +} + + +std::string yarp::dev::icubFloatingBaseEstimatorV1::getEstimationJointsList() +{ + yarp::os::LockGuard guard(m_device_mutex); + std::stringstream ss; + for (int i = 0; i < m_estimation_joint_names.size(); i++) + { + if (i != m_estimation_joint_names.size() - 1) + { + ss << m_estimation_joint_names[i] << ","; + } + else + { + ss << m_estimation_joint_names[i]; + break; + } + } + + return ss.str(); +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::setPrimaryFoot(const std::string& primary_foot) +{ + yarp::os::LockGuard guard(m_device_mutex); + if (primary_foot == "right") + { + m_current_fixed_frame = "r_foot"; + m_biped_foot_contact_classifier->setPrimaryFoot(iDynTree::BipedFootContactClassifier::RIGHT_FOOT); + } + else if (primary_foot == "left") + { + m_current_fixed_frame = "l_foot"; + m_biped_foot_contact_classifier->setPrimaryFoot(iDynTree::BipedFootContactClassifier::LEFT_FOOT); + } + else + { + m_current_fixed_frame = "unknown"; + } + + m_previous_fixed_frame = "unknown"; + if (m_current_fixed_frame == "unknown") + { + return false; + } + m_legged_odometry->changeFixedFrame(m_current_fixed_frame); + return true; +} + +std::string yarp::dev::icubFloatingBaseEstimatorV1::getRefFrameForWorld() +{ + yarp::os::LockGuard guard(m_device_mutex); + return m_initial_reference_frame_for_world; +} + +Pose6D yarp::dev::icubFloatingBaseEstimatorV1::getRefPose6DForWorld() +{ + yarp::os::LockGuard guard(m_device_mutex); + Pose6D ref_pose_world; + ref_pose_world.x = m_initial_reference_frame_H_world.getPosition()(0); + ref_pose_world.y = m_initial_reference_frame_H_world.getPosition()(1); + ref_pose_world.z = m_initial_reference_frame_H_world.getPosition()(2); + ref_pose_world.roll = m_initial_reference_frame_H_world.getRotation().asRPY()(0); + ref_pose_world.pitch = m_initial_reference_frame_H_world.getRotation().asRPY()(1); + ref_pose_world.yaw = m_initial_reference_frame_H_world.getRotation().asRPY()(2); + return ref_pose_world; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::resetLeggedOdometry() +{ + yarp::os::LockGuard guard(m_device_mutex); + m_legged_odometry->updateKinematics(m_joint_positions); + bool ok = m_legged_odometry->init(m_initial_fixed_frame, m_initial_reference_frame_for_world, m_initial_reference_frame_H_world); + return ok; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::startFloatingBaseFilter() +{ + yarp::os::LockGuard guard(m_device_mutex); + m_state = FilterFSM::RUNNING; + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::resetLeggedOdometryWithRefFrame(const std::string& ref_frame, + const double x, const double y, const double z, + const double roll, const double pitch, const double yaw) +{ + yarp::os::LockGuard guard(m_device_mutex); + m_initial_reference_frame_for_world = ref_frame; + iDynTree::Position w_p_b(x, y ,z); + iDynTree::Rotation w_R_b{iDynTree::Rotation::RPY(roll, pitch, yaw)}; + + m_initial_reference_frame_H_world = iDynTree::Transform(w_R_b, w_p_b); + m_legged_odometry->updateKinematics(m_joint_positions); + m_initial_fixed_frame = ref_frame; + yInfo() << "Initial fixed frame changed to " << m_initial_fixed_frame; + bool ok = m_legged_odometry->init(m_initial_fixed_frame, m_initial_reference_frame_for_world, m_initial_reference_frame_H_world); + yInfo() << "Initial ref_frame to world transform: " << m_initial_reference_frame_H_world.toString(); + if (ref_frame == "r_sole") + { + m_biped_foot_contact_classifier->setPrimaryFoot(iDynTree::BipedFootContactClassifier::RIGHT_FOOT); + } + else if (ref_frame == "l_sole") + { + m_biped_foot_contact_classifier->setPrimaryFoot(iDynTree::BipedFootContactClassifier::LEFT_FOOT); + } + m_legged_odometry->changeFixedFrame(ref_frame); + + m_state = FilterFSM::RUNNING; + return ok; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::setJointVelocityLPFCutoffFrequency(const double freq) +{ + m_joint_vel_filter_cutoff_freq = freq; + m_joint_velocities_filter->setCutFrequency(freq); + return true; +} + +bool yarp::dev::icubFloatingBaseEstimatorV1::useJointVelocityLPF(const bool flag) +{ + m_use_lpf = flag; + return true; +} diff --git a/thrifts/floatingBaseEstimationRPC.thrift b/thrifts/floatingBaseEstimationRPC.thrift new file mode 100644 index 0000000..35d4de2 --- /dev/null +++ b/thrifts/floatingBaseEstimationRPC.thrift @@ -0,0 +1,40 @@ +/* +################################################################################ +# # +# Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia (IIT) # +# All Rights Reserved. # +# # +################################################################################ + +# @authors: Prashanth Ramadoss +# Giulio Romualdi +# Silvio Traversaro +# Daniele Pucci +*/ + +struct Pose6D +{ + 1: double x; 2: double y; 3: double z; + 4: double roll; 5: double pitch; 6: double yaw; +} + +service floatingBaseEstimationRPC +{ + string getEstimationJointsList(); + bool setMahonyKp(1: double kp); + bool setMahonyKi(1: double ki); + bool setMahonyTimeStep(1: double timestep); + bool setContactSchmittThreshold(1: double l_fz_break, 2: double l_fz_make, + 3: double r_fz_break, 4: double r_fz_make); + + bool setPrimaryFoot(1: string primary_foot); + + bool resetLeggedOdometry(); + bool resetLeggedOdometryWithRefFrame(1: string ref_frame, 2: double x, 3: double y, 4: double z, + 5: double roll, 6: double pitch, 7: double yaw); + string getRefFrameForWorld(); + Pose6D getRefPose6DForWorld(); + bool useJointVelocityLPF(1: bool flag); + bool setJointVelocityLPFCutoffFrequency(1: double freq); + bool startFloatingBaseFilter(); +} From 714c6ccc684abcde00050378b2ddb09d6054ddd9 Mon Sep 17 00:00:00 2001 From: Prashanth Date: Mon, 3 Jun 2019 18:09:58 +0200 Subject: [PATCH 2/5] [fbev1] refactor repo structure --- CMakeLists.txt | 84 ++------- README.md | 83 ++------- app/CMakeLists.txt | 34 ---- cmake/AddInstallRPATHSupport.cmake | 168 ++++++++++++++++++ cmake/AddUninstallTarget.cmake | 70 ++++++++ devices/CMakeLists.txt | 6 + devices/baseEstimatorV1/CMakeLists.txt | 69 +++++++ devices/baseEstimatorV1/README.md | 62 +++++++ devices/baseEstimatorV1/app/CMakeLists.txt | 25 +++ .../robots/iCubGazeboV2_5/fbe-analogsens.xml | 17 +- .../iCubGazeboV2_5/launch-fbe-analogsens.xml | 12 +- .../wholebodydynamics-external.xml | 8 + .../robots/iCubGenova04/fbe-analogsens.xml | 10 +- .../iCubGenova04/launch-fbe-analogsens.xml | 8 + .../wholebodydynamics-external.xml | 8 + devices/baseEstimatorV1/baseEstimatorV1.ini | 4 + .../baseEstimatorV1/include}/Utils.hpp | 0 .../baseEstimatorV1/include}/Utils.tpp | 0 .../include}/WalkingLogger.hpp | 0 .../include}/WalkingLogger.tpp | 0 .../baseEstimatorV1/include/baseEstimatorV1.h | 36 ++-- .../baseEstimatorV1/ros}/fbeViz.launch | 7 + .../baseEstimatorV1/scope}/base_scope.xml | 8 + .../baseEstimatorV1/scope}/base_velocity.xml | 8 + .../baseEstimatorV1/scope}/contact_scope.xml | 13 +- .../scope}/waist_imu_scope.xml | 10 +- .../baseEstimatorV1/src}/Utils.cpp | 0 .../baseEstimatorV1/src}/WalkingLogger.cpp | 0 .../baseEstimatorV1/src/baseEstimatorV1.cpp | 121 ++++++------- .../src}/configureEstimator.cpp | 37 ++-- .../src}/fbeRobotInterface.cpp | 61 +++---- .../thrifts}/floatingBaseEstimationRPC.thrift | 21 +-- icubFloatingBaseEstimatorV1.ini | 4 - 33 files changed, 653 insertions(+), 341 deletions(-) delete mode 100644 app/CMakeLists.txt create mode 100644 cmake/AddInstallRPATHSupport.cmake create mode 100644 cmake/AddUninstallTarget.cmake create mode 100644 devices/CMakeLists.txt create mode 100644 devices/baseEstimatorV1/CMakeLists.txt create mode 100644 devices/baseEstimatorV1/README.md create mode 100644 devices/baseEstimatorV1/app/CMakeLists.txt rename {app => devices/baseEstimatorV1/app}/robots/iCubGazeboV2_5/fbe-analogsens.xml (89%) rename {app => devices/baseEstimatorV1/app}/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml (91%) rename {app/robots/iCubGenova04 => devices/baseEstimatorV1/app/robots/iCubGazeboV2_5}/wholebodydynamics-external.xml (95%) rename {app => devices/baseEstimatorV1/app}/robots/iCubGenova04/fbe-analogsens.xml (94%) rename {app => devices/baseEstimatorV1/app}/robots/iCubGenova04/launch-fbe-analogsens.xml (94%) rename {app/robots/iCubGazeboV2_5 => devices/baseEstimatorV1/app/robots/iCubGenova04}/wholebodydynamics-external.xml (95%) create mode 100644 devices/baseEstimatorV1/baseEstimatorV1.ini rename {include => devices/baseEstimatorV1/include}/Utils.hpp (100%) rename {include => devices/baseEstimatorV1/include}/Utils.tpp (100%) rename {include => devices/baseEstimatorV1/include}/WalkingLogger.hpp (100%) rename {include => devices/baseEstimatorV1/include}/WalkingLogger.tpp (100%) rename include/icubFloatingBaseEstimatorV1.h => devices/baseEstimatorV1/include/baseEstimatorV1.h (95%) rename {ros => devices/baseEstimatorV1/ros}/fbeViz.launch (60%) rename {scope => devices/baseEstimatorV1/scope}/base_scope.xml (96%) rename {scope => devices/baseEstimatorV1/scope}/base_velocity.xml (95%) rename {scope => devices/baseEstimatorV1/scope}/contact_scope.xml (94%) rename {scope => devices/baseEstimatorV1/scope}/waist_imu_scope.xml (92%) rename {src => devices/baseEstimatorV1/src}/Utils.cpp (100%) rename {src => devices/baseEstimatorV1/src}/WalkingLogger.cpp (100%) rename src/icubFloatingBaseEstimatorV1.cpp => devices/baseEstimatorV1/src/baseEstimatorV1.cpp (87%) rename {src => devices/baseEstimatorV1/src}/configureEstimator.cpp (93%) rename {src => devices/baseEstimatorV1/src}/fbeRobotInterface.cpp (87%) rename {thrifts => devices/baseEstimatorV1/thrifts}/floatingBaseEstimationRPC.thrift (57%) delete mode 100644 icubFloatingBaseEstimatorV1.ini diff --git a/CMakeLists.txt b/CMakeLists.txt index a2b4079..3e71aad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,86 +1,26 @@ -################################################################################ -# # -# Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT) # -# All Rights Reserved. # -# # -################################################################################ - -# @authors: Prashanth Ramadoss -# Giulio Romualdi -# Silvio Traversaro -# Daniele Pucci - +# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. cmake_minimum_required(VERSION 3.5) -project(icubFloatingBaseEstimatorV1 - VERSION 1.0 - LANGUAGES CXX) -set(CMAKE_CXX_STANDARD 11) +project(whole-body-estimators LANGUAGES CXX + VERSION 0.1) + +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) + option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) -set(YARP_REQUIRED_VERSION 3.0.1) -set(iDynTree_REQUIRED_VERSION 0.11.0) +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) -set(CMAKE_INCLUDE_CURRENT_DIR TRUE) +set(YARP_REQUIRED_VERSION 3.0.1) find_package(YARP REQUIRED) if(${YARP_VERSION} VERSION_LESS ${YARP_REQUIRED_VERSION}) message(FATAL_ERROR "YARP version ${YARP_VERSION} not sufficient, at least version ${YARP_REQUIRED_VERSION} is required.") endif() +add_subdirectory(devices) -find_package(Eigen3 REQUIRED) -find_package(iDynTree REQUIRED) -if(${iDynTree_VERSION} VERSION_LESS ${iDynTree_REQUIRED_VERSION}) - message(FATAL_ERROR "iDyntree version ${iDynTree_VERSION} not sufficient, at least version ${iDynTree_REQUIRED_VERSION} is required.") -endif() - -find_package(ICUB REQUIRED) -find_package(codyco-modules REQUIRED) - - -set(FBE_HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/include/icubFloatingBaseEstimatorV1.h - ${CMAKE_CURRENT_SOURCE_DIR}/include/WalkingLogger.tpp - ${CMAKE_CURRENT_SOURCE_DIR}/include/WalkingLogger.hpp - ${CMAKE_CURRENT_SOURCE_DIR}/include/Utils.tpp - ${CMAKE_CURRENT_SOURCE_DIR}/include/Utils.hpp) - -set(FBE_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/icubFloatingBaseEstimatorV1.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/fbeRobotInterface.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/configureEstimator.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/WalkingLogger.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/Utils.cpp) - -yarp_prepare_plugin(icubFloatingBaseEstimatorV1 CATEGORY device - TYPE yarp::dev::icubFloatingBaseEstimatorV1 - INCLUDE ${FBE_HEADERS} - DEFAULT ON) - -yarp_add_idl(THRIFT "${CMAKE_CURRENT_SOURCE_DIR}/thrifts/floatingBaseEstimationRPC.thrift") -add_library(floatingBaseEstimationRPC-service STATIC ${THRIFT}) -target_include_directories(floatingBaseEstimationRPC-service PUBLIC ${CMAKE_CURRENT_BINARY_DIR}/include) -target_link_libraries(floatingBaseEstimationRPC-service YARP::YARP_init YARP::YARP_OS) -set_property(TARGET floatingBaseEstimationRPC-service PROPERTY POSITION_INDEPENDENT_CODE ON) - -yarp_add_plugin(icubFloatingBaseEstimatorV1 ${FBE_SOURCES} ${FBE_HEADERS}) - -target_include_directories(icubFloatingBaseEstimatorV1 PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/include - ${Eigen3_INCLUDE_DIRS}) - -target_link_libraries(icubFloatingBaseEstimatorV1 ${YARP_LIBRARIES} - ${iDynTree_LIBRARIES} - floatingBaseEstimationRPC-service - ${codyco-modules_LIBRARIES}) - -yarp_install(TARGETS icubFloatingBaseEstimatorV1 - COMPONENT Runtime - LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}/ - ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}/) - -yarp_install(FILES icubFloatingBaseEstimatorV1.ini - COMPONENT runtime - DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}/) +include(AddUninstallTarget) -add_subdirectory(app) diff --git a/README.md b/README.md index 2d4bef5..f6f05a7 100644 --- a/README.md +++ b/README.md @@ -1,23 +1,21 @@ # whole-body-estimators YARP-based estimators for humanoid robots. -Reported below are the details for using a simple bipedal floating base estimation algorithm alongside walking controllers. - # Overview - [:orange_book: Implementation](#orange_book-implementation) - [:page_facing_up: Dependencies](#page_facing_up-dependencies) - [:hammer: Build the suite](#hammer-build-the-suite) - - [:computer: How to run the simulation](#computer-how-to-run-the-simulation) - - [:running: How to test on iCub](#running-how-to-test-on-icub) # :orange_book: Implementation +The current implementations available in the `devices` folder include, +- `baseEstimatorV1` includes a simple humanoid floating base estimation algorithm fusing legged odometry and IMU attitude estimation, aimed to be used along side walking controllers. ![Floating Base Estimation Algorithm V1](doc/resources/fbeV1.png) + # :page_facing_up: Dependencies * [YARP](http://www.yarp.it/): to handle the comunication with the robot; * [iDynTree](https://github.com/robotology/idyntree/tree/devel): to setup the floating base estimation algorithm; -* [codyco-modules](https://github.com/robotology/codyco-modules): to get contacts information through the whole body dynamics estimation algorithm * [ICUB](https://github.com/robotology/icub-main): to use the utilities like low pass filters from the `ctrLib` library * [Gazebo](http://gazebosim.org/): for the simulation (tested Gazebo 8 and 9). @@ -28,11 +26,11 @@ It must be noted that all of these dependencies can be directly installed togeth # :hammer: Build the suite -## Linux/macOs +## Linux ```sh -git clone https://github.com/robotology/walking-controllers.git -cd walking-controllers +git clone https://github.com/robotology/whole-body-estimators.git +cd whole-body-estimators mkdir build && cd build cmake ../ make @@ -40,65 +38,18 @@ make ``` Notice: `sudo` is not necessary if you specify the `CMAKE_INSTALL_PREFIX`. In this case it is necessary to add in the `.bashrc` or `.bash_profile` the following lines: ``` sh -export BaseEstimator_INSTALL_DIR=/path/where/you/installed -export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${BaseEstimator_INSTALL_DIR}/lib/yarp -export YARP_DATA_DIRS=${YARP_DATA_DIRS}:${BaseEstimator_INSTALL_DIR}/share/yarp:${BaseEstimator_INSTALL_DIR}/lib/yarp +export WBDEstimator_INSTALL_DIR=/path/where/you/installed +export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${WBDEstimator_INSTALL_DIR}/lib/yarp +export YARP_DATA_DIRS=${YARP_DATA_DIRS}:${WBDEstimator_INSTALL_DIR}/share/yarp:${WBDEstimator_INSTALL_DIR}/lib/yarp ``` -# :computer: How to run the simulation -1. Set the `YARP_ROBOT_NAME` environment variable according to the chosen Gazebo model: - ```sh - export YARP_ROBOT_NAME="iCubGazeboV2_5" - ``` -2. Run `yarpserver` - ``` sh - yarpserver --write - ``` -3. Run gazebo and drag and drop iCub (e.g. icubGazeboSim or iCubGazeboV2_5): - - ``` sh - gazebo -slibgazebo_yarp_clock.so - ``` -4. Run `yarprobotinterface` - - ``` sh - YARP_CLOCK=/clock yarprobotinterface --config launch-fbe-analogsens.xml - ``` - This launches both the floating base estimation device and the whole body dynamics device. -5. Reset the offset of the FT sensors. Open a terminal and write - - ``` - yarp rpc /wholeBodyDynamics/rpc - >> resetOffset all - ``` - -6. communicate with the `base-estimator` through RPC service calls: - ``` - yarp rpc /base-estimator/rpc - ``` - the following commands are allowed: - * `startFloatingBaseFilter`: fill this; - * `setContactSchmittThreshold lbreak lmake rbreak rmake`: fill this; - * `setPrimaryFoot foot`: fill this; - * `useJointVelocityLPF flag`: fill this; - * `setJointVelocityLPFCutoffFrequency freq`: fill this; - * `resetLeggedOdometry`: fill this; - * `resetLeggedOdometryWithRefFrame frame x y z roll pitch yaw`: fill this; - * `getRefFrameForWorld`: fill this; +# Using the estimators +- [`baseEstimatorV1`](devices/baseEstimatorV1/README.md) Please follow the documentation available here to run the floating base estimator. -## How to dump data -Before run `yarprobotinterface` check if [`dump_data`](app/robots/iCubGazeboV2_5/fbe-analogsens.xml#L14) is set to `true` - -If `true`, run the Logger Module -``` sh -YARP_CLOCK=/clock WalkingLoggerModule +# Authors +``` +Prashanth Ramadoss +Giulio Romualdi +Silvio Traversaro +Daniele Pucci ``` - -All the data will be saved in the current folder inside a `txt` named `Dataset_YYYY_MM_DD_HH_MM_SS.txt` - -# :running: How to test on iCub -You can follow the same instructions of the simulation section without using `YARP_CLOCK=/clock`. Make sure your `YARP_ROBOT_NAME` is coherent with the name of the robot (e.g. iCubGenova04) -## :warning: Warning -Currently the supported robots are only: -- ``iCubGenova04`` -- ``icubGazeboV2_5`` diff --git a/app/CMakeLists.txt b/app/CMakeLists.txt deleted file mode 100644 index f6e084b..0000000 --- a/app/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -################################################################################ -# # -# Copyright (C) 2018 Fondazione Istitito Italiano di Tecnologia (IIT) # -# All Rights Reserved. # -# # -################################################################################ - -# @authors: Prashanth Ramadoss -# Giulio Romualdi -# Silvio Traversaro -# Daniele Pucci -# -# Thanks to Stefano Dafarra for this CMakeLists.txt - - - -# List the subdirectory (http://stackoverflow.com/questions/7787823/cmake-how-to-get-the-name-of-all-subdirectories-of-a-directory) -macro(SUBDIRLIST result curdir) - file(GLOB children RELATIVE ${curdir} ${curdir}/*) - set(dirlist "") - foreach(child ${children}) - if(IS_DIRECTORY ${curdir}/${child}) - list(APPEND dirlist ${child}) - endif() - endforeach() - set(${result} ${dirlist}) -endmacro() - -# Get list of models -subdirlist(subdirs ${CMAKE_CURRENT_SOURCE_DIR}/robots/) -# Install each model -foreach (dir ${subdirs}) - yarp_install(DIRECTORY robots/${dir} DESTINATION ${YARP_ROBOTS_INSTALL_DIR}) -endforeach () diff --git a/cmake/AddInstallRPATHSupport.cmake b/cmake/AddInstallRPATHSupport.cmake new file mode 100644 index 0000000..a0d12da --- /dev/null +++ b/cmake/AddInstallRPATHSupport.cmake @@ -0,0 +1,168 @@ +#.rst: +# AddInstallRPATHSupport +# ---------------------- +# +# Add support to RPATH during installation to your project:: +# +# add_install_rpath_support([BIN_DIRS dir [dir]] +# [LIB_DIRS dir [dir]] +# [INSTALL_NAME_DIR [dir]] +# [DEPENDS condition [condition]] +# [USE_LINK_PATH]) +# +# Normally (depending on the platform) when you install a shared +# library you can either specify its absolute path as the install name, +# or leave just the library name itself. In the former case the library +# will be correctly linked during run time by all executables and other +# shared libraries, but it must not change its install location. This +# is often the case for libraries installed in the system default +# library directory (e.g. ``/usr/lib``). +# In the latter case, instead, the library can be moved anywhere in the +# file system but at run time the dynamic linker must be able to find +# it. This is often accomplished by setting environmental variables +# (i.e. ``LD_LIBRARY_PATH`` on Linux). +# This procedure is usually not desirable for two main reasons: +# +# - by setting the variable you are changing the default behaviour +# of the dynamic linker thus potentially breaking executables (not as +# destructive as ``LD_PRELOAD``) +# - the variable will be used only by applications spawned by the shell +# and not by other processes. +# +# RPATH is aimed to solve the issues introduced by the second +# installation method. Using run-path dependent libraries you can +# create a directory structure containing executables and dependent +# libraries that users can relocate without breaking it. +# A run-path dependent library is a dependent library whose complete +# install name is not known when the library is created. +# Instead, the library specifies that the dynamic loader must resolve +# the library’s install name when it loads the executable that depends +# on the library. The executable or the other shared library will +# hardcode in the binary itself the additional search directories +# to be passed to the dynamic linker. This works great in conjunction +# with relative paths. +# This command will enable support to RPATH to your project. +# It will enable the following things: +# +# - If the project builds shared libraries it will generate a run-path +# enabled shared library, i.e. its install name will be resolved +# only at run time. +# - In all cases (building executables and/or shared libraries) +# dependent shared libraries with RPATH support will be properly +# +# The command has the following parameters: +# +# Options: +# - ``USE_LINK_PATH``: if passed the command will automatically adds to +# the RPATH the path to all the dependent libraries. +# +# Arguments: +# - ``BIN_DIRS`` list of directories when the targets (executable and +# plugins) will be installed. +# - ``LIB_DIRS`` list of directories to be added to the RPATH. These +# directories will be added "relative" w.r.t. the ``BIN_DIRS`` and +# ``LIB_DIRS``. +# - ``INSTALL_NAME_DIR`` directory where the libraries will be installed. +# This variable will be used only if ``CMAKE_SKIP_RPATH`` or +# ``CMAKE_SKIP_INSTALL_RPATH`` is set to ``TRUE`` as it will set the +# ``INSTALL_NAME_DIR`` on all targets +# - ``DEPENDS`` list of conditions that should be ``TRUE`` to enable +# RPATH, for example ``FOO; NOT BAR``. +# +# Note: see https://gitlab.kitware.com/cmake/cmake/issues/16589 for further +# details. + +#======================================================================= +# Copyright 2014 Istituto Italiano di Tecnologia (IIT) +# @author Francesco Romano +# +# Distributed under the OSI-approved BSD License (the "License"); +# see accompanying file Copyright.txt for details. +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#======================================================================= +# (To distribute this file outside of CMake, substitute the full +# License text for the above reference.) + + +include(CMakeParseArguments) + + +function(ADD_INSTALL_RPATH_SUPPORT) + + set(_options USE_LINK_PATH) + set(_oneValueArgs INSTALL_NAME_DIR) + set(_multiValueArgs BIN_DIRS + LIB_DIRS + DEPENDS) + + cmake_parse_arguments(_ARS "${_options}" + "${_oneValueArgs}" + "${_multiValueArgs}" + "${ARGN}") + + # if either RPATH or INSTALL_RPATH is disabled + # and the INSTALL_NAME_DIR variable is set, then hardcode the install name + if(CMAKE_SKIP_RPATH OR CMAKE_SKIP_INSTALL_RPATH) + if(DEFINED _ARS_INSTALL_NAME_DIR) + set(CMAKE_INSTALL_NAME_DIR ${_ARS_INSTALL_NAME_DIR} PARENT_SCOPE) + endif() + endif() + + if (CMAKE_SKIP_RPATH OR (CMAKE_SKIP_INSTALL_RPATH AND CMAKE_SKIP_BUILD_RPATH)) + return() + endif() + + + set(_rpath_available 1) + if(DEFINED _ARS_DEPENDS) + foreach(_dep ${_ARS_DEPENDS}) + string(REGEX REPLACE " +" ";" _dep "${_dep}") + if(NOT (${_dep})) + set(_rpath_available 0) + endif() + endforeach() + endif() + + if(_rpath_available) + + # Enable RPATH on OSX. + set(CMAKE_MACOSX_RPATH TRUE PARENT_SCOPE) + + # Find system implicit lib directories + set(_system_lib_dirs ${CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES}) + if(EXISTS "/etc/debian_version") # is this a debian system ? + if(CMAKE_LIBRARY_ARCHITECTURE) + list(APPEND _system_lib_dirs "/lib/${CMAKE_LIBRARY_ARCHITECTURE}" + "/usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}") + endif() + endif() + # This is relative RPATH for libraries built in the same project + foreach(lib_dir ${_ARS_LIB_DIRS}) + list(FIND _system_lib_dirs "${lib_dir}" isSystemDir) + if("${isSystemDir}" STREQUAL "-1") + foreach(bin_dir ${_ARS_LIB_DIRS} ${_ARS_BIN_DIRS}) + file(RELATIVE_PATH _rel_path ${bin_dir} ${lib_dir}) + if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + list(APPEND CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}") + else() + list(APPEND CMAKE_INSTALL_RPATH "$ORIGIN/${_rel_path}") + endif() + endforeach() + endif() + endforeach() + if(NOT "${CMAKE_INSTALL_RPATH}" STREQUAL "") + list(REMOVE_DUPLICATES CMAKE_INSTALL_RPATH) + endif() + set(CMAKE_INSTALL_RPATH ${CMAKE_INSTALL_RPATH} PARENT_SCOPE) + + # add the automatically determined parts of the RPATH + # which point to directories outside the build tree to the install RPATH + set(CMAKE_INSTALL_RPATH_USE_LINK_PATH ${_ARS_USE_LINK_PATH} PARENT_SCOPE) + + endif() + +endfunction() + diff --git a/cmake/AddUninstallTarget.cmake b/cmake/AddUninstallTarget.cmake new file mode 100644 index 0000000..cd45be5 --- /dev/null +++ b/cmake/AddUninstallTarget.cmake @@ -0,0 +1,70 @@ +#.rst: +# AddUninstallTarget +# ------------------ +# +# Add the "uninstall" target for your project:: +# +# include(AddUninstallTarget) +# +# +# will create a file cmake_uninstall.cmake in the build directory and add a +# custom target uninstall that will remove the files installed by your package +# (using install_manifest.txt) + +#============================================================================= +# Copyright 2008-2013 Kitware, Inc. +# Copyright 2013 iCub Facility, Istituto Italiano di Tecnologia +# Authors: Daniele E. Domenichelli +# +# Distributed under the OSI-approved BSD License (the "License"); +# see accompanying file Copyright.txt for details. +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#============================================================================= +# (To distribute this file outside of CMake, substitute the full +# License text for the above reference.) + + +if(DEFINED __ADD_UNINSTALL_TARGET_INCLUDED) + return() +endif() +set(__ADD_UNINSTALL_TARGET_INCLUDED TRUE) + + +set(_filename ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake) + +file(WRITE ${_filename} + "if(NOT EXISTS \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\") + message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\") + return() +endif() +file(READ \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\" files) +string(STRIP \"${files}\" files) +string(REGEX REPLACE \"\\n\" \";\" files \"${files}\") +list(REVERSE files) +foreach(file ${files}) + message(STATUS \"Uninstalling: $ENV{DESTDIR}${file}\") + if(EXISTS \"$ENV{DESTDIR}${file}\") + execute_process( + COMMAND ${CMAKE_COMMAND} -E remove \"$ENV{DESTDIR}${file}\" + OUTPUT_VARIABLE rm_out + RESULT_VARIABLE rm_retval) + if(NOT \"${rm_retval}\" EQUAL 0) + message(FATAL_ERROR \"Problem when removing \\\"$ENV{DESTDIR}${file}\\\"\") + endif() + else() + message(STATUS \"File \\\"$ENV{DESTDIR}${file}\\\" does not exist.\") + endif() +endforeach(file) +") + +if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)") + set(_uninstall "UNINSTALL") +else() + set(_uninstall "uninstall") +endif() +add_custom_target(${_uninstall} COMMAND "${CMAKE_COMMAND}" -P "${_filename}") +set_property(TARGET ${_uninstall} PROPERTY FOLDER "CMakePredefinedTargets") + diff --git a/devices/CMakeLists.txt b/devices/CMakeLists.txt new file mode 100644 index 0000000..c9baf6c --- /dev/null +++ b/devices/CMakeLists.txt @@ -0,0 +1,6 @@ +# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +add_subdirectory(baseEstimatorV1) + diff --git a/devices/baseEstimatorV1/CMakeLists.txt b/devices/baseEstimatorV1/CMakeLists.txt new file mode 100644 index 0000000..4524b61 --- /dev/null +++ b/devices/baseEstimatorV1/CMakeLists.txt @@ -0,0 +1,69 @@ +# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +set(iDynTree_REQUIRED_VERSION 0.11.0) + +set(CMAKE_INCLUDE_CURRENT_DIR TRUE) + +option(ENABLE_RPATH "Enable RPATH for this library" ON) +mark_as_advanced(ENABLE_RPATH) +include(AddInstallRPATHSupport) +add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}" + LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}" + INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}" + DEPENDS ENABLE_RPATH + USE_LINK_PATH) + +find_package(Eigen3 REQUIRED) +find_package(iDynTree REQUIRED) +if(${iDynTree_VERSION} VERSION_LESS ${iDynTree_REQUIRED_VERSION}) + message(FATAL_ERROR "iDyntree version ${iDynTree_VERSION} not sufficient, at least version ${iDynTree_REQUIRED_VERSION} is required.") +endif() + +find_package(ICUB REQUIRED) + +set(FBE_HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/include/baseEstimatorV1.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/WalkingLogger.tpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/WalkingLogger.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/Utils.tpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/Utils.hpp) + +set(FBE_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/baseEstimatorV1.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/fbeRobotInterface.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/configureEstimator.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/WalkingLogger.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/Utils.cpp) + +yarp_prepare_plugin(baseEstimatorV1 CATEGORY device + TYPE yarp::dev::baseEstimatorV1 + INCLUDE ${FBE_HEADERS} + DEFAULT ON) + +yarp_add_idl(THRIFT "${CMAKE_CURRENT_SOURCE_DIR}/thrifts/floatingBaseEstimationRPC.thrift") +add_library(floatingBaseEstimationRPC-service STATIC ${THRIFT}) +target_include_directories(floatingBaseEstimationRPC-service PUBLIC ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}/include) +target_link_libraries(floatingBaseEstimationRPC-service YARP::YARP_init YARP::YARP_OS) +set_property(TARGET floatingBaseEstimationRPC-service PROPERTY POSITION_INDEPENDENT_CODE ON) + +yarp_add_plugin(baseEstimatorV1 ${FBE_SOURCES} ${FBE_HEADERS}) + +target_include_directories(baseEstimatorV1 PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${Eigen3_INCLUDE_DIRS}) + +target_link_libraries(baseEstimatorV1 ${YARP_LIBRARIES} + ${iDynTree_LIBRARIES} + floatingBaseEstimationRPC-service) + +yarp_install(TARGETS baseEstimatorV1 + COMPONENT Runtime + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}/ + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}/) + +yarp_install(FILES baseEstimatorV1.ini + COMPONENT runtime + DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}/) + +add_subdirectory(app) + diff --git a/devices/baseEstimatorV1/README.md b/devices/baseEstimatorV1/README.md new file mode 100644 index 0000000..3e9ef44 --- /dev/null +++ b/devices/baseEstimatorV1/README.md @@ -0,0 +1,62 @@ +# Overview +- [:computer: How to run the simulation](#computer-how-to-run-the-simulation) +- [:running: How to test on iCub](#running-how-to-test-on-icub) + +# :computer: How to run the simulation +1. Set the `YARP_ROBOT_NAME` environment variable according to the chosen Gazebo model: + ```sh + export YARP_ROBOT_NAME="iCubGazeboV2_5" + ``` +2. Run `yarpserver` + ``` sh + yarpserver --write + ``` +3. Run gazebo and drag and drop iCub (e.g. icubGazeboSim or iCubGazeboV2_5): + + ``` sh + gazebo -slibgazebo_yarp_clock.so + ``` +4. Run `yarprobotinterface` with corresponding device configuration file. + - To run `baseEstimatorV1`, + + ``` sh + YARP_CLOCK=/clock yarprobotinterface --config launch-fbe-analogsens.xml + ``` + This launches both the floating base estimation device and the whole body dynamics device. +5. Reset the offset of the FT sensors. Open a terminal and write + + ``` + yarp rpc /wholeBodyDynamics/rpc + >> resetOffset all + ``` + +6. communicate with the `base-estimator` through RPC service calls: + ``` + yarp rpc /base-estimator/rpc + ``` + the following commands are allowed: + * `startFloatingBaseFilter`: fill this; + * `setContactSchmittThreshold lbreak lmake rbreak rmake`: fill this; + * `setPrimaryFoot foot`: fill this; + * `useJointVelocityLPF flag`: fill this; + * `setJointVelocityLPFCutoffFrequency freq`: fill this; + * `resetLeggedOdometry`: fill this; + * `resetLeggedOdometryWithRefFrame frame x y z roll pitch yaw`: fill this; + * `getRefFrameForWorld`: fill this; + +## How to dump data +Before run `yarprobotinterface` check if [`dump_data`](app/robots/iCubGazeboV2_5/fbe-analogsens.xml#L14) is set to `true` + +If `true`, run the Logger Module +``` sh +YARP_CLOCK=/clock WalkingLoggerModule +``` + +All the data will be saved in the current folder inside a `txt` named `Dataset_YYYY_MM_DD_HH_MM_SS.txt` + +# :running: How to test on iCub +You can follow the same instructions of the simulation section without using `YARP_CLOCK=/clock`. Make sure your `YARP_ROBOT_NAME` is coherent with the name of the robot (e.g. iCubGenova04) +## :warning: Warning +Currently the supported robots are only: +- ``iCubGenova04`` +- ``icubGazeboV2_5`` diff --git a/devices/baseEstimatorV1/app/CMakeLists.txt b/devices/baseEstimatorV1/app/CMakeLists.txt new file mode 100644 index 0000000..bfd8a52 --- /dev/null +++ b/devices/baseEstimatorV1/app/CMakeLists.txt @@ -0,0 +1,25 @@ +# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +# Thanks to Stefano Dafarra for this CMakeLists.txt + +# List the subdirectory (http://stackoverflow.com/questions/7787823/cmake-how-to-get-the-name-of-all-subdirectories-of-a-directory) +macro(SUBDIRLIST result curdir) + file(GLOB children RELATIVE ${curdir} ${curdir}/*) + set(dirlist "") + foreach(child ${children}) + if(IS_DIRECTORY ${curdir}/${child}) + list(APPEND dirlist ${child}) + endif() + endforeach() + set(${result} ${dirlist}) +endmacro() + +# Get list of models +subdirlist(subdirs ${CMAKE_CURRENT_SOURCE_DIR}/robots/) +# Install each model +foreach (dir ${subdirs}) + yarp_install(DIRECTORY robots/${dir} DESTINATION ${YARP_ROBOTS_INSTALL_DIR}) +endforeach () + diff --git a/app/robots/iCubGazeboV2_5/fbe-analogsens.xml b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/fbe-analogsens.xml similarity index 89% rename from app/robots/iCubGazeboV2_5/fbe-analogsens.xml rename to devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/fbe-analogsens.xml index f7e1241..2a24309 100644 --- a/app/robots/iCubGazeboV2_5/fbe-analogsens.xml +++ b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/fbe-analogsens.xml @@ -1,6 +1,13 @@ + - + model.urdf icubSim @@ -31,7 +38,8 @@ 0.7 0.001 false - root_link_ems_acc_eb5 + + head_imu_acc_1x1 (0 0 0) @@ -70,8 +78,8 @@ all_joints_mc - - inertial + inertial + left_upper_arm_strain right_upper_arm_strain @@ -86,3 +94,4 @@ + diff --git a/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml similarity index 91% rename from app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml rename to devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml index 5f5ec46..3800914 100644 --- a/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml +++ b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml @@ -1,3 +1,10 @@ + @@ -35,8 +42,8 @@ - /icubSim/waist/inertial - /baseestimation/waist/imu:i + /icubSim/inertial + /baseestimation/head/imu:i @@ -82,3 +89,4 @@ + diff --git a/app/robots/iCubGenova04/wholebodydynamics-external.xml b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/wholebodydynamics-external.xml similarity index 95% rename from app/robots/iCubGenova04/wholebodydynamics-external.xml rename to devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/wholebodydynamics-external.xml index af4a8f3..abcd0f2 100644 --- a/app/robots/iCubGenova04/wholebodydynamics-external.xml +++ b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/wholebodydynamics-external.xml @@ -1,3 +1,10 @@ + @@ -71,3 +78,4 @@ + diff --git a/app/robots/iCubGenova04/fbe-analogsens.xml b/devices/baseEstimatorV1/app/robots/iCubGenova04/fbe-analogsens.xml similarity index 94% rename from app/robots/iCubGenova04/fbe-analogsens.xml rename to devices/baseEstimatorV1/app/robots/iCubGenova04/fbe-analogsens.xml index e55afab..e1fb908 100644 --- a/app/robots/iCubGenova04/fbe-analogsens.xml +++ b/devices/baseEstimatorV1/app/robots/iCubGenova04/fbe-analogsens.xml @@ -1,6 +1,13 @@ + - + model.urdf icub @@ -87,3 +94,4 @@ + diff --git a/app/robots/iCubGenova04/launch-fbe-analogsens.xml b/devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml similarity index 94% rename from app/robots/iCubGenova04/launch-fbe-analogsens.xml rename to devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml index 22265f9..ef60cbf 100644 --- a/app/robots/iCubGenova04/launch-fbe-analogsens.xml +++ b/devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml @@ -1,3 +1,10 @@ + @@ -83,3 +90,4 @@ + diff --git a/app/robots/iCubGazeboV2_5/wholebodydynamics-external.xml b/devices/baseEstimatorV1/app/robots/iCubGenova04/wholebodydynamics-external.xml similarity index 95% rename from app/robots/iCubGazeboV2_5/wholebodydynamics-external.xml rename to devices/baseEstimatorV1/app/robots/iCubGenova04/wholebodydynamics-external.xml index af4a8f3..abcd0f2 100644 --- a/app/robots/iCubGazeboV2_5/wholebodydynamics-external.xml +++ b/devices/baseEstimatorV1/app/robots/iCubGenova04/wholebodydynamics-external.xml @@ -1,3 +1,10 @@ + @@ -71,3 +78,4 @@ + diff --git a/devices/baseEstimatorV1/baseEstimatorV1.ini b/devices/baseEstimatorV1/baseEstimatorV1.ini new file mode 100644 index 0000000..0b8448e --- /dev/null +++ b/devices/baseEstimatorV1/baseEstimatorV1.ini @@ -0,0 +1,4 @@ +[plugin baseEstimatorV1] +type device +name baseEstimatorV1 +library baseEstimatorV1 diff --git a/include/Utils.hpp b/devices/baseEstimatorV1/include/Utils.hpp similarity index 100% rename from include/Utils.hpp rename to devices/baseEstimatorV1/include/Utils.hpp diff --git a/include/Utils.tpp b/devices/baseEstimatorV1/include/Utils.tpp similarity index 100% rename from include/Utils.tpp rename to devices/baseEstimatorV1/include/Utils.tpp diff --git a/include/WalkingLogger.hpp b/devices/baseEstimatorV1/include/WalkingLogger.hpp similarity index 100% rename from include/WalkingLogger.hpp rename to devices/baseEstimatorV1/include/WalkingLogger.hpp diff --git a/include/WalkingLogger.tpp b/devices/baseEstimatorV1/include/WalkingLogger.tpp similarity index 100% rename from include/WalkingLogger.tpp rename to devices/baseEstimatorV1/include/WalkingLogger.tpp diff --git a/include/icubFloatingBaseEstimatorV1.h b/devices/baseEstimatorV1/include/baseEstimatorV1.h similarity index 95% rename from include/icubFloatingBaseEstimatorV1.h rename to devices/baseEstimatorV1/include/baseEstimatorV1.h index 0e53d5b..5254d13 100644 --- a/include/icubFloatingBaseEstimatorV1.h +++ b/devices/baseEstimatorV1/include/baseEstimatorV1.h @@ -1,19 +1,13 @@ /* -################################################################################ -# # -# Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT) # -# All Rights Reserved. # -# # -################################################################################ - -# @authors: Prashanth Ramadoss -# Giulio Romualdi -# Silvio Traversaro -# Daniele Pucci -*/ - -#ifndef ICUB_FLOATING_BASE_ESTIMATOR_V1_H -#define ICUB_FLOATING_BASE_ESTIMATOR_V1_H + * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BASE_ESTIMATOR_V1_H +#define BASE_ESTIMATOR_V1_H #include #include @@ -24,7 +18,7 @@ #include #include #include -#include +#include #include #include @@ -71,15 +65,15 @@ namespace yarp { namespace dev { - class icubFloatingBaseEstimatorV1 : public yarp::dev::DeviceDriver, + class baseEstimatorV1 : public yarp::dev::DeviceDriver, public yarp::dev::IMultipleWrapper, public yarp::os::PeriodicThread, public floatingBaseEstimationRPC { public: - explicit icubFloatingBaseEstimatorV1(double period, yarp::os::ShouldUseSystemClock useSystemClock = yarp::os::ShouldUseSystemClock::No); - icubFloatingBaseEstimatorV1(); - ~icubFloatingBaseEstimatorV1(); + explicit baseEstimatorV1(double period, yarp::os::ShouldUseSystemClock useSystemClock = yarp::os::ShouldUseSystemClock::No); + baseEstimatorV1(); + ~baseEstimatorV1(); /** * @brief Open the estimator device @@ -173,7 +167,7 @@ namespace yarp { * @param[in] func_t pointer of readFunc() * @return true/false success/failure */ - bool sensorReadDryRun(bool verbose, bool (icubFloatingBaseEstimatorV1::*func_t)(bool)); + bool sensorReadDryRun(bool verbose, bool (baseEstimatorV1::*func_t)(bool)); /** * @brief read IMU sensors diff --git a/ros/fbeViz.launch b/devices/baseEstimatorV1/ros/fbeViz.launch similarity index 60% rename from ros/fbeViz.launch rename to devices/baseEstimatorV1/ros/fbeViz.launch index 9d550b3..6dfb57b 100644 --- a/ros/fbeViz.launch +++ b/devices/baseEstimatorV1/ros/fbeViz.launch @@ -1,3 +1,10 @@ + diff --git a/scope/base_scope.xml b/devices/baseEstimatorV1/scope/base_scope.xml similarity index 96% rename from scope/base_scope.xml rename to devices/baseEstimatorV1/scope/base_scope.xml index cbe3f29..f4f73f9 100644 --- a/scope/base_scope.xml +++ b/devices/baseEstimatorV1/scope/base_scope.xml @@ -1,3 +1,10 @@ + + diff --git a/scope/base_velocity.xml b/devices/baseEstimatorV1/scope/base_velocity.xml similarity index 95% rename from scope/base_velocity.xml rename to devices/baseEstimatorV1/scope/base_velocity.xml index 3c5630e..4155617 100644 --- a/scope/base_velocity.xml +++ b/devices/baseEstimatorV1/scope/base_velocity.xml @@ -1,3 +1,10 @@ + + diff --git a/scope/contact_scope.xml b/devices/baseEstimatorV1/scope/contact_scope.xml similarity index 94% rename from scope/contact_scope.xml rename to devices/baseEstimatorV1/scope/contact_scope.xml index b99b325..5ef2e14 100644 --- a/scope/contact_scope.xml +++ b/devices/baseEstimatorV1/scope/contact_scope.xml @@ -1,3 +1,10 @@ + - - - - - + diff --git a/scope/waist_imu_scope.xml b/devices/baseEstimatorV1/scope/waist_imu_scope.xml similarity index 92% rename from scope/waist_imu_scope.xml rename to devices/baseEstimatorV1/scope/waist_imu_scope.xml index 2ad9311..7362197 100644 --- a/scope/waist_imu_scope.xml +++ b/devices/baseEstimatorV1/scope/waist_imu_scope.xml @@ -1,3 +1,10 @@ + - - + diff --git a/src/Utils.cpp b/devices/baseEstimatorV1/src/Utils.cpp similarity index 100% rename from src/Utils.cpp rename to devices/baseEstimatorV1/src/Utils.cpp diff --git a/src/WalkingLogger.cpp b/devices/baseEstimatorV1/src/WalkingLogger.cpp similarity index 100% rename from src/WalkingLogger.cpp rename to devices/baseEstimatorV1/src/WalkingLogger.cpp diff --git a/src/icubFloatingBaseEstimatorV1.cpp b/devices/baseEstimatorV1/src/baseEstimatorV1.cpp similarity index 87% rename from src/icubFloatingBaseEstimatorV1.cpp rename to devices/baseEstimatorV1/src/baseEstimatorV1.cpp index 45c0d33..12ca431 100644 --- a/src/icubFloatingBaseEstimatorV1.cpp +++ b/devices/baseEstimatorV1/src/baseEstimatorV1.cpp @@ -1,21 +1,15 @@ /* -################################################################################ -# # -# Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT) # -# All Rights Reserved. # -# # -################################################################################ + * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ -# @authors: Prashanth Ramadoss -# Giulio Romualdi -# Silvio Traversaro -# Daniele Pucci -*/ +#include -#include - -bool yarp::dev::icubFloatingBaseEstimatorV1::getJointNamesList(const yarp::os::Searchable& config, std::vector< std::__cxx11::string >& joint_list) +bool yarp::dev::baseEstimatorV1::getJointNamesList(const yarp::os::Searchable& config, std::vector< std::string >& joint_list) { yarp::os::Property property; property.fromString(config.toString().c_str()); @@ -35,7 +29,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::getJointNamesList(const yarp::os::S return true; } -void yarp::dev::icubFloatingBaseEstimatorV1::resizeBuffers() +void yarp::dev::baseEstimatorV1::resizeBuffers() { m_joint_positions.resize(m_model); m_joint_velocities.resize(m_joint_positions.size()); @@ -55,16 +49,16 @@ void yarp::dev::icubFloatingBaseEstimatorV1::resizeBuffers() // wbd contact wrenches, ft sensors and imu measurement buffers are resized in the respective attach methods. } -yarp::dev::icubFloatingBaseEstimatorV1::icubFloatingBaseEstimatorV1(double period, yarp::os::ShouldUseSystemClock useSystemClock): PeriodicThread(period, useSystemClock) +yarp::dev::baseEstimatorV1::baseEstimatorV1(double period, yarp::os::ShouldUseSystemClock useSystemClock): PeriodicThread(period, useSystemClock) { } -yarp::dev::icubFloatingBaseEstimatorV1::icubFloatingBaseEstimatorV1(): PeriodicThread(0.01, yarp::os::ShouldUseSystemClock::No) +yarp::dev::baseEstimatorV1::baseEstimatorV1(): PeriodicThread(0.01, yarp::os::ShouldUseSystemClock::No) { } -bool yarp::dev::icubFloatingBaseEstimatorV1::open(yarp::os::Searchable& config) +bool yarp::dev::baseEstimatorV1::open(yarp::os::Searchable& config) { yarp::os::LockGuard guard(m_device_mutex); if (!configureWholeBodyDynamics(config)) @@ -99,7 +93,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::open(yarp::os::Searchable& config) return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::loadLeggedOdometry() +bool yarp::dev::baseEstimatorV1::loadLeggedOdometry() { if (!m_model.setDefaultBaseLink(m_model.getFrameIndex(m_base_link_name))) { @@ -125,7 +119,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::loadLeggedOdometry() return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::loadBipedFootContactClassifier() +bool yarp::dev::baseEstimatorV1::loadBipedFootContactClassifier() { m_biped_foot_contact_classifier = std::make_unique(m_left_foot_contact_schmitt_params, m_right_foot_contact_schmitt_params); m_biped_foot_contact_classifier->setContactSwitchingPattern(iDynTree::ALTERNATE_CONTACT); @@ -133,7 +127,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::loadBipedFootContactClassifier() return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::loadIMUAttitudeMahonyEstimator() +bool yarp::dev::baseEstimatorV1::loadIMUAttitudeMahonyEstimator() { m_imu_attitude_observer = std::make_unique(); m_imu_attitude_observer->setGainkp(m_imu_attitude_observer_params.kp); @@ -143,7 +137,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::loadIMUAttitudeMahonyEstimator() return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::loadIMUAttitudeQEKF() +bool yarp::dev::baseEstimatorV1::loadIMUAttitudeQEKF() { m_imu_attitude_qekf = std::make_unique(); m_imu_attitude_qekf->setParameters(m_imu_attitude_qekf_params); @@ -151,7 +145,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::loadIMUAttitudeQEKF() } -bool yarp::dev::icubFloatingBaseEstimatorV1::initializeLeggedOdometry() +bool yarp::dev::baseEstimatorV1::initializeLeggedOdometry() { bool ok = m_legged_odometry->updateKinematics(m_joint_positions); ok = ok && m_legged_odometry->init(m_initial_fixed_frame, m_initial_reference_frame_for_world, m_initial_reference_frame_H_world); @@ -161,7 +155,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::initializeLeggedOdometry() return ok; } -bool yarp::dev::icubFloatingBaseEstimatorV1::initializeBipedFootContactClassifier() +bool yarp::dev::baseEstimatorV1::initializeBipedFootContactClassifier() { if (m_initial_primary_foot == "left") { @@ -178,7 +172,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::initializeBipedFootContactClassifie return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::initializeIMUAttitudeEstimator() +bool yarp::dev::baseEstimatorV1::initializeIMUAttitudeEstimator() { iDynTree::VectorDynSize state; state.resize((int)m_imu_attitude_observer->getInternalStateSize()); @@ -194,7 +188,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::initializeIMUAttitudeEstimator() return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::initializeIMUAttitudeQEKF() +bool yarp::dev::baseEstimatorV1::initializeIMUAttitudeQEKF() { if (!m_imu_attitude_qekf->initializeFilter()) { @@ -218,7 +212,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::initializeIMUAttitudeQEKF() } -void yarp::dev::icubFloatingBaseEstimatorV1::getFeetCartesianWrenches() +void yarp::dev::baseEstimatorV1::getFeetCartesianWrenches() { // get these wrenches from whole body dynamics to avoid errors due to calibration offsets m_left_foot_contact_normal_force = m_left_foot_cartesian_wrench(2); @@ -226,7 +220,7 @@ void yarp::dev::icubFloatingBaseEstimatorV1::getFeetCartesianWrenches() } -bool yarp::dev::icubFloatingBaseEstimatorV1::updateLeggedOdometry() +bool yarp::dev::baseEstimatorV1::updateLeggedOdometry() { m_no_foot_in_contact = false; m_biped_foot_contact_classifier->updateFootContactState(yarp::os::Time::now(), m_left_foot_contact_normal_force, m_right_foot_contact_normal_force); @@ -285,7 +279,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::updateLeggedOdometry() return m_legged_odometry_update_went_well; } -bool yarp::dev::icubFloatingBaseEstimatorV1::updateIMUAttitudeEstimator() +bool yarp::dev::baseEstimatorV1::updateIMUAttitudeEstimator() { for (size_t imu = 0; imu < (size_t)m_whole_body_imu_interface.size(); imu++) { @@ -303,7 +297,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::updateIMUAttitudeEstimator() return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::updateIMUAttitudeQEKF() +bool yarp::dev::baseEstimatorV1::updateIMUAttitudeQEKF() { m_imu_attitude_qekf->propagateStates(); for (size_t imu = 0; imu < (size_t)m_whole_body_imu_interface.size(); imu++) @@ -319,7 +313,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::updateIMUAttitudeQEKF() return true; } -iDynTree::Transform yarp::dev::icubFloatingBaseEstimatorV1::getHeadIMU_H_NeckBaseAtZero() +iDynTree::Transform yarp::dev::baseEstimatorV1::getHeadIMU_H_NeckBaseAtZero() { iDynTree::KinDynComputations temp_kin_comp; temp_kin_comp.loadRobotModel(m_model); @@ -335,7 +329,7 @@ iDynTree::Transform yarp::dev::icubFloatingBaseEstimatorV1::getHeadIMU_H_NeckBas return temp_kin_comp.getRelativeTransform(m_head_imu_name, "head"); } -iDynTree::Transform yarp::dev::icubFloatingBaseEstimatorV1::getHeadIMUCorrectionWithNeckKinematics() +iDynTree::Transform yarp::dev::baseEstimatorV1::getHeadIMUCorrectionWithNeckKinematics() { // this funciton returns imu_H_imuAssumingNeckBaseToZero if (!m_imu_aligned) @@ -348,7 +342,7 @@ iDynTree::Transform yarp::dev::icubFloatingBaseEstimatorV1::getHeadIMUCorrection } -bool yarp::dev::icubFloatingBaseEstimatorV1::alignIMUFrames() +bool yarp::dev::baseEstimatorV1::alignIMUFrames() { iDynTree::Rotation b_R_head_imu = m_kin_dyn_comp.getRelativeTransform(m_base_link_name, m_head_imu_name).getRotation(); iDynTree::Rotation wIMU_R_IMU_0; @@ -369,7 +363,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::alignIMUFrames() return true; } -iDynTree::Rotation yarp::dev::icubFloatingBaseEstimatorV1::getBaseOrientationFromIMU() +iDynTree::Rotation yarp::dev::baseEstimatorV1::getBaseOrientationFromIMU() { iDynTree::Rotation wIMU_R_IMU; if (m_attitude_estimator_type == "mahony") @@ -391,7 +385,7 @@ iDynTree::Rotation yarp::dev::icubFloatingBaseEstimatorV1::getBaseOrientationFro return (m_head_imu_calibration_matrix * wIMU_R_b); } -bool yarp::dev::icubFloatingBaseEstimatorV1::updateBasePoseWithIMUEstimates() +bool yarp::dev::baseEstimatorV1::updateBasePoseWithIMUEstimates() { double updated_roll; double updated_pitch; @@ -434,7 +428,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::updateBasePoseWithIMUEstimates() return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::updateBaseVelocity() +bool yarp::dev::baseEstimatorV1::updateBaseVelocity() { using iDynTree::toiDynTree; using iDynTree::toEigen; @@ -479,7 +473,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::updateBaseVelocity() return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::updateBaseVelocityWithIMU() +bool yarp::dev::baseEstimatorV1::updateBaseVelocityWithIMU() { using iDynTree::toEigen; iDynTree::Vector3 y_acc; @@ -530,7 +524,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::updateBaseVelocityWithIMU() } -void yarp::dev::icubFloatingBaseEstimatorV1::publishFloatingBaseState() +void yarp::dev::baseEstimatorV1::publishFloatingBaseState() { yarp::os::Bottle &state_bottle = m_floating_base_state_port.prepare(); state_bottle.clear(); @@ -547,7 +541,7 @@ void yarp::dev::icubFloatingBaseEstimatorV1::publishFloatingBaseState() m_floating_base_state_port.write(); } -void yarp::dev::icubFloatingBaseEstimatorV1::publishFloatingBasePoseVelocity() +void yarp::dev::baseEstimatorV1::publishFloatingBasePoseVelocity() { yarp::os::Bottle &state_bottle = m_floating_base_pose_port.prepare(); state_bottle.clear(); @@ -564,7 +558,7 @@ void yarp::dev::icubFloatingBaseEstimatorV1::publishFloatingBasePoseVelocity() m_floating_base_pose_port.write(); } -void yarp::dev::icubFloatingBaseEstimatorV1::publishContactState() +void yarp::dev::baseEstimatorV1::publishContactState() { yarp::os::Bottle &state_bottle = m_contact_state_port.prepare(); state_bottle.clear(); @@ -577,7 +571,7 @@ void yarp::dev::icubFloatingBaseEstimatorV1::publishContactState() } -void yarp::dev::icubFloatingBaseEstimatorV1::publishIMUAttitudeEstimatorStates() +void yarp::dev::baseEstimatorV1::publishIMUAttitudeEstimatorStates() { iDynTree::VectorDynSize attitude_observer_state; if (m_attitude_estimator_type == "mahony") @@ -615,7 +609,7 @@ void yarp::dev::icubFloatingBaseEstimatorV1::publishIMUAttitudeEstimatorStates() m_imu_attitude_observer_estimated_state_port.write(); } -void yarp::dev::icubFloatingBaseEstimatorV1::publishIMUAttitudeQEKFEstimates() +void yarp::dev::baseEstimatorV1::publishIMUAttitudeQEKFEstimates() { iDynTree::RPY rpy; m_imu_attitude_qekf->getOrientationEstimateAsRPY(rpy); @@ -630,12 +624,12 @@ void yarp::dev::icubFloatingBaseEstimatorV1::publishIMUAttitudeQEKFEstimates() } -void yarp::dev::icubFloatingBaseEstimatorV1::publishTransform() +void yarp::dev::baseEstimatorV1::publishTransform() { m_transform_interface->setTransform(m_base_link_name, "world", m_world_H_base); } -bool yarp::dev::icubFloatingBaseEstimatorV1::initializeLogger() +bool yarp::dev::baseEstimatorV1::initializeLogger() { m_logger->startRecord({"record","fbe_x", "fbe_y", "fbe_z", "fbe_roll", "fbe_pitch", "fbe_yaw", @@ -649,7 +643,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::initializeLogger() return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::updateLogger() +bool yarp::dev::baseEstimatorV1::updateLogger() { yarp::sig::Vector feet_contact_state; yarp::sig::Vector feet_contact_normal_forces; @@ -666,7 +660,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::updateLogger() } -void yarp::dev::icubFloatingBaseEstimatorV1::publish() +void yarp::dev::baseEstimatorV1::publish() { publishFloatingBasePoseVelocity(); publishContactState(); @@ -683,7 +677,7 @@ void yarp::dev::icubFloatingBaseEstimatorV1::publish() publishTransform(); } -void yarp::dev::icubFloatingBaseEstimatorV1::run() +void yarp::dev::baseEstimatorV1::run() { yarp::os::LockGuard guard(m_device_mutex); @@ -751,7 +745,7 @@ void yarp::dev::icubFloatingBaseEstimatorV1::run() } -void yarp::dev::icubFloatingBaseEstimatorV1::closeDevice() +void yarp::dev::baseEstimatorV1::closeDevice() { if (!m_imu_attitude_observer_estimated_state_port.isClosed()) { @@ -821,7 +815,7 @@ void yarp::dev::icubFloatingBaseEstimatorV1::closeDevice() } } -bool yarp::dev::icubFloatingBaseEstimatorV1::close() +bool yarp::dev::baseEstimatorV1::close() { yarp::os::LockGuard guard(m_device_mutex); closeDevice(); @@ -829,35 +823,35 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::close() return true; } -yarp::dev::icubFloatingBaseEstimatorV1::~icubFloatingBaseEstimatorV1() +yarp::dev::baseEstimatorV1::~baseEstimatorV1() { } /// RPC methods -bool yarp::dev::icubFloatingBaseEstimatorV1::setMahonyKp(const double kp) +bool yarp::dev::baseEstimatorV1::setMahonyKp(const double kp) { yarp::os::LockGuard guard(m_device_mutex); m_imu_attitude_observer->setGainkp(kp); return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::setMahonyKi(const double ki) +bool yarp::dev::baseEstimatorV1::setMahonyKi(const double ki) { yarp::os::LockGuard guard(m_device_mutex); m_imu_attitude_observer->setGainki(ki); return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::setMahonyTimeStep(const double timestep) +bool yarp::dev::baseEstimatorV1::setMahonyTimeStep(const double timestep) { yarp::os::LockGuard guard(m_device_mutex); m_imu_attitude_observer->setTimeStepInSeconds(timestep); return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::setContactSchmittThreshold(const double l_fz_break, const double l_fz_make, +bool yarp::dev::baseEstimatorV1::setContactSchmittThreshold(const double l_fz_break, const double l_fz_make, const double r_fz_break, const double r_fz_make) { yarp::os::LockGuard guard(m_device_mutex); @@ -872,7 +866,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::setContactSchmittThreshold(const do } -std::string yarp::dev::icubFloatingBaseEstimatorV1::getEstimationJointsList() +std::string yarp::dev::baseEstimatorV1::getEstimationJointsList() { yarp::os::LockGuard guard(m_device_mutex); std::stringstream ss; @@ -892,7 +886,7 @@ std::string yarp::dev::icubFloatingBaseEstimatorV1::getEstimationJointsList() return ss.str(); } -bool yarp::dev::icubFloatingBaseEstimatorV1::setPrimaryFoot(const std::string& primary_foot) +bool yarp::dev::baseEstimatorV1::setPrimaryFoot(const std::string& primary_foot) { yarp::os::LockGuard guard(m_device_mutex); if (primary_foot == "right") @@ -919,13 +913,13 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::setPrimaryFoot(const std::string& p return true; } -std::string yarp::dev::icubFloatingBaseEstimatorV1::getRefFrameForWorld() +std::string yarp::dev::baseEstimatorV1::getRefFrameForWorld() { yarp::os::LockGuard guard(m_device_mutex); return m_initial_reference_frame_for_world; } -Pose6D yarp::dev::icubFloatingBaseEstimatorV1::getRefPose6DForWorld() +Pose6D yarp::dev::baseEstimatorV1::getRefPose6DForWorld() { yarp::os::LockGuard guard(m_device_mutex); Pose6D ref_pose_world; @@ -938,7 +932,7 @@ Pose6D yarp::dev::icubFloatingBaseEstimatorV1::getRefPose6DForWorld() return ref_pose_world; } -bool yarp::dev::icubFloatingBaseEstimatorV1::resetLeggedOdometry() +bool yarp::dev::baseEstimatorV1::resetLeggedOdometry() { yarp::os::LockGuard guard(m_device_mutex); m_legged_odometry->updateKinematics(m_joint_positions); @@ -946,14 +940,14 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::resetLeggedOdometry() return ok; } -bool yarp::dev::icubFloatingBaseEstimatorV1::startFloatingBaseFilter() +bool yarp::dev::baseEstimatorV1::startFloatingBaseFilter() { yarp::os::LockGuard guard(m_device_mutex); m_state = FilterFSM::RUNNING; return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::resetLeggedOdometryWithRefFrame(const std::string& ref_frame, +bool yarp::dev::baseEstimatorV1::resetLeggedOdometryWithRefFrame(const std::string& ref_frame, const double x, const double y, const double z, const double roll, const double pitch, const double yaw) { @@ -982,15 +976,16 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::resetLeggedOdometryWithRefFrame(con return ok; } -bool yarp::dev::icubFloatingBaseEstimatorV1::setJointVelocityLPFCutoffFrequency(const double freq) +bool yarp::dev::baseEstimatorV1::setJointVelocityLPFCutoffFrequency(const double freq) { m_joint_vel_filter_cutoff_freq = freq; m_joint_velocities_filter->setCutFrequency(freq); return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::useJointVelocityLPF(const bool flag) +bool yarp::dev::baseEstimatorV1::useJointVelocityLPF(const bool flag) { m_use_lpf = flag; return true; } + diff --git a/src/configureEstimator.cpp b/devices/baseEstimatorV1/src/configureEstimator.cpp similarity index 93% rename from src/configureEstimator.cpp rename to devices/baseEstimatorV1/src/configureEstimator.cpp index ab2d73d..696d6c8 100644 --- a/src/configureEstimator.cpp +++ b/devices/baseEstimatorV1/src/configureEstimator.cpp @@ -1,20 +1,14 @@ /* -################################################################################ -# # -# Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia (IIT) # -# All Rights Reserved. # -# # -################################################################################ - -# @authors: Prashanth Ramadoss -# Giulio Romualdi -# Silvio Traversaro -# Daniele Pucci -*/ - -#include - -bool yarp::dev::icubFloatingBaseEstimatorV1::loadEstimatorParametersFromConfig(const yarp::os::Searchable& config) + * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#include + +bool yarp::dev::baseEstimatorV1::loadEstimatorParametersFromConfig(const yarp::os::Searchable& config) { if (config.check("model_file") && config.find("model_file").isString()) { @@ -179,7 +173,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::loadEstimatorParametersFromConfig(c return ok; } -bool yarp::dev::icubFloatingBaseEstimatorV1::loadLeggedOdometryParametersFromConfig(const yarp::os::Searchable& config) +bool yarp::dev::baseEstimatorV1::loadLeggedOdometryParametersFromConfig(const yarp::os::Searchable& config) { if (config.check("initial_fixed_frame") && config.find("initial_fixed_frame").isString()) { @@ -235,7 +229,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::loadLeggedOdometryParametersFromCon return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::loadBipedFootContactClassifierParametersFromConfig(const yarp::os::Searchable& config) +bool yarp::dev::baseEstimatorV1::loadBipedFootContactClassifierParametersFromConfig(const yarp::os::Searchable& config) { if (config.check("initial_primary_foot") && config.find("initial_primary_foot").isString()) { @@ -318,7 +312,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::loadBipedFootContactClassifierParam return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::loadIMUAttitudeMahonyEstimatorParametersFromConfig(const yarp::os::Searchable& config) +bool yarp::dev::baseEstimatorV1::loadIMUAttitudeMahonyEstimatorParametersFromConfig(const yarp::os::Searchable& config) { if (config.check("mahony_kp") && config.find("mahony_kp").isDouble()) { @@ -359,7 +353,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::loadIMUAttitudeMahonyEstimatorParam return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::loadIMUAttitudeQEKFParamtersFromConfig(const yarp::os::Searchable& config) +bool yarp::dev::baseEstimatorV1::loadIMUAttitudeQEKFParamtersFromConfig(const yarp::os::Searchable& config) { if (config.check("qekf_discretization_time_step_in_seconds") && config.find("qekf_discretization_time_step_in_seconds").isDouble()) { @@ -454,7 +448,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::loadIMUAttitudeQEKFParamtersFromCon return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::openComms() +bool yarp::dev::baseEstimatorV1::openComms() { bool ok{false}; ok = m_imu_attitude_observer_estimated_state_port.open(m_port_prefix + "/mahony_state/state:o"); @@ -502,3 +496,4 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::openComms() return true; } + diff --git a/src/fbeRobotInterface.cpp b/devices/baseEstimatorV1/src/fbeRobotInterface.cpp similarity index 87% rename from src/fbeRobotInterface.cpp rename to devices/baseEstimatorV1/src/fbeRobotInterface.cpp index 0d56af1..19a03f2 100644 --- a/src/fbeRobotInterface.cpp +++ b/devices/baseEstimatorV1/src/fbeRobotInterface.cpp @@ -1,21 +1,15 @@ /* -################################################################################ -# # -# Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT) # -# All Rights Reserved. # -# # -################################################################################ + * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ -# @authors: Prashanth Ramadoss -# Giulio Romualdi -# Silvio Traversaro -# Daniele Pucci -*/ +#include -#include - -bool yarp::dev::icubFloatingBaseEstimatorV1::sensorReadDryRun(bool verbose, bool (yarp::dev::icubFloatingBaseEstimatorV1::*func_t)(bool)) +bool yarp::dev::baseEstimatorV1::sensorReadDryRun(bool verbose, bool (yarp::dev::baseEstimatorV1::*func_t)(bool)) { double tic{yarp::os::Time::now()}; double time_elapsed_trying_to_read_sensors{0.0}; @@ -34,12 +28,12 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::sensorReadDryRun(bool verbose, boo return read_success; } -bool yarp::dev::icubFloatingBaseEstimatorV1::attachAll(const yarp::dev::PolyDriverList& p) +bool yarp::dev::baseEstimatorV1::attachAll(const yarp::dev::PolyDriverList& p) { yarp::os::LockGuard guard(m_device_mutex); if (!attachAllControlBoards(p)) { - yError() << "icubFloatingBaseEstimatorV1: " << "Could not attach the control boards"; + yError() << "baseEstimatorV1: " << "Could not attach the control boards"; return false; } @@ -76,7 +70,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::attachAll(const yarp::dev::PolyDriv return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::attachAllControlBoards(const yarp::dev::PolyDriverList& p) +bool yarp::dev::baseEstimatorV1::attachAllControlBoards(const yarp::dev::PolyDriverList& p) { bool ok{false}; for (size_t dev_idx = 0; dev_idx < (size_t)p.size(); dev_idx++) @@ -96,7 +90,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::attachAllControlBoards(const yarp:: return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::loadEstimator() +bool yarp::dev::baseEstimatorV1::loadEstimator() { yarp::os::ResourceFinder &rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); std::string model_file_path = rf.findFileByName(m_model_file_name); @@ -145,12 +139,12 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::loadEstimator() return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::attachMultipleAnalogSensors(const yarp::dev::PolyDriverList& p) +bool yarp::dev::baseEstimatorV1::attachMultipleAnalogSensors(const yarp::dev::PolyDriverList& p) { return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::attachAllForceTorqueSensors(const yarp::dev::PolyDriverList& p) +bool yarp::dev::baseEstimatorV1::attachAllForceTorqueSensors(const yarp::dev::PolyDriverList& p) { std::vector ft_sensor_list; std::vector ft_sensor_name; @@ -201,7 +195,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::attachAllForceTorqueSensors(const y // dry run of readFTSensors() to check if all FTs work and to initialize buffers m_ft_measurements_from_yarp_server.resize(m_nr_of_channels_in_YARP_FT_sensor); bool verbose{false}; - if (!sensorReadDryRun(verbose, &yarp::dev::icubFloatingBaseEstimatorV1::readFTSensors)) + if (!sensorReadDryRun(verbose, &yarp::dev::baseEstimatorV1::readFTSensors)) { return false; } @@ -209,7 +203,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::attachAllForceTorqueSensors(const y return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::attachAllInertialMeasurementUnits(const yarp::dev::PolyDriverList& p) +bool yarp::dev::baseEstimatorV1::attachAllInertialMeasurementUnits(const yarp::dev::PolyDriverList& p) { std::vector imu_sensor_list; std::vector imu_sensor_name; @@ -270,14 +264,14 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::attachAllInertialMeasurementUnits(c m_imu_meaaurements_from_yarp_server[imu].resize(m_nr_of_channels_in_YARP_IMU_sensor); } bool verbose{false}; - if (!sensorReadDryRun(verbose, &yarp::dev::icubFloatingBaseEstimatorV1::readIMUSensors)) + if (!sensorReadDryRun(verbose, &yarp::dev::baseEstimatorV1::readIMUSensors)) { return false; } return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::readIMUSensors(bool verbose) +bool yarp::dev::baseEstimatorV1::readIMUSensors(bool verbose) { bool all_IMUs_read_correctly{true}; for (size_t imu = 0; imu < m_nr_of_IMUs_detected; imu++) @@ -312,7 +306,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::readIMUSensors(bool verbose) return all_IMUs_read_correctly; } -bool yarp::dev::icubFloatingBaseEstimatorV1::readFTSensors(bool verbose) +bool yarp::dev::baseEstimatorV1::readFTSensors(bool verbose) { bool ft_sensors_read_correctly{true}; for (size_t ft = 0; ft < m_nr_of_forcetorque_sensors_detected; ft++) @@ -352,7 +346,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::readFTSensors(bool verbose) return ft_sensors_read_correctly; } -bool yarp::dev::icubFloatingBaseEstimatorV1::readEncoders(bool verbose) +bool yarp::dev::baseEstimatorV1::readEncoders(bool verbose) { int ax; m_remapped_control_board_interfaces.encs->getAxes(&ax); bool encoders_read_correctly = m_remapped_control_board_interfaces.encs->getEncoders(m_joint_positions.data()); @@ -388,7 +382,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::readEncoders(bool verbose) } -bool yarp::dev::icubFloatingBaseEstimatorV1::configureWholeBodyDynamics(const yarp::os::Searchable& config) +bool yarp::dev::baseEstimatorV1::configureWholeBodyDynamics(const yarp::os::Searchable& config) { if (config.check("left_foot_cartesian_wrench_port") && config.find("left_foot_cartesian_wrench_port").isString()) { @@ -432,7 +426,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::configureWholeBodyDynamics(const ya m_left_foot_cartesian_wrench.resize(6); bool verbose{false}; - if (!sensorReadDryRun(verbose, &yarp::dev::icubFloatingBaseEstimatorV1::readWholeBodyDynamicsContactWrenches)) + if (!sensorReadDryRun(verbose, &yarp::dev::baseEstimatorV1::readWholeBodyDynamicsContactWrenches)) { return false; } @@ -441,7 +435,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::configureWholeBodyDynamics(const ya return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::calibFTSensorsStanding() +bool yarp::dev::baseEstimatorV1::calibFTSensorsStanding() { yarp::os::RpcClient wbd_rpc_port; wbd_rpc_port.open("/wholeBodyDynamics/local/rpc"); @@ -497,7 +491,7 @@ bool readCartesianWrenchesFromPorts(yarp::os::BufferedPort& p return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::readWholeBodyDynamicsContactWrenches(bool verbose) +bool yarp::dev::baseEstimatorV1::readWholeBodyDynamicsContactWrenches(bool verbose) { bool ok = readCartesianWrenchesFromPorts(m_left_foot_cartesian_wrench_wbd_port, m_left_foot_cartesian_wrench, m_verbose); ok = readCartesianWrenchesFromPorts(m_right_foot_cartesian_wrench_wbd_port, m_right_foot_cartesian_wrench, m_verbose) && ok; @@ -505,7 +499,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::readWholeBodyDynamicsContactWrenche return ok; } -bool yarp::dev::icubFloatingBaseEstimatorV1::readSensors(bool verbose) +bool yarp::dev::baseEstimatorV1::readSensors(bool verbose) { bool all_sensors_read_correctly{true}; all_sensors_read_correctly = readEncoders(m_verbose) && all_sensors_read_correctly; @@ -516,7 +510,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::readSensors(bool verbose) return all_sensors_read_correctly; } -bool yarp::dev::icubFloatingBaseEstimatorV1::loadTransformBroadcaster() +bool yarp::dev::baseEstimatorV1::loadTransformBroadcaster() { yarp::os::Property tf_broadcaster_settings; tf_broadcaster_settings.put("device", "transformClient"); @@ -547,7 +541,7 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::loadTransformBroadcaster() return true; } -bool yarp::dev::icubFloatingBaseEstimatorV1::detachAll() +bool yarp::dev::baseEstimatorV1::detachAll() { yarp::os::LockGuard guard(m_device_mutex); m_device_initialized_correctly = false; @@ -557,3 +551,4 @@ bool yarp::dev::icubFloatingBaseEstimatorV1::detachAll() } return true; } + diff --git a/thrifts/floatingBaseEstimationRPC.thrift b/devices/baseEstimatorV1/thrifts/floatingBaseEstimationRPC.thrift similarity index 57% rename from thrifts/floatingBaseEstimationRPC.thrift rename to devices/baseEstimatorV1/thrifts/floatingBaseEstimationRPC.thrift index 35d4de2..f86be3b 100644 --- a/thrifts/floatingBaseEstimationRPC.thrift +++ b/devices/baseEstimatorV1/thrifts/floatingBaseEstimationRPC.thrift @@ -1,17 +1,11 @@ /* -################################################################################ -# # -# Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia (IIT) # -# All Rights Reserved. # -# # -################################################################################ - -# @authors: Prashanth Ramadoss -# Giulio Romualdi -# Silvio Traversaro -# Daniele Pucci -*/ - + * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + struct Pose6D { 1: double x; 2: double y; 3: double z; @@ -38,3 +32,4 @@ service floatingBaseEstimationRPC bool setJointVelocityLPFCutoffFrequency(1: double freq); bool startFloatingBaseFilter(); } + diff --git a/icubFloatingBaseEstimatorV1.ini b/icubFloatingBaseEstimatorV1.ini deleted file mode 100644 index 1097f6c..0000000 --- a/icubFloatingBaseEstimatorV1.ini +++ /dev/null @@ -1,4 +0,0 @@ -[plugin icubFloatingBaseEstimatorV1] -type device -name icubFloatingBaseEstimatorV1 -library icubFloatingBaseEstimatorV1 From 76d8f27c837da6c91eb4c3040a36ad79e6d48b2f Mon Sep 17 00:00:00 2001 From: Prashanth Date: Thu, 13 Jun 2019 13:35:43 +0200 Subject: [PATCH 3/5] [fbeV1] update docs --- devices/baseEstimatorV1/README.md | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/devices/baseEstimatorV1/README.md b/devices/baseEstimatorV1/README.md index 3e9ef44..abb8247 100644 --- a/devices/baseEstimatorV1/README.md +++ b/devices/baseEstimatorV1/README.md @@ -1,4 +1,6 @@ # Overview +This estimator uses the kinematics, IMU mounted on the head and the contact forces information to estimate the floating base of the robot. The pose of floating base with respect to the inertial frame is computed through legged odometry and is fused along with the attitude estimates from the head IMU. The base velocity is obtained from the contact Jacobian computed using the kinematics information by enforcing the unilateral constraint of the foot. + - [:computer: How to run the simulation](#computer-how-to-run-the-simulation) - [:running: How to test on iCub](#running-how-to-test-on-icub) @@ -35,14 +37,22 @@ yarp rpc /base-estimator/rpc ``` the following commands are allowed: - * `startFloatingBaseFilter`: fill this; - * `setContactSchmittThreshold lbreak lmake rbreak rmake`: fill this; - * `setPrimaryFoot foot`: fill this; - * `useJointVelocityLPF flag`: fill this; - * `setJointVelocityLPFCutoffFrequency freq`: fill this; - * `resetLeggedOdometry`: fill this; - * `resetLeggedOdometryWithRefFrame frame x y z roll pitch yaw`: fill this; - * `getRefFrameForWorld`: fill this; + * `startFloatingBaseFilter`: starts the estimator, this needs to be run after the FT sensors are reset; + + other optional commands include, + + * `setContactSchmittThreshold lbreak lmake rbreak rmake`: used to set contact force thresholds for feet contact detection; + * `setPrimaryFoot foot`: set the foot to the one that does not break the contact initially during walking, `foot` can be `left` or `right`; + * `useJointVelocityLPF flag`: use a low pass filter on the joint velocities, `flag` can be `true` or `false`; + * `setJointVelocityLPFCutoffFrequency freq`: set the cut-off frequency for the low pass filter on the joint velocities; + * `resetLeggedOdometry`: reset the floating base pose and reset the legged odometry to the inital state; + * `resetLeggedOdometryWithRefFrame frame x y z roll pitch yaw`: reset the legged odometry by mentioning an intial reference to the world frame with respect to the initial fixed frame; + * `getRefFrameForWorld`: get the initial fixed frame with respect to which the world frame was set; + +## Configuration + +The configuration file for the estimator can be found `app/robots/${YARP_ROBOT_NAME}/fbe-analogsens.xml`. +The attitude estimation for the head IMU can be chosen to be either a QuaternionEKF or a non-linear complementary filter. The gains should be tuned accordingly. ## How to dump data Before run `yarprobotinterface` check if [`dump_data`](app/robots/iCubGazeboV2_5/fbe-analogsens.xml#L14) is set to `true` From 82d978f1892d49beb5bdc29df0f343a153b2678a Mon Sep 17 00:00:00 2001 From: Prashanth Date: Mon, 1 Jul 2019 13:30:45 +0200 Subject: [PATCH 4/5] [fbeV1] address reviews --- CMakeLists.txt | 14 +++- README.md | 1 - cmake/AddInstallRPATHSupport.cmake | 24 +++--- cmake/AddUninstallTarget.cmake | 82 +++++++++++++------ devices/baseEstimatorV1/CMakeLists.txt | 17 +--- .../iCubGazeboV2_5/launch-fbe-analogsens.xml | 1 - .../wholebodydynamics-external.xml | 81 ------------------ .../iCubGenova04/launch-fbe-analogsens.xml | 1 - .../wholebodydynamics-external.xml | 81 ------------------ 9 files changed, 81 insertions(+), 221 deletions(-) delete mode 100644 devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/wholebodydynamics-external.xml delete mode 100644 devices/baseEstimatorV1/app/robots/iCubGenova04/wholebodydynamics-external.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 3e71aad..6ca21f9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,12 +13,18 @@ option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) +option(ENABLE_RPATH "Enable RPATH for this library" ON) +mark_as_advanced(ENABLE_RPATH) +include(AddInstallRPATHSupport) +add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}" + LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}" + INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}" + DEPENDS ENABLE_RPATH + USE_LINK_PATH) + set(YARP_REQUIRED_VERSION 3.0.1) -find_package(YARP REQUIRED) -if(${YARP_VERSION} VERSION_LESS ${YARP_REQUIRED_VERSION}) - message(FATAL_ERROR "YARP version ${YARP_VERSION} not sufficient, at least version ${YARP_REQUIRED_VERSION} is required.") -endif() +find_package(YARP ${YARP_REQUIRED_VERSION} REQUIRED) add_subdirectory(devices) diff --git a/README.md b/README.md index f6f05a7..22f177a 100644 --- a/README.md +++ b/README.md @@ -39,7 +39,6 @@ make Notice: `sudo` is not necessary if you specify the `CMAKE_INSTALL_PREFIX`. In this case it is necessary to add in the `.bashrc` or `.bash_profile` the following lines: ``` sh export WBDEstimator_INSTALL_DIR=/path/where/you/installed -export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${WBDEstimator_INSTALL_DIR}/lib/yarp export YARP_DATA_DIRS=${YARP_DATA_DIRS}:${WBDEstimator_INSTALL_DIR}/share/yarp:${WBDEstimator_INSTALL_DIR}/lib/yarp ``` diff --git a/cmake/AddInstallRPATHSupport.cmake b/cmake/AddInstallRPATHSupport.cmake index a0d12da..cefdd02 100644 --- a/cmake/AddInstallRPATHSupport.cmake +++ b/cmake/AddInstallRPATHSupport.cmake @@ -29,7 +29,7 @@ # - the variable will be used only by applications spawned by the shell # and not by other processes. # -# RPATH is aimed to solve the issues introduced by the second +# RPATH aims in solving the issues introduced by the second # installation method. Using run-path dependent libraries you can # create a directory structure containing executables and dependent # libraries that users can relocate without breaking it. @@ -48,7 +48,9 @@ # enabled shared library, i.e. its install name will be resolved # only at run time. # - In all cases (building executables and/or shared libraries) -# dependent shared libraries with RPATH support will be properly +# dependent shared libraries with RPATH support will have their name +# resolved only at run time, by embedding the search path directly +# into the built binary. # # The command has the following parameters: # @@ -95,13 +97,13 @@ function(ADD_INSTALL_RPATH_SUPPORT) set(_options USE_LINK_PATH) set(_oneValueArgs INSTALL_NAME_DIR) set(_multiValueArgs BIN_DIRS - LIB_DIRS - DEPENDS) + LIB_DIRS + DEPENDS) cmake_parse_arguments(_ARS "${_options}" - "${_oneValueArgs}" - "${_multiValueArgs}" - "${ARGN}") + "${_oneValueArgs}" + "${_multiValueArgs}" + "${ARGN}") # if either RPATH or INSTALL_RPATH is disabled # and the INSTALL_NAME_DIR variable is set, then hardcode the install name @@ -134,10 +136,10 @@ function(ADD_INSTALL_RPATH_SUPPORT) # Find system implicit lib directories set(_system_lib_dirs ${CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES}) if(EXISTS "/etc/debian_version") # is this a debian system ? - if(CMAKE_LIBRARY_ARCHITECTURE) - list(APPEND _system_lib_dirs "/lib/${CMAKE_LIBRARY_ARCHITECTURE}" - "/usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}") - endif() + if(CMAKE_LIBRARY_ARCHITECTURE) + list(APPEND _system_lib_dirs "/lib/${CMAKE_LIBRARY_ARCHITECTURE}" + "/usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}") + endif() endif() # This is relative RPATH for libraries built in the same project foreach(lib_dir ${_ARS_LIB_DIRS}) diff --git a/cmake/AddUninstallTarget.cmake b/cmake/AddUninstallTarget.cmake index cd45be5..f47198d 100644 --- a/cmake/AddUninstallTarget.cmake +++ b/cmake/AddUninstallTarget.cmake @@ -7,13 +7,24 @@ # include(AddUninstallTarget) # # -# will create a file cmake_uninstall.cmake in the build directory and add a -# custom target uninstall that will remove the files installed by your package -# (using install_manifest.txt) +# will create a file ``cmake_uninstall.cmake`` in the build directory and add a +# custom target ``uninstall`` (or ``UNINSTALL`` on Visual Studio and Xcode) that +# will remove the files installed by your package (using +# ``install_manifest.txt``). +# See also +# https://gitlab.kitware.com/cmake/community/wikis/FAQ#can-i-do-make-uninstall-with-cmake +# +# The :module:`AddUninstallTarget` module must be included in your main +# ``CMakeLists.txt``. If included in a subdirectory it does nothing. +# This allows you to use it safely in your main ``CMakeLists.txt`` and include +# your project using ``add_subdirectory`` (for example when using it with +# :cmake:module:`FetchContent`). +# +# If the ``uninstall`` target already exists, the module does nothing. #============================================================================= # Copyright 2008-2013 Kitware, Inc. -# Copyright 2013 iCub Facility, Istituto Italiano di Tecnologia +# Copyright 2013 Istituto Italiano di Tecnologia (IIT) # Authors: Daniele E. Domenichelli # # Distributed under the OSI-approved BSD License (the "License"); @@ -27,44 +38,65 @@ # License text for the above reference.) -if(DEFINED __ADD_UNINSTALL_TARGET_INCLUDED) +# AddUninstallTarget works only when included in the main CMakeLists.txt +if(NOT "${CMAKE_CURRENT_BINARY_DIR}" STREQUAL "${CMAKE_BINARY_DIR}") + return() +endif() + +# The name of the target is uppercase in MSVC and Xcode (for coherence with the +# other standard targets) +if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)") + set(_uninstall "UNINSTALL") +else() + set(_uninstall "uninstall") +endif() + +# If target is already defined don't do anything +if(TARGET ${_uninstall}) return() endif() -set(__ADD_UNINSTALL_TARGET_INCLUDED TRUE) -set(_filename ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake) +set(_filename cmake_uninstall.cmake) -file(WRITE ${_filename} - "if(NOT EXISTS \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\") - message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\") - return() +file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/${_filename}" +"if(NOT EXISTS \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\") + message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\") + return() endif() file(READ \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\" files) string(STRIP \"${files}\" files) string(REGEX REPLACE \"\\n\" \";\" files \"${files}\") list(REVERSE files) foreach(file ${files}) + if(IS_SYMLINK \"$ENV{DESTDIR}${file}\" OR EXISTS \"$ENV{DESTDIR}${file}\") message(STATUS \"Uninstalling: $ENV{DESTDIR}${file}\") - if(EXISTS \"$ENV{DESTDIR}${file}\") - execute_process( - COMMAND ${CMAKE_COMMAND} -E remove \"$ENV{DESTDIR}${file}\" - OUTPUT_VARIABLE rm_out - RESULT_VARIABLE rm_retval) - if(NOT \"${rm_retval}\" EQUAL 0) - message(FATAL_ERROR \"Problem when removing \\\"$ENV{DESTDIR}${file}\\\"\") - endif() - else() - message(STATUS \"File \\\"$ENV{DESTDIR}${file}\\\" does not exist.\") + execute_process( + COMMAND ${CMAKE_COMMAND} -E remove \"$ENV{DESTDIR}${file}\" + OUTPUT_VARIABLE rm_out + RESULT_VARIABLE rm_retval) + if(NOT \"${rm_retval}\" EQUAL 0) + message(FATAL_ERROR \"Problem when removing \\\"$ENV{DESTDIR}${file}\\\"\") endif() + else() + message(STATUS \"Not-found: $ENV{DESTDIR}${file}\") + endif() endforeach(file) ") -if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)") - set(_uninstall "UNINSTALL") +set(_desc "Uninstall the project...") +if(CMAKE_GENERATOR STREQUAL "Unix Makefiles") + set(_comment COMMAND $\(CMAKE_COMMAND\) -E cmake_echo_color --switch=$\(COLOR\) --cyan "${_desc}") else() - set(_uninstall "uninstall") + set(_comment COMMENT "${_desc}") endif() -add_custom_target(${_uninstall} COMMAND "${CMAKE_COMMAND}" -P "${_filename}") +add_custom_target(${_uninstall} + ${_comment} + COMMAND ${CMAKE_COMMAND} -P ${_filename} + USES_TERMINAL + BYPRODUCTS uninstall_byproduct + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}") +set_property(SOURCE uninstall_byproduct PROPERTY SYMBOLIC 1) + set_property(TARGET ${_uninstall} PROPERTY FOLDER "CMakePredefinedTargets") diff --git a/devices/baseEstimatorV1/CMakeLists.txt b/devices/baseEstimatorV1/CMakeLists.txt index 4524b61..0332fbe 100644 --- a/devices/baseEstimatorV1/CMakeLists.txt +++ b/devices/baseEstimatorV1/CMakeLists.txt @@ -4,22 +4,7 @@ set(iDynTree_REQUIRED_VERSION 0.11.0) -set(CMAKE_INCLUDE_CURRENT_DIR TRUE) - -option(ENABLE_RPATH "Enable RPATH for this library" ON) -mark_as_advanced(ENABLE_RPATH) -include(AddInstallRPATHSupport) -add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}" - LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}" - INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}" - DEPENDS ENABLE_RPATH - USE_LINK_PATH) - -find_package(Eigen3 REQUIRED) -find_package(iDynTree REQUIRED) -if(${iDynTree_VERSION} VERSION_LESS ${iDynTree_REQUIRED_VERSION}) - message(FATAL_ERROR "iDyntree version ${iDynTree_VERSION} not sufficient, at least version ${iDynTree_REQUIRED_VERSION} is required.") -endif() +find_package(iDynTree ${iDynTree_VERSION} REQUIRED) find_package(ICUB REQUIRED) diff --git a/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml index 3800914..9409289 100644 --- a/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml +++ b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml @@ -86,7 +86,6 @@ 0.2 - diff --git a/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/wholebodydynamics-external.xml b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/wholebodydynamics-external.xml deleted file mode 100644 index abcd0f2..0000000 --- a/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/wholebodydynamics-external.xml +++ /dev/null @@ -1,81 +0,0 @@ - - - - - (torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup,r_wrist_pitch,r_wrist_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) - model.urdf - (0,0,-9.81) - (l_hand,r_hand,root_link,l_sole,r_sole,l_upper_leg,r_upper_leg,l_elbow_1,r_elbow_1) - imu_frame - true - true - - - - (root_link,1,0) - (chest,1,2) - (l_upper_arm,3,2) - (l_elbow_1, 3, 4) - (r_upper_arm,4,2) - (l_hand_dh_frame,3,6) - (r_elbow_1, 4, 4) - (r_hand_dh_frame,4,6) - (l_upper_leg,5,2) - (l_lower_leg,5,3) - (l_ankle_1,5,4) - (l_foot_dh_frame,5,5) - (r_upper_leg,6,2) - (r_lower_leg,6,3) - (r_ankle_1,6,4) - (r_foot_dh_frame,6,5) - - - - true - root_link - (torso_pitch,torso_roll,torso_yaw,neck_pitch,neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow) - - - - (l_hand,l_hand_dh_frame) - (r_hand,r_hand_dh_frame) - (l_foot,l_sole,root_link) - (r_foot,r_sole,root_link) - (l_foot,l_sole,l_sole) - (r_foot,r_sole,r_sole) - - - - - - left_leg_mc - right_leg_mc - torso_mc - right_arm_mc - left_arm_mc - head_mc - - inertial - - left_upper_arm_strain - right_upper_arm_strain - left_upper_leg_strain - right_upper_leg_strain - left_lower_leg_strain - right_lower_leg_strain - - - - - - - - diff --git a/devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml b/devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml index ef60cbf..cd4444d 100644 --- a/devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml +++ b/devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml @@ -87,7 +87,6 @@ 0.2 - diff --git a/devices/baseEstimatorV1/app/robots/iCubGenova04/wholebodydynamics-external.xml b/devices/baseEstimatorV1/app/robots/iCubGenova04/wholebodydynamics-external.xml deleted file mode 100644 index abcd0f2..0000000 --- a/devices/baseEstimatorV1/app/robots/iCubGenova04/wholebodydynamics-external.xml +++ /dev/null @@ -1,81 +0,0 @@ - - - - - (torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup,r_wrist_pitch,r_wrist_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) - model.urdf - (0,0,-9.81) - (l_hand,r_hand,root_link,l_sole,r_sole,l_upper_leg,r_upper_leg,l_elbow_1,r_elbow_1) - imu_frame - true - true - - - - (root_link,1,0) - (chest,1,2) - (l_upper_arm,3,2) - (l_elbow_1, 3, 4) - (r_upper_arm,4,2) - (l_hand_dh_frame,3,6) - (r_elbow_1, 4, 4) - (r_hand_dh_frame,4,6) - (l_upper_leg,5,2) - (l_lower_leg,5,3) - (l_ankle_1,5,4) - (l_foot_dh_frame,5,5) - (r_upper_leg,6,2) - (r_lower_leg,6,3) - (r_ankle_1,6,4) - (r_foot_dh_frame,6,5) - - - - true - root_link - (torso_pitch,torso_roll,torso_yaw,neck_pitch,neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow) - - - - (l_hand,l_hand_dh_frame) - (r_hand,r_hand_dh_frame) - (l_foot,l_sole,root_link) - (r_foot,r_sole,root_link) - (l_foot,l_sole,l_sole) - (r_foot,r_sole,r_sole) - - - - - - left_leg_mc - right_leg_mc - torso_mc - right_arm_mc - left_arm_mc - head_mc - - inertial - - left_upper_arm_strain - right_upper_arm_strain - left_upper_leg_strain - right_upper_leg_strain - left_lower_leg_strain - right_lower_leg_strain - - - - - - - - From fd3c170d5cd35450439e05bcf5f03eb1015ac438 Mon Sep 17 00:00:00 2001 From: Prashanth Date: Mon, 1 Jul 2019 16:13:40 +0200 Subject: [PATCH 5/5] [fbeV1] fix yarprobotinterface warnings --- .../app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml | 3 ++- .../app/robots/iCubGenova04/launch-fbe-analogsens.xml | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml index 9409289..254cba6 100644 --- a/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml +++ b/devices/baseEstimatorV1/app/robots/iCubGazeboV2_5/launch-fbe-analogsens.xml @@ -6,7 +6,8 @@ GNU Lesser General Public License v2.1 or any later version. --> - + + /icubSim/torso diff --git a/devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml b/devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml index cd4444d..007f251 100644 --- a/devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml +++ b/devices/baseEstimatorV1/app/robots/iCubGenova04/launch-fbe-analogsens.xml @@ -6,7 +6,8 @@ GNU Lesser General Public License v2.1 or any later version. --> - + + /icub/torso