From ddd4efad543d2436aee3d0c47e7596af6c514f61 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 6 Dec 2023 16:52:57 +0100 Subject: [PATCH 01/97] Add a new simulation model --- .../vehicle_model/sim_model_pymodels.hpp | 150 ++++++++++++++++++ .../vehicle_model/sim_model_pymodels.cpp | 123 ++++++++++++++ 2 files changed, 273 insertions(+) create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp new file mode 100644 index 0000000000000..c83934cf9a9ec --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -0,0 +1,150 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +#include +#include + +#include +#include +#include +/** + * @class SimModelPymodels + * @brief calculate delay steering dynamics + */ +class SimModelPymodels : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] vx_lim velocity limit [m/s] + * @param [in] steer_lim steering limit [rad] + * @param [in] vx_rate_lim acceleration limit [m/ss] + * @param [in] steer_rate_lim steering angular velocity limit [rad/ss] + * @param [in] wheelbase vehicle wheelbase length [m] + * @param [in] dt delta time information to set input buffer for delay + * @param [in] vx_delay time delay for velocity command [s] + * @param [in] vx_time_constant time constant for 1D model of velocity dynamics + * @param [in] steer_delay time delay for steering command [s] + * @param [in] steer_time_constant time constant for 1D model of steering dynamics + * @param [in] steer_dead_band dead band for steering angle [rad] + */ + SimModelPymodels( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double vx_delay, double vx_time_constant, double steer_delay, + double steer_time_constant, double steer_dead_band); + + /** + * @brief destructor + */ + ~SimModelPymodels() = default; + +private: + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant --> TODO what is this? + + enum IDX { + X = 0, + Y, + YAW, + VX, + STEER, + }; + enum IDX_U { + VX_DES = 0, + STEER_DES, + }; + + const double vx_lim_; //!< @brief velocity limit + const double vx_rate_lim_; //!< @brief acceleration limit + const double steer_lim_; //!< @brief steering limit [rad] + const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] + const double wheelbase_; //!< @brief vehicle wheelbase length [m] + double prev_vx_ = 0.0; + double current_ax_ = 0.0; + + std::deque vx_input_queue_; //!< @brief buffer for velocity command + std::deque steer_input_queue_; //!< @brief buffer for angular velocity command + const double vx_delay_; //!< @brief time delay for velocity command [s] + const double vx_time_constant_; + //!< @brief time constant for 1D model of velocity dynamics + const double steer_delay_; //!< @brief time delay for angular-velocity command [s] + const double + steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics + const double steer_dead_band_; //!< @brief dead band for steering angle [rad] + + /** + * @brief set queue buffer for input command + * @param [in] dt delta time + */ + void initializeInputQueue(const double & dt); + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle longitudinal velocity + */ + double getVx() override; + + /** + * @brief get vehicle lateral velocity + */ + double getVy() override; + + /** + * @brief get vehicle longitudinal acceleration + */ + double getAx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with delay steering model + * @param [in] state current model state + * @param [in] input input vector to model + */ + Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp new file mode 100644 index 0000000000000..dffd0d3a70312 --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -0,0 +1,123 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" + +#include + +SimModelPymodels::SimModelPymodels( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double vx_delay, double vx_time_constant, double steer_delay, + double steer_time_constant, double steer_dead_band) +: SimModelInterface(5 /* dim x */, 2 /* dim u */), + MIN_TIME_CONSTANT(0.03), + vx_lim_(vx_lim), + vx_rate_lim_(vx_rate_lim), + steer_lim_(steer_lim), + steer_rate_lim_(steer_rate_lim), + wheelbase_(wheelbase), + vx_delay_(vx_delay), + vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), + steer_delay_(steer_delay), + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), + steer_dead_band_(steer_dead_band) +{ + //initializeInputQueue(dt); +} + +double SimModelPymodels::getX() +{ + return state_(IDX::X); +} +double SimModelPymodels::getY() +{ + return state_(IDX::Y); +} +double SimModelPymodels::getYaw() +{ + return state_(IDX::YAW); +} +double SimModelPymodels::getVx() +{ + return state_(IDX::VX); +} +double SimModelPymodels::getVy() +{ + return 0.0; +} +double SimModelPymodels::getAx() +{ + return current_ax_; +} +double SimModelPymodels::getWz() +{ + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; +} +double SimModelPymodels::getSteer() +{ + return state_(IDX::STEER); +} +void SimModelPymodels::update(const double & dt) +{ + Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); + + vx_input_queue_.push_back(input_(IDX_U::VX_DES)); + delayed_input(IDX_U::VX_DES) = vx_input_queue_.front(); + vx_input_queue_.pop_front(); + steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); + delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); + steer_input_queue_.pop_front(); + // do not use deadzone_delta_steer (Steer IF does not exist in this model) + updateRungeKutta(dt, delayed_input); + current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; + prev_vx_ = input_(IDX_U::VX_DES); +} + +Eigen::VectorXd SimModelPymodels::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + auto sat = [](double val, double u, double l) { return std::max(std::min(val, u), l); }; + + const double vx = sat(state(IDX::VX), vx_lim_, -vx_lim_); + const double steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_); + const double yaw = state(IDX::YAW); + const double delay_input_vx = input(IDX_U::VX_DES); + const double delay_input_steer = input(IDX_U::STEER_DES); + const double delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); + const double vx_rate = sat(-(vx - delay_vx_des) / vx_time_constant_, vx_rate_lim_, -vx_rate_lim_); + const double delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); + + const double steer_diff = steer - delay_steer_des; + const double steer_diff_with_dead_band = std::invoke([&]() { + if (steer_diff > steer_dead_band_) { + return steer_diff - steer_dead_band_; + } else if (steer_diff < -steer_dead_band_) { + return steer_diff + steer_dead_band_; + } else { + return 0.0; + } + }); + + const double steer_rate = + sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vx * cos(yaw); + d_state(IDX::Y) = vx * sin(yaw); + d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; + d_state(IDX::VX) = vx_rate; + d_state(IDX::STEER) = steer_rate; + + return d_state; +} From ef04b5790ca63e50bfb4c4fc4667361799053b25 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 6 Dec 2023 18:36:40 +0100 Subject: [PATCH 02/97] Add new model -> currently same as delay_steer_vel --- simulator/simple_planning_simulator/CMakeLists.txt | 1 + .../simple_planning_simulator_core.hpp | 3 ++- .../vehicle_model/sim_model.hpp | 1 + .../vehicle_model/sim_model_pymodels.hpp | 2 +- .../simple_planning_simulator_core.cpp | 13 ++++++++++--- .../vehicle_model/sim_model_pymodels.cpp | 4 ++-- 6 files changed, 17 insertions(+), 7 deletions(-) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 87d0f7e5fef0b..06fe692c8e3a7 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -14,6 +14,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp + src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 38f689c124439..1b0e3c7dd3350 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -210,7 +210,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node DELAY_STEER_ACC_GEARED = 3, IDEAL_STEER_VEL = 4, DELAY_STEER_VEL = 5, - DELAY_STEER_MAP_ACC_GEARED = 6 + DELAY_STEER_MAP_ACC_GEARED = 6, + PYMODELS = 7 } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp index 9fa4516c1a522..fcc4baaffcb93 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp @@ -19,6 +19,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index c83934cf9a9ec..b0b1e24975106 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -27,7 +27,7 @@ * @class SimModelPymodels * @brief calculate delay steering dynamics */ -class SimModelPymodels : public SimModelInterface +class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ { public: /** diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 9a0ea6a6c3659..63c3aa9922a65 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -278,7 +278,12 @@ void SimplePlanningSimulator::initialize_vehicle_model() vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, acceleration_map_path); - } else { + } else if (vehicle_model_type_str == "PYMODELS"){ + vehicle_model_type_ = VehicleModelType::PYMODELS; + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, + vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, steer_dead_band); + }else{ throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } } @@ -473,7 +478,8 @@ void SimplePlanningSimulator::set_input( if ( vehicle_model_type_ == VehicleModelType::IDEAL_STEER_VEL || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL || + vehicle_model_type_ == VehicleModelType::PYMODELS) { input << vel, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || @@ -574,7 +580,8 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED) { state << x, y, yaw, vx; } else if ( // NOLINT - vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL || + vehicle_model_type_ == VehicleModelType::PYMODELS) { state << x, y, yaw, vx, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC || diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index dffd0d3a70312..e8a533dd6b824 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -33,7 +33,7 @@ SimModelPymodels::SimModelPymodels( steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), steer_dead_band_(steer_dead_band) { - //initializeInputQueue(dt); + std::cout << dt << std::endl; } double SimModelPymodels::getX() @@ -71,7 +71,7 @@ double SimModelPymodels::getSteer() void SimModelPymodels::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); - + std::cout << dt << std::endl; vx_input_queue_.push_back(input_(IDX_U::VX_DES)); delayed_input(IDX_U::VX_DES) = vx_input_queue_.front(); vx_input_queue_.pop_front(); From b2d18af48a5a3928b5e2a7e171fe6850d9c3956d Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Fri, 8 Dec 2023 11:29:36 +0100 Subject: [PATCH 03/97] Add python.h library --- simulator/simple_planning_simulator/CMakeLists.txt | 6 ++++++ .../vehicle_model/sim_model_pymodels.hpp | 3 +++ .../vehicle_model/sim_model_pymodels.cpp | 1 + 3 files changed, 10 insertions(+) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 06fe692c8e3a7..58e00e3a2517f 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -4,6 +4,10 @@ project(simple_planning_simulator) find_package(autoware_cmake REQUIRED) autoware_package() +#find_package(Python REQUIRED) # Python testing +#include_directories(${Python_INCLUDE_DIRS}) +include_directories("/usr/include/python3.10") + # Component ament_auto_add_library(${PROJECT_NAME} SHARED include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -30,6 +34,8 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_exe ) +target_link_libraries(${PROJECT_NAME} ${Python_LIBRARIES}) + if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_simple_planning_simulator test/test_simple_planning_simulator.cpp diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index b0b1e24975106..16e2938133ff8 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -23,6 +23,9 @@ #include #include #include + +#include + /** * @class SimModelPymodels * @brief calculate delay steering dynamics diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index e8a533dd6b824..3986c2472f782 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -15,6 +15,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" #include +#include SimModelPymodels::SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, From 0cf474d47000e45d8ff89216fff5f5eadfc8b18e Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Fri, 8 Dec 2023 20:24:46 +0100 Subject: [PATCH 04/97] Add working simple steering model programed in python launched from C++ --- .../simple_planning_simulator/CMakeLists.txt | 4 +- .../vehicle_model/sim_model_pymodels.hpp | 15 ++ .../vehicle_model/sim_model_pymodels.cpp | 146 +++++++++++++++--- 3 files changed, 142 insertions(+), 23 deletions(-) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 58e00e3a2517f..bfe79488ce899 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -4,8 +4,8 @@ project(simple_planning_simulator) find_package(autoware_cmake REQUIRED) autoware_package() -#find_package(Python REQUIRED) # Python testing -#include_directories(${Python_INCLUDE_DIRS}) +find_package(Python REQUIRED) # Python testing +include_directories(${Python_INCLUDE_DIRS}) include_directories("/usr/include/python3.10") # Component diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index 16e2938133ff8..fc7bba2a8476f 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -25,6 +25,7 @@ #include #include +#include /** * @class SimModelPymodels @@ -90,6 +91,20 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics const double steer_dead_band_; //!< @brief dead band for steering angle [rad] + + PyObject *steer_base_model; + + int num_states_steering; + int num_inputs_steering; + + // Create vectors to store system input and state (C++) + Eigen::VectorXd state_steering; + Eigen::VectorXd input_steering; + + // Create vectors to store system input and state (python) + PyObject *py_state_steering, *py_input_steering; + + /** * @brief set queue buffer for input command * @param [in] dt delta time diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 3986c2472f782..b15daaaed5c34 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -16,6 +16,8 @@ #include #include +#include +#include SimModelPymodels::SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, @@ -34,7 +36,81 @@ SimModelPymodels::SimModelPymodels( steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), steer_dead_band_(steer_dead_band) { + + // Initialize python library + dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. + /* + More about the line above here: + https://stackoverflow.com/questions/60719987/embedding-python-which-uses-numpy-in-c-doesnt-work-in-library-dynamically-loa + https://mail.python.org/pipermail/new-bugs-announce/2008-November/003322.html + https://stackoverflow.com/questions/67891197/ctypes-cpython-39-x86-64-linux-gnu-so-undefined-symbol-pyfloat-type-in-embedd + https://man7.org/linux/man-pages/man3/dlopen.3.html + */ + Py_Initialize(); + PyObject *py_module, *py_module_dict, *python_class; + + char *model_save_path = (char*)"/home/tomas/f1tenth/ws_testing/base_model_save"; + + // Import python module + py_module = PyImport_Import(PyUnicode_FromString((char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis")); + + if (py_module == nullptr) { + PyErr_Print(); + return; + } + + py_module_dict = PyModule_GetDict(py_module); + if (py_module_dict == nullptr) { + PyErr_Print(); + return; + } + + python_class = PyDict_GetItemString(py_module_dict, "SimpleSteeringHyst"); + if (python_class == nullptr) { + PyErr_Print(); + return; + } + + if (PyCallable_Check(python_class)) { + steer_base_model = PyObject_CallObject(python_class, nullptr); + Py_DECREF(python_class); + } else { + Py_DECREF(python_class); + return; + } + + // Load model params + PyObject *load_params_succ = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"load_params"), PyUnicode_FromString(model_save_path), NULL); + + if(!PyBool_Check(load_params_succ)){ + return; + } + + PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"reset"), NULL); + + + // Set number model states and inputs + num_states_steering = 1; + num_inputs_steering = 1; + + // Initialize state and input vectors + state_steering = Eigen::VectorXd::Zero(num_states_steering); + input_steering = Eigen::VectorXd::Zero(num_inputs_steering); + + + py_state_steering = PyList_New(num_states_steering); + py_input_steering = PyList_New(num_inputs_steering); + + for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ + PyList_SetItem(py_state_steering, state_idx, PyFloat_FromDouble(state_steering(state_idx))); + } + + for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ + PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); + } + std::cout << dt << std::endl; + std::cout << "OKAAAAAYYYYYYY" << std::endl; } double SimModelPymodels::getX() @@ -72,15 +148,43 @@ double SimModelPymodels::getSteer() void SimModelPymodels::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); - std::cout << dt << std::endl; + //std::cout << dt << std::endl; + + // Model the delay of speed vx_input_queue_.push_back(input_(IDX_U::VX_DES)); delayed_input(IDX_U::VX_DES) = vx_input_queue_.front(); vx_input_queue_.pop_front(); - steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); - delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); - steer_input_queue_.pop_front(); + + // Model the delay of steering -- TODO remove this after implementing python models + // steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); + // delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); + // steer_input_queue_.pop_front(); + + // Simulate the system (car kinematics and dynamics) // do not use deadzone_delta_steer (Steer IF does not exist in this model) updateRungeKutta(dt, delayed_input); + + // Simulate the steering system using python model + input_steering(0) = input_(IDX_U::STEER_DES); + + for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ + PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); + } + + // Call forward() of the python model + PyObject *value = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"forward"), py_input_steering, py_state_steering, NULL); + py_state_steering = PyTuple_GetItem(value, 0); // The first output of the model forward is the next state + // PyObject *py_delayed_input = PyTuple_GetItem(value, 1); // The second output of the model forward is delayed input + + // State vector from python to C++ + for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ + state_steering(state_idx) = PyFloat_AsDouble(PyList_GetItem(PyList_GetItem(py_state_steering, 0), state_idx)); + } + //std::cout << "Required steer: " << input_(IDX_U::STEER_DES) << " State steering: " << state_steering(0) << std::endl; + + state_(IDX::STEER) = state_steering(0); + + // Calculate current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; prev_vx_ = input_(IDX_U::VX_DES); } @@ -94,31 +198,31 @@ Eigen::VectorXd SimModelPymodels::calcModel( const double steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_); const double yaw = state(IDX::YAW); const double delay_input_vx = input(IDX_U::VX_DES); - const double delay_input_steer = input(IDX_U::STEER_DES); + //const double delay_input_steer = input(IDX_U::STEER_DES); const double delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); const double vx_rate = sat(-(vx - delay_vx_des) / vx_time_constant_, vx_rate_lim_, -vx_rate_lim_); - const double delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); - - const double steer_diff = steer - delay_steer_des; - const double steer_diff_with_dead_band = std::invoke([&]() { - if (steer_diff > steer_dead_band_) { - return steer_diff - steer_dead_band_; - } else if (steer_diff < -steer_dead_band_) { - return steer_diff + steer_dead_band_; - } else { - return 0.0; - } - }); - - const double steer_rate = - sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); + // const double delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); + + // const double steer_diff = steer - delay_steer_des; + // const double steer_diff_with_dead_band = std::invoke([&]() { + // if (steer_diff > steer_dead_band_) { + // return steer_diff - steer_dead_band_; + // } else if (steer_diff < -steer_dead_band_) { + // return steer_diff + steer_dead_band_; + // } else { + // return 0.0; + // } + // }); + + //const double steer_rate = + // sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vx * cos(yaw); d_state(IDX::Y) = vx * sin(yaw); d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; d_state(IDX::VX) = vx_rate; - d_state(IDX::STEER) = steer_rate; + d_state(IDX::STEER) = 0.0; //Use python models to update steer return d_state; } From ec662973fb3c0cfcee35b5fecd19391fbe120b33 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 13 Dec 2023 18:21:52 +0100 Subject: [PATCH 05/97] Fixing linker problem (WIP) --- .../simple_planning_simulator/CMakeLists.txt | 21 +- .../vehicle_model/sim_model_pymodels.hpp | 23 +- .../vehicle_model/sim_model_pymodels.cpp | 196 +++++++++++------- 3 files changed, 154 insertions(+), 86 deletions(-) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index bfe79488ce899..74c75db70fee5 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -4,9 +4,17 @@ project(simple_planning_simulator) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(Python REQUIRED) # Python testing -include_directories(${Python_INCLUDE_DIRS}) -include_directories("/usr/include/python3.10") +find_package(Python3 COMPONENTS Interpreter Development) +find_package(pybind11 CONFIG) + +message("Pybind11_FOUND:${pybind11_FOUND}") +message("Pybind11_Version:${pybind11_VERSION}") + +message("Python_FOUND:${Python3_FOUND}") +message("Python_VERSION:${Python3_VERSION}") +message("Python_Development_FOUND:${Python3_Development_FOUND}") +message("Python_LIBRARIES:${Python3_LIBRARIES}") + # Component ament_auto_add_library(${PROJECT_NAME} SHARED @@ -26,6 +34,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${tf2_INCLUDE_DIRS}) + target_compile_options(${PROJECT_NAME} PRIVATE -Wno-old-style-cast) # RCLCPP_ERROR_THROTTLE() has built-in old-style casts. # Node executable @@ -34,7 +43,11 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_exe ) -target_link_libraries(${PROJECT_NAME} ${Python_LIBRARIES}) +# target_link_options(${PROJECT_NAME} PRIVATE -shared -fPIC -export-dynamic) # + +# target_link_libraries(${PROJECT_NAME} ${Python3_LIBRARIES}) 3 +# /usr/lib/x86_64-linux-gnu/libpython3.10.so +target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_simple_planning_simulator diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index fc7bba2a8476f..1aa65c338d9b2 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -61,6 +61,24 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ private: const double MIN_TIME_CONSTANT; //!< @brief minimum time constant --> TODO what is this? + /* + Specify string names for states and inputs. So we can automatically map states and + inputs of this model to states and inputs of the python submodels. + In order not to compare strings each iteration, we compute mappings between the states + and inputs of this model and python submodels. + */ + const char* STATE_NAMES[5] = {"POS_X", "POS_Y", "YAW", "VX", "STEER"}; + const char* INPUT_NAMES[2] = {"VX_DES", "STEER_DES"}; + + /* + Index means state or input in this model. First 5 indexes are states, last 2 indexes are inputs. + ---------------------------- { STATES }{INPUTS} */ + int MAP_TO_PYSTEER_INPUT[7] = {-1, -1, -1, -1, -1, -1, -1}; // <<- Value means index of the input in python steer model. -1 means not used. + /* + Index means state in this model. + ------------------------------- { STATES }*/ + int MAP_FROM_PYSTEER_OUTPUT[5] = {-1, -1, -1, -1, -1}; // <<- Value means index of the output in python steer model. -1 means not used. + enum IDX { X = 0, Y, @@ -72,6 +90,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ VX_DES = 0, STEER_DES, }; + const double vx_lim_; //!< @brief velocity limit const double vx_rate_lim_; //!< @brief acceleration limit @@ -92,7 +111,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ const double steer_dead_band_; //!< @brief dead band for steering angle [rad] - PyObject *steer_base_model; + //PyObject *steer_base_model; int num_states_steering; int num_inputs_steering; @@ -102,7 +121,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ Eigen::VectorXd input_steering; // Create vectors to store system input and state (python) - PyObject *py_state_steering, *py_input_steering; + //PyObject *py_state_steering, *py_input_steering; /** diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index b15daaaed5c34..0aee8a36a5aa5 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -15,9 +15,10 @@ #include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" #include -#include -#include -#include +// #include +// #include +#include +namespace py = pybind11; SimModelPymodels::SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, @@ -38,7 +39,9 @@ SimModelPymodels::SimModelPymodels( { // Initialize python library - dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. + //dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. + + /* More about the line above here: https://stackoverflow.com/questions/60719987/embedding-python-which-uses-numpy-in-c-doesnt-work-in-library-dynamically-loa @@ -46,68 +49,102 @@ SimModelPymodels::SimModelPymodels( https://stackoverflow.com/questions/67891197/ctypes-cpython-39-x86-64-linux-gnu-so-undefined-symbol-pyfloat-type-in-embedd https://man7.org/linux/man-pages/man3/dlopen.3.html */ - Py_Initialize(); - PyObject *py_module, *py_module_dict, *python_class; + // Py_Initialize(); + // PyObject *py_module, *py_module_dict, *python_class; - char *model_save_path = (char*)"/home/tomas/f1tenth/ws_testing/base_model_save"; + // char *model_save_path = (char*)"/home/tomas/f1tenth/ws_testing/base_model_save"; // Import python module - py_module = PyImport_Import(PyUnicode_FromString((char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis")); - - if (py_module == nullptr) { - PyErr_Print(); - return; - } - - py_module_dict = PyModule_GetDict(py_module); - if (py_module_dict == nullptr) { - PyErr_Print(); - return; - } - - python_class = PyDict_GetItemString(py_module_dict, "SimpleSteeringHyst"); - if (python_class == nullptr) { - PyErr_Print(); - return; - } - - if (PyCallable_Check(python_class)) { - steer_base_model = PyObject_CallObject(python_class, nullptr); - Py_DECREF(python_class); - } else { - Py_DECREF(python_class); - return; - } + // py_module = PyImport_Import(PyUnicode_FromString((char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis")); + + // if (py_module == nullptr) { + // PyErr_Print(); + // return; + // } + + // py_module_dict = PyModule_GetDict(py_module); + // if (py_module_dict == nullptr) { + // PyErr_Print(); + // return; + // } + + // python_class = PyDict_GetItemString(py_module_dict, "SimpleSteeringHyst"); + // if (python_class == nullptr) { + // PyErr_Print(); + // return; + // } + + // if (PyCallable_Check(python_class)) { + // steer_base_model = PyObject_CallObject(python_class, nullptr); + // Py_DECREF(python_class); + // } else { + // Py_DECREF(python_class); + // return; + // } + + + // TODO call function to get model signal names + // From the names create a mapping table for correctly pass arguments to the python models + + // PyObject *py_sig_state_names = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"get_sig_state_names"), NULL); + + // char *my_result; + + // PyObject * temp_bytes = PyUnicode_AsEncodedString(PyList_GetItem(py_sig_state_names, 0), "UTF-8", "strict"); + // my_result = PyBytes_AS_STRING(temp_bytes); + // my_result = strdup(my_result); + // Py_DECREF(temp_bytes); + + // wcstombs(buffer, PyList_GetItem(py_sig_state_names, 0), sizeof(buffer)); + + // int len_ = PyList_Size(py_sig_state_names); + // std::cout << "STATE NAMES: -" << my_result << "-" << std::endl; + // std::cout << "STEER: " << len_ << std::endl; + // std::cout << "STEER: " << strcmp(my_result, "STEER") << std::endl; + // std::cout << "STEER: " << strcmp(my_result, STATE_NAMES[4]) << std::endl; + // std::cout << "STEERR: " << strcmp(my_result, "STEERR") << std::endl; + // Load model params - PyObject *load_params_succ = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"load_params"), PyUnicode_FromString(model_save_path), NULL); + // PyObject *load_params_succ = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"load_params"), PyUnicode_FromString(model_save_path), NULL); - if(!PyBool_Check(load_params_succ)){ - return; - } + // if(!PyBool_Check(load_params_succ)){ + // return; + // } - PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"reset"), NULL); + // Reset the python model after loading the parameter file + // PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"reset"), NULL); // Set number model states and inputs - num_states_steering = 1; - num_inputs_steering = 1; + // num_states_steering = 1; + // num_inputs_steering = 1; - // Initialize state and input vectors - state_steering = Eigen::VectorXd::Zero(num_states_steering); - input_steering = Eigen::VectorXd::Zero(num_inputs_steering); + // // Initialize state and input vectors + // state_steering = Eigen::VectorXd::Zero(num_states_steering); + // input_steering = Eigen::VectorXd::Zero(num_inputs_steering); - py_state_steering = PyList_New(num_states_steering); - py_input_steering = PyList_New(num_inputs_steering); + // py_state_steering = PyList_New(num_states_steering); + // py_input_steering = PyList_New(num_inputs_steering); - for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ - PyList_SetItem(py_state_steering, state_idx, PyFloat_FromDouble(state_steering(state_idx))); - } + // for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ + // PyList_SetItem(py_state_steering, state_idx, PyFloat_FromDouble(state_steering(state_idx))); + // } - for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ - PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); - } + // for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ + // PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); + // } + + py::scoped_interpreter guard{}; // start the interpreter and keep it alive + + py::print("Testing py::print"); // use the Python API + + // py::module_ sys = py::module_::import("control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis"); + + py::module_ torch = py::module_::import("torch"); + + // py::object result = torch.attr("add")(1, 2); std::cout << dt << std::endl; std::cout << "OKAAAAAYYYYYYY" << std::endl; @@ -156,33 +193,47 @@ void SimModelPymodels::update(const double & dt) vx_input_queue_.pop_front(); // Model the delay of steering -- TODO remove this after implementing python models - // steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); - // delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); - // steer_input_queue_.pop_front(); + steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); + delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); + steer_input_queue_.pop_front(); // Simulate the system (car kinematics and dynamics) // do not use deadzone_delta_steer (Steer IF does not exist in this model) updateRungeKutta(dt, delayed_input); // Simulate the steering system using python model - input_steering(0) = input_(IDX_U::STEER_DES); - for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ - PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); - } + // TODO: define inputs to the model in a more general way + /* + Following inputs to the steering system can be used: + + - IDX_U::STEER_DES + - IDX::VX + */ + // input_steering(0) = 0; + + + // for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ + // PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); + // } // Call forward() of the python model - PyObject *value = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"forward"), py_input_steering, py_state_steering, NULL); - py_state_steering = PyTuple_GetItem(value, 0); // The first output of the model forward is the next state + // PyObject *value = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"forward"), py_input_steering, py_state_steering, NULL); + // py_state_steering = PyTuple_GetItem(value, 0); // The first output of the model forward is the next state // PyObject *py_delayed_input = PyTuple_GetItem(value, 1); // The second output of the model forward is delayed input // State vector from python to C++ - for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ - state_steering(state_idx) = PyFloat_AsDouble(PyList_GetItem(PyList_GetItem(py_state_steering, 0), state_idx)); - } + // for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ + // state_steering(state_idx) = PyFloat_AsDouble(PyList_GetItem(PyList_GetItem(py_state_steering, 0), state_idx)); + // } //std::cout << "Required steer: " << input_(IDX_U::STEER_DES) << " State steering: " << state_steering(0) << std::endl; - state_(IDX::STEER) = state_steering(0); + // TODO: define outputs of the model in a more general way + /* + Following outputs from the steering system can be used: + - IDX::STEER + */ + state_(IDX::STEER) = 0; // Calculate current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; @@ -198,24 +249,9 @@ Eigen::VectorXd SimModelPymodels::calcModel( const double steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_); const double yaw = state(IDX::YAW); const double delay_input_vx = input(IDX_U::VX_DES); - //const double delay_input_steer = input(IDX_U::STEER_DES); const double delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); const double vx_rate = sat(-(vx - delay_vx_des) / vx_time_constant_, vx_rate_lim_, -vx_rate_lim_); - // const double delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); - - // const double steer_diff = steer - delay_steer_des; - // const double steer_diff_with_dead_band = std::invoke([&]() { - // if (steer_diff > steer_dead_band_) { - // return steer_diff - steer_dead_band_; - // } else if (steer_diff < -steer_dead_band_) { - // return steer_diff + steer_dead_band_; - // } else { - // return 0.0; - // } - // }); - - //const double steer_rate = - // sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vx * cos(yaw); From e183aac41312f62af573ca6cbf7665a7b02ebb07 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 18 Dec 2023 20:16:45 +0100 Subject: [PATCH 06/97] Split python models to models and learned model --- simulator/learned_model/CMakeLists.txt | 86 +++++++++++ simulator/learned_model/README.md | 1 + .../include/sim_pymodel_steer_vel.hpp | 15 ++ simulator/learned_model/package.xml | 25 ++++ .../simple_planning_simulator/CMakeLists.txt | 22 +-- .../vehicle_model/sim_model_pymodels.hpp | 30 ++-- .../simple_planning_simulator/package.xml | 3 + .../vehicle_model/sim_model_pymodels.cpp | 138 ++++++------------ 8 files changed, 203 insertions(+), 117 deletions(-) create mode 100644 simulator/learned_model/CMakeLists.txt create mode 100644 simulator/learned_model/README.md create mode 100644 simulator/learned_model/include/sim_pymodel_steer_vel.hpp create mode 100644 simulator/learned_model/package.xml diff --git a/simulator/learned_model/CMakeLists.txt b/simulator/learned_model/CMakeLists.txt new file mode 100644 index 0000000000000..d3d4025b4a77f --- /dev/null +++ b/simulator/learned_model/CMakeLists.txt @@ -0,0 +1,86 @@ +cmake_minimum_required(VERSION 3.14.4) +project(learned_model) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + + +## dependencies +find_package(ament_cmake_auto REQUIRED) +# ament_auto_find_build_dependencies() + +# find_package(autoware_cmake REQUIRED) +# autoware_package() + +find_package(Python3 COMPONENTS Interpreter Development) +find_package(pybind11 CONFIG) + +message("Pybind11_FOUND:${pybind11_FOUND}") +message("Pybind11_Version:${pybind11_VERSION}") + +message("Python_FOUND:${Python3_FOUND}") +message("Python_VERSION:${Python3_VERSION}") +message("Python_Development_FOUND:${Python3_Development_FOUND}") +message("Python_LIBRARIES:${Python3_LIBRARIES}") + +include_directories(include) + +# Component + +# ament_auto_add_library +add_library(${PROJECT_NAME} SHARED + # .cpp and .hpp files to instal + include/sim_pymodel_steer_vel.hpp +) + + +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$") + + + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME}/ +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) +target_compile_options(${PROJECT_NAME} PRIVATE -fvisibility=hidden) + +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(Python3 pybind11) + + +message("CMAKE_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX}") + +target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + + +if(BUILD_TESTING) + +endif() + + + +ament_auto_package() diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md new file mode 100644 index 0000000000000..d490d175585e6 --- /dev/null +++ b/simulator/learned_model/README.md @@ -0,0 +1 @@ +# learned_model diff --git a/simulator/learned_model/include/sim_pymodel_steer_vel.hpp b/simulator/learned_model/include/sim_pymodel_steer_vel.hpp new file mode 100644 index 0000000000000..d232e8c7ea8c7 --- /dev/null +++ b/simulator/learned_model/include/sim_pymodel_steer_vel.hpp @@ -0,0 +1,15 @@ +#ifndef LEARNED_MODEL__SIM_PYMODEL_STEER_VEL_HPP_ +#define LEARNED_MODEL__SIM_PYMODEL_STEER_VEL_HPP_ + +#include +#include +namespace py = pybind11; + +class SimPymodelSteerVel { + public: + int test_variable = 123; + py::scoped_interpreter guard{}; + py::print("Testing py::print LIB"); // use the Python API +}; + +#endif // LEARNED_MODEL__SIM_PYMODEL_STEER_VEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/package.xml b/simulator/learned_model/package.xml new file mode 100644 index 0000000000000..0b93550680900 --- /dev/null +++ b/simulator/learned_model/package.xml @@ -0,0 +1,25 @@ + + + + learned_model + 1.0.0 + learned_model for simple_planning_simulator + Takamasa Horibe + Tomoya Kimura + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_cmake + + + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 74c75db70fee5..dd4478dbae6c8 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -4,16 +4,11 @@ project(simple_planning_simulator) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(Python3 COMPONENTS Interpreter Development) -find_package(pybind11 CONFIG) +find_package(learned_model REQUIRED) -message("Pybind11_FOUND:${pybind11_FOUND}") -message("Pybind11_Version:${pybind11_VERSION}") - -message("Python_FOUND:${Python3_FOUND}") -message("Python_VERSION:${Python3_VERSION}") -message("Python_Development_FOUND:${Python3_Development_FOUND}") -message("Python_LIBRARIES:${Python3_LIBRARIES}") +message("learned_model_FOUND: ${learned_model_FOUND}") +message("learned_model_LIBRARIES: ${learned_model_LIBRARIES}") +message("learned_model_INCLUDE_DIRS: ${learned_model_INCLUDE_DIRS}") # Component @@ -34,8 +29,10 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${tf2_INCLUDE_DIRS}) +ament_target_dependencies(${PROJECT_NAME} learned_model) + -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-old-style-cast) # RCLCPP_ERROR_THROTTLE() has built-in old-style casts. +target_compile_options(${PROJECT_NAME} PRIVATE -Wno-old-style-cast -fvisibility=hidden) # RCLCPP_ERROR_THROTTLE() has built-in old-style casts. # Node executable rclcpp_components_register_node(${PROJECT_NAME} @@ -43,11 +40,6 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_exe ) -# target_link_options(${PROJECT_NAME} PRIVATE -shared -fPIC -export-dynamic) # - -# target_link_libraries(${PROJECT_NAME} ${Python3_LIBRARIES}) 3 -# /usr/lib/x86_64-linux-gnu/libpython3.10.so -target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_simple_planning_simulator diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index 1aa65c338d9b2..3267221ac88d2 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -24,8 +24,8 @@ #include #include -#include -#include +// #include +// namespace py = pybind11; /** * @class SimModelPymodels @@ -47,6 +47,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ * @param [in] steer_delay time delay for steering command [s] * @param [in] steer_time_constant time constant for 1D model of steering dynamics * @param [in] steer_dead_band dead band for steering angle [rad] + * @param [in] HELLO_FROM_CODING_GOD PRAISE THE MESSIAH!!!!! <3 */ SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, @@ -67,17 +68,17 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ In order not to compare strings each iteration, we compute mappings between the states and inputs of this model and python submodels. */ - const char* STATE_NAMES[5] = {"POS_X", "POS_Y", "YAW", "VX", "STEER"}; - const char* INPUT_NAMES[2] = {"VX_DES", "STEER_DES"}; + // const char* STATE_NAMES[5] = {"POS_X", "POS_Y", "YAW", "VX", "STEER"}; + // const char* INPUT_NAMES[2] = {"VX_DES", "STEER_DES"}; /* Index means state or input in this model. First 5 indexes are states, last 2 indexes are inputs. ---------------------------- { STATES }{INPUTS} */ - int MAP_TO_PYSTEER_INPUT[7] = {-1, -1, -1, -1, -1, -1, -1}; // <<- Value means index of the input in python steer model. -1 means not used. + // int MAP_TO_PYSTEER_INPUT[7] = {-1, -1, -1, -1, -1, -1, -1}; // <<- Value means index of the input in python steer model. -1 means not used. /* Index means state in this model. ------------------------------- { STATES }*/ - int MAP_FROM_PYSTEER_OUTPUT[5] = {-1, -1, -1, -1, -1}; // <<- Value means index of the output in python steer model. -1 means not used. + // int MAP_FROM_PYSTEER_OUTPUT[5] = {-1, -1, -1, -1, -1}; // <<- Value means index of the output in python steer model. -1 means not used. enum IDX { X = 0, @@ -90,6 +91,8 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ VX_DES = 0, STEER_DES, }; + + // py::scoped_interpreter guard{}; const double vx_lim_; //!< @brief velocity limit @@ -113,15 +116,20 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ //PyObject *steer_base_model; - int num_states_steering; - int num_inputs_steering; + // int num_states_steering; + // int num_inputs_steering; // Create vectors to store system input and state (C++) - Eigen::VectorXd state_steering; - Eigen::VectorXd input_steering; + // Eigen::VectorXd state_steering; + // Eigen::VectorXd input_steering; // Create vectors to store system input and state (python) - //PyObject *py_state_steering, *py_input_steering; + // PyObject *py_state_steering, *py_input_steering; + + // std::vector state_steering; + // std::vector input_steering; + + // py::object steering_module_class; /** diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index a2038486029dc..478804e5da151 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_cmake + learned_model autoware_auto_control_msgs autoware_auto_mapping_msgs @@ -35,6 +36,8 @@ tier4_vehicle_msgs vehicle_info_util + + learned_model launch_ros ament_cmake_ros diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 0aee8a36a5aa5..140fddbd75187 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -17,8 +17,11 @@ #include // #include // #include -#include -namespace py = pybind11; +// #include +// #include +// namespace py = pybind11; + +#include SimModelPymodels::SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, @@ -39,9 +42,7 @@ SimModelPymodels::SimModelPymodels( { // Initialize python library - //dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. - - + // dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. /* More about the line above here: https://stackoverflow.com/questions/60719987/embedding-python-which-uses-numpy-in-c-doesnt-work-in-library-dynamically-loa @@ -49,102 +50,68 @@ SimModelPymodels::SimModelPymodels( https://stackoverflow.com/questions/67891197/ctypes-cpython-39-x86-64-linux-gnu-so-undefined-symbol-pyfloat-type-in-embedd https://man7.org/linux/man-pages/man3/dlopen.3.html */ - // Py_Initialize(); - // PyObject *py_module, *py_module_dict, *python_class; - + // start the interpreter and keep it alive + // char *model_save_path = (char*)"/home/tomas/f1tenth/ws_testing/base_model_save"; - // Import python module - // py_module = PyImport_Import(PyUnicode_FromString((char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis")); + // // Import python module + // py::module_ steering_model_module = py::module_::import("control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis"); - // if (py_module == nullptr) { - // PyErr_Print(); - // return; - // } + // steering_module_class = steering_model_module.attr("SimpleSteeringHyst")(nullptr); - // py_module_dict = PyModule_GetDict(py_module); - // if (py_module_dict == nullptr) { - // PyErr_Print(); - // return; - // } - // python_class = PyDict_GetItemString(py_module_dict, "SimpleSteeringHyst"); - // if (python_class == nullptr) { - // PyErr_Print(); - // return; - // } - - // if (PyCallable_Check(python_class)) { - // steer_base_model = PyObject_CallObject(python_class, nullptr); - // Py_DECREF(python_class); - // } else { - // Py_DECREF(python_class); - // return; - // } - - - // TODO call function to get model signal names - // From the names create a mapping table for correctly pass arguments to the python models - - // PyObject *py_sig_state_names = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"get_sig_state_names"), NULL); + // // TODO call function to get model signal names + // // From the names create a mapping table for correctly pass arguments to the python models + // py::object sig_state_names = steering_module_class.attr("get_sig_state_names")(); // char *my_result; - - // PyObject * temp_bytes = PyUnicode_AsEncodedString(PyList_GetItem(py_sig_state_names, 0), "UTF-8", "strict"); + // PyObject *temp_bytes = PyUnicode_AsEncodedString(PyList_GetItem(sig_state_names.ptr(), 0), "UTF-8", "strict"); // my_result = PyBytes_AS_STRING(temp_bytes); // my_result = strdup(my_result); // Py_DECREF(temp_bytes); - // wcstombs(buffer, PyList_GetItem(py_sig_state_names, 0), sizeof(buffer)); + // // wcstombs(buffer, PyList_GetItem(py_sig_state_names, 0), sizeof(buffer)); - // int len_ = PyList_Size(py_sig_state_names); + // // int len_ = PyList_Size(py_sig_state_names); // std::cout << "STATE NAMES: -" << my_result << "-" << std::endl; - // std::cout << "STEER: " << len_ << std::endl; - // std::cout << "STEER: " << strcmp(my_result, "STEER") << std::endl; - // std::cout << "STEER: " << strcmp(my_result, STATE_NAMES[4]) << std::endl; - // std::cout << "STEERR: " << strcmp(my_result, "STEERR") << std::endl; + // // std::cout << "STEER: " << len_ << std::endl; + // // std::cout << "STEER: " << strcmp(my_result, "STEER") << std::endl; + // // std::cout << "STEER: " << strcmp(my_result, STATE_NAMES[4]) << std::endl; + // // std::cout << "STEERR: " << strcmp(my_result, "STEERR") << std::endl; - // Load model params - // PyObject *load_params_succ = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"load_params"), PyUnicode_FromString(model_save_path), NULL); + // // Load model params + // py::object load_params_succ = steering_module_class.attr("load_params")(model_save_path); - // if(!PyBool_Check(load_params_succ)){ - // return; - // } - // Reset the python model after loading the parameter file - // PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"reset"), NULL); + // // Reset the python model after loading the parameter file + // steering_module_class.attr("reset")(); - // Set number model states and inputs + // // Set number model states and inputs // num_states_steering = 1; // num_inputs_steering = 1; // // Initialize state and input vectors - // state_steering = Eigen::VectorXd::Zero(num_states_steering); - // input_steering = Eigen::VectorXd::Zero(num_inputs_steering); - - - // py_state_steering = PyList_New(num_states_steering); - // py_input_steering = PyList_New(num_inputs_steering); - - // for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ - // PyList_SetItem(py_state_steering, state_idx, PyFloat_FromDouble(state_steering(state_idx))); + // for (int i = 0; i < num_inputs_steering; i++){ + // input_steering.push_back(0.0); // } - // for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ - // PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); + // for (int i = 0; i < num_states_steering; i++){ + // state_steering.push_back(0.0); // } - py::scoped_interpreter guard{}; // start the interpreter and keep it alive - - py::print("Testing py::print"); // use the Python API - // py::module_ sys = py::module_::import("control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis"); + // std::vector state_steering(num_states_steering, 0); + // std::vector input_steering(num_inputs_steering, 0); + + // std::cout << action_delayed[0]<< std::endl; + + SimPymodelSteerVel test_; + std::cout << "-----> " << test_.test_variable << std::endl; - py::module_ torch = py::module_::import("torch"); - // py::object result = torch.attr("add")(1, 2); + // py::print("Testing py::print"); // use the Python API std::cout << dt << std::endl; std::cout << "OKAAAAAYYYYYYY" << std::endl; @@ -192,11 +159,6 @@ void SimModelPymodels::update(const double & dt) delayed_input(IDX_U::VX_DES) = vx_input_queue_.front(); vx_input_queue_.pop_front(); - // Model the delay of steering -- TODO remove this after implementing python models - steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); - delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); - steer_input_queue_.pop_front(); - // Simulate the system (car kinematics and dynamics) // do not use deadzone_delta_steer (Steer IF does not exist in this model) updateRungeKutta(dt, delayed_input); @@ -210,29 +172,23 @@ void SimModelPymodels::update(const double & dt) - IDX_U::STEER_DES - IDX::VX */ - // input_steering(0) = 0; - - - // for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ - // PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); - // } + // input_steering[0] = input_(IDX_U::STEER_DES); + - // Call forward() of the python model - // PyObject *value = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"forward"), py_input_steering, py_state_steering, NULL); - // py_state_steering = PyTuple_GetItem(value, 0); // The first output of the model forward is the next state - // PyObject *py_delayed_input = PyTuple_GetItem(value, 1); // The second output of the model forward is delayed input + // // Call forward() of the python model + // py::tuple res = steering_module_class.attr("forward")(state_steering, input_steering); - // State vector from python to C++ - // for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ - // state_steering(state_idx) = PyFloat_AsDouble(PyList_GetItem(PyList_GetItem(py_state_steering, 0), state_idx)); - // } - //std::cout << "Required steer: " << input_(IDX_U::STEER_DES) << " State steering: " << state_steering(0) << std::endl; + // std::vector next_state = res[0].cast>>()[0]; + // state_steering[0] = next_state[0]; + // std::vector action_delayed = res[1].cast>>()[0]; + // std::cout << next_state[0]<< std::endl; // TODO: define outputs of the model in a more general way /* Following outputs from the steering system can be used: - IDX::STEER */ + // state_(IDX::STEER) = next_state[0]; state_(IDX::STEER) = 0; // Calculate From e4a65b4237b921298eae6b790359a4834628b0e8 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 19 Dec 2023 11:18:44 +0100 Subject: [PATCH 07/97] Changes from Maxime --- simulator/learned_model/CMakeLists.txt | 80 ++----------------- .../include/sim_pymodel_steer_vel.hpp | 12 ++- simulator/learned_model/package.xml | 5 +- .../src/sim_pymodel_steer_vel.cpp | 1 + .../simple_planning_simulator/CMakeLists.txt | 18 +---- .../simple_planning_simulator/package.xml | 4 +- .../vehicle_model/sim_model_pymodels.cpp | 2 +- 7 files changed, 25 insertions(+), 97 deletions(-) create mode 100644 simulator/learned_model/src/sim_pymodel_steer_vel.cpp diff --git a/simulator/learned_model/CMakeLists.txt b/simulator/learned_model/CMakeLists.txt index d3d4025b4a77f..31f068529a440 100644 --- a/simulator/learned_model/CMakeLists.txt +++ b/simulator/learned_model/CMakeLists.txt @@ -1,86 +1,22 @@ cmake_minimum_required(VERSION 3.14.4) project(learned_model) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - - - -## dependencies -find_package(ament_cmake_auto REQUIRED) -# ament_auto_find_build_dependencies() - -# find_package(autoware_cmake REQUIRED) -# autoware_package() +find_package(autoware_cmake REQUIRED) +autoware_package() find_package(Python3 COMPONENTS Interpreter Development) find_package(pybind11 CONFIG) -message("Pybind11_FOUND:${pybind11_FOUND}") -message("Pybind11_Version:${pybind11_VERSION}") - -message("Python_FOUND:${Python3_FOUND}") -message("Python_VERSION:${Python3_VERSION}") -message("Python_Development_FOUND:${Python3_Development_FOUND}") -message("Python_LIBRARIES:${Python3_LIBRARIES}") - -include_directories(include) - -# Component - -# ament_auto_add_library -add_library(${PROJECT_NAME} SHARED - # .cpp and .hpp files to instal - include/sim_pymodel_steer_vel.hpp +ament_auto_add_library(${PROJECT_NAME} SHARED + # include/sim_pymodel_steer_vel.hpp + src/sim_pymodel_steer_vel.cpp ) - - -target_include_directories(${PROJECT_NAME} - PUBLIC - "$" - "$") - - +target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) +target_include_directories(${PROJECT_NAME} PRIVATE ${Python3_INCLUDE_DIRS}) install( DIRECTORY include/ - DESTINATION include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} ) -install( - TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - -set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) -target_compile_options(${PROJECT_NAME} PRIVATE -fvisibility=hidden) - -ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(Python3 pybind11) - - -message("CMAKE_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX}") - -target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) - - -if(BUILD_TESTING) - -endif() - - - ament_auto_package() diff --git a/simulator/learned_model/include/sim_pymodel_steer_vel.hpp b/simulator/learned_model/include/sim_pymodel_steer_vel.hpp index d232e8c7ea8c7..5237d27acc833 100644 --- a/simulator/learned_model/include/sim_pymodel_steer_vel.hpp +++ b/simulator/learned_model/include/sim_pymodel_steer_vel.hpp @@ -5,11 +5,15 @@ #include namespace py = pybind11; -class SimPymodelSteerVel { - public: - int test_variable = 123; +class SimPymodelSteerVel +{ +public: + int test_variable = 123; + SimPymodelSteerVel() + { py::scoped_interpreter guard{}; - py::print("Testing py::print LIB"); // use the Python API + py::print("Testing py::print LIB"); // use the Python API + } }; #endif // LEARNED_MODEL__SIM_PYMODEL_STEER_VEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/package.xml b/simulator/learned_model/package.xml index 0b93550680900..9fa8c511a858d 100644 --- a/simulator/learned_model/package.xml +++ b/simulator/learned_model/package.xml @@ -13,7 +13,8 @@ autoware_cmake - + pybind11 + python3-dev ament_cmake_ros ament_lint_auto @@ -22,4 +23,4 @@ ament_cmake - + \ No newline at end of file diff --git a/simulator/learned_model/src/sim_pymodel_steer_vel.cpp b/simulator/learned_model/src/sim_pymodel_steer_vel.cpp new file mode 100644 index 0000000000000..cce3792a68a19 --- /dev/null +++ b/simulator/learned_model/src/sim_pymodel_steer_vel.cpp @@ -0,0 +1 @@ +#include "learned_model/sim_pymodel_steer_vel.hpp" //"learned_model/include/sim_pymodel_steer_vel.hpp" \ No newline at end of file diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index dd4478dbae6c8..5d22a98ec8a28 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -2,15 +2,10 @@ cmake_minimum_required(VERSION 3.14) project(simple_planning_simulator) find_package(autoware_cmake REQUIRED) +find_package(Python3 COMPONENTS Interpreter Development) +find_package(learned_model) autoware_package() -find_package(learned_model REQUIRED) - -message("learned_model_FOUND: ${learned_model_FOUND}") -message("learned_model_LIBRARIES: ${learned_model_LIBRARIES}") -message("learned_model_INCLUDE_DIRS: ${learned_model_INCLUDE_DIRS}") - - # Component ament_auto_add_library(${PROJECT_NAME} SHARED include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -27,20 +22,13 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp src/simple_planning_simulator/utils/csv_loader.cpp ) -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${tf2_INCLUDE_DIRS}) - -ament_target_dependencies(${PROJECT_NAME} learned_model) - - -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-old-style-cast -fvisibility=hidden) # RCLCPP_ERROR_THROTTLE() has built-in old-style casts. - +target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS}) # Node executable rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "simulation::simple_planning_simulator::SimplePlanningSimulator" EXECUTABLE ${PROJECT_NAME}_exe ) - if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_simple_planning_simulator test/test_simple_planning_simulator.cpp diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 478804e5da151..52f8591c0ee48 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -12,7 +12,6 @@ autoware_cmake autoware_cmake - learned_model autoware_auto_control_msgs autoware_auto_mapping_msgs @@ -22,6 +21,7 @@ geometry_msgs lanelet2_core lanelet2_extension + learned_model motion_utils nav_msgs rclcpp @@ -36,8 +36,6 @@ tier4_vehicle_msgs vehicle_info_util - - learned_model launch_ros ament_cmake_ros diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 140fddbd75187..db4a96488d9d6 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -21,7 +21,7 @@ // #include // namespace py = pybind11; -#include +#include "learned_model/include/sim_pymodel_steer_vel.hpp" SimModelPymodels::SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, From 6bebdcd8a525bc7014a38074a1a4387347b4c8ed Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 19 Dec 2023 11:51:32 +0100 Subject: [PATCH 08/97] Fix build issiues --- simulator/learned_model/CMakeLists.txt | 1 - simulator/learned_model/src/sim_pymodel_steer_vel.cpp | 2 +- simulator/simple_planning_simulator/CMakeLists.txt | 4 ++-- .../vehicle_model/sim_model_pymodels.cpp | 2 +- 4 files changed, 4 insertions(+), 5 deletions(-) diff --git a/simulator/learned_model/CMakeLists.txt b/simulator/learned_model/CMakeLists.txt index 31f068529a440..be3e67d0cd114 100644 --- a/simulator/learned_model/CMakeLists.txt +++ b/simulator/learned_model/CMakeLists.txt @@ -8,7 +8,6 @@ find_package(Python3 COMPONENTS Interpreter Development) find_package(pybind11 CONFIG) ament_auto_add_library(${PROJECT_NAME} SHARED - # include/sim_pymodel_steer_vel.hpp src/sim_pymodel_steer_vel.cpp ) target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) diff --git a/simulator/learned_model/src/sim_pymodel_steer_vel.cpp b/simulator/learned_model/src/sim_pymodel_steer_vel.cpp index cce3792a68a19..98222cac22134 100644 --- a/simulator/learned_model/src/sim_pymodel_steer_vel.cpp +++ b/simulator/learned_model/src/sim_pymodel_steer_vel.cpp @@ -1 +1 @@ -#include "learned_model/sim_pymodel_steer_vel.hpp" //"learned_model/include/sim_pymodel_steer_vel.hpp" \ No newline at end of file +#include "../include/sim_pymodel_steer_vel.hpp" \ No newline at end of file diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 5d22a98ec8a28..85ce6986ee111 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -3,7 +3,7 @@ project(simple_planning_simulator) find_package(autoware_cmake REQUIRED) find_package(Python3 COMPONENTS Interpreter Development) -find_package(learned_model) +find_package(learned_model REQUIRED) autoware_package() # Component @@ -22,7 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp src/simple_planning_simulator/utils/csv_loader.cpp ) -target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS}) +target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS} ${learned_model_INCLUDE_DIRS}) # Node executable rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "simulation::simple_planning_simulator::SimplePlanningSimulator" diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index db4a96488d9d6..d7f4f3635850b 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -21,7 +21,7 @@ // #include // namespace py = pybind11; -#include "learned_model/include/sim_pymodel_steer_vel.hpp" +#include "sim_pymodel_steer_vel.hpp" SimModelPymodels::SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, From 30dfee22ff2c2a27b8254dca688c9fc99e231f8e Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Thu, 18 Jan 2024 18:29:15 +0100 Subject: [PATCH 09/97] Add C++ library that runs python models --- .../include/model_connections_helpers.hpp | 5 + .../include/pymodel_base_error.hpp | 81 +++++++ .../include/pymodel_interconnected_model.hpp | 207 ++++++++++++++++++ .../include/pymodel_interface.hpp | 39 ++++ .../include/pymodel_simple_model.hpp | 180 +++++++++++++++ .../include/sim_pymodel_steer_vel.hpp | 19 -- .../src/model_connections_helpers.cpp | 1 + .../learned_model/src/pymodel_base_error.cpp | 1 + .../src/pymodel_interconnected_model.cpp | 1 + .../learned_model/src/pymodel_interface.cpp | 1 + .../src/pymodel_simple_model.cpp | 1 + .../src/sim_pymodel_steer_vel.cpp | 1 - 12 files changed, 517 insertions(+), 20 deletions(-) create mode 100644 simulator/learned_model/include/model_connections_helpers.hpp create mode 100644 simulator/learned_model/include/pymodel_base_error.hpp create mode 100644 simulator/learned_model/include/pymodel_interconnected_model.hpp create mode 100644 simulator/learned_model/include/pymodel_interface.hpp create mode 100644 simulator/learned_model/include/pymodel_simple_model.hpp delete mode 100644 simulator/learned_model/include/sim_pymodel_steer_vel.hpp create mode 100644 simulator/learned_model/src/model_connections_helpers.cpp create mode 100644 simulator/learned_model/src/pymodel_base_error.cpp create mode 100644 simulator/learned_model/src/pymodel_interconnected_model.cpp create mode 100644 simulator/learned_model/src/pymodel_interface.cpp create mode 100644 simulator/learned_model/src/pymodel_simple_model.cpp delete mode 100644 simulator/learned_model/src/sim_pymodel_steer_vel.cpp diff --git a/simulator/learned_model/include/model_connections_helpers.hpp b/simulator/learned_model/include/model_connections_helpers.hpp new file mode 100644 index 0000000000000..0acdc60333e20 --- /dev/null +++ b/simulator/learned_model/include/model_connections_helpers.hpp @@ -0,0 +1,5 @@ +#ifndef LEARNED_MODEL__MODEL_CONNECTION_HELPERS_ +#define LEARNED_MODEL__MODEL_CONNECTION_HELPERS_ + + +#endif // LEARNED_MODEL__MODEL_CONNECTION_HELPERS_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_base_error.hpp b/simulator/learned_model/include/pymodel_base_error.hpp new file mode 100644 index 0000000000000..ef2e6116ea072 --- /dev/null +++ b/simulator/learned_model/include/pymodel_base_error.hpp @@ -0,0 +1,81 @@ +#ifndef LEARNED_MODEL__SIM_PYMODEL_BASE_ERROR_HPP_ +#define LEARNED_MODEL__SIM_PYMODEL_BASE_ERROR_HPP_ + +#include "pymodel_simple_model.hpp" +#include "pymodel_interface.hpp" + + +class SimPymodelBaseError : public PymodelInterface +{ +private: + PymodelSimpleModel base_model; + PymodelSimpleModel error_model; + +public: + SimPymodelBaseError(std::tuple base_desc, std::tuple error_desc) + : base_model(std::get<0>(base_desc), std::get<1>(base_desc), std::get<2>(base_desc)), + error_model(std::get<0>(error_desc), std::get<1>(error_desc), std::get<2>(error_desc)) + { + } + + /** + * @brief create a map from model signal vector to python model inputs + * @param [in] signal_vec_names names of signals in model signal vector + */ + void mapInputs(std::vector signals_vec_names) override + { + base_model.mapInputs(signals_vec_names); + error_model.mapInputs(signals_vec_names); + } + + /** + * @brief create a map from python outputs to model signal vector + * @param [in] signal_vec_names names of signals in model signal vector + */ + void mapOutputs(std::vector signals_vec_names) override + { + base_model.mapOutputs(signals_vec_names); + error_model.mapOutputs(signals_vec_names); + } + + /** + * @brief get names of inputs of python model + */ + std::vector getInputNames() override + { + std::vector base_input_name = base_model.getInputNames(); + std::vector error_input_name = error_model.getInputNames(); + base_input_name.insert(base_input_name.end(), error_input_name.begin(), error_input_name.end()); + return base_input_name; + } + + /** + * @brief get names of states of python model + */ + std::vector getStateNames() override + { + std::vector base_state_name = base_model.getStateNames(); + std::vector error_state_name = error_model.getStateNames(); + base_state_name.insert(base_state_name.end(), error_state_name.begin(), error_state_name.end()); + return base_state_name; + } + + /** + * @brief calculate the next state of this submodule + * @param [in] model_signals_vec values of signals in model signal vector + * @param [in] model_signals_vec_next values of signals in model signal vector to update + */ + std::vector getNextState(std::vector model_signals_vec, std::vector model_signals_vec_next) override + { + std::vector base_next_state = base_model.getNextState(model_signals_vec, model_signals_vec_next); + std::vector base_next_state_delayed = base_model.signals_w_delayed_input; + std::vector next_state_error(model_signals_vec.size()); + std::fill(next_state_error.begin(), next_state_error.end(), 0.0); + std::vector error_correction = error_model.getNextState(base_next_state_delayed, next_state_error); // need to change input in all_variable input to delayed one from base model + for (size_t i = 0; i < base_next_state.size(); i++) base_next_state[i] += error_correction[i]; + return base_next_state; + } +}; + + +#endif // LEARNED_MODEL__SIM_PYMODEL_BASE_ERROR_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_interconnected_model.hpp b/simulator/learned_model/include/pymodel_interconnected_model.hpp new file mode 100644 index 0000000000000..0dfbc23df70e7 --- /dev/null +++ b/simulator/learned_model/include/pymodel_interconnected_model.hpp @@ -0,0 +1,207 @@ +#ifndef LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ +#define LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ + +#include "pymodel_base_error.hpp" +#include "pymodel_interface.hpp" +#include "pymodel_simple_model.hpp" +#include "model_connections_helpers.hpp" + +#include +#include +#include + +#include + +namespace py = pybind11; +// sim_pymodel_steer_vel +class InterconnectedModel +{ + int num_signals; + + std::vector model_signals_vec; + std::vector signals_vec_names; + + std::vector> submodels; + + std::vector + map_in_to_sig_vec; // index in "map_in_to_pyin" is index in "py_inputs" and value in + // "map_in_to_pyin" is index in "all_variables_names" + std::vector + map_sig_vec_to_out; // index in "map_pyout_to_out" is index in "pymodel_outputs" and value in + // "map_pyout_to_out" is index in "all_variables_names" + +public: + py::scoped_interpreter guard{}; // start the interpreter and keep it alive + + /** + * @brief constructor + */ + InterconnectedModel() + { + // Initialize python library + dlopen( + "libpython3.10.so", + RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. + /* + More about the line above here: + https://stackoverflow.com/questions/60719987/embedding-python-which-uses-numpy-in-c-doesnt-work-in-library-dynamically-loa + https://mail.python.org/pipermail/new-bugs-announce/2008-November/003322.html + https://stackoverflow.com/questions/67891197/ctypes-cpython-39-x86-64-linux-gnu-so-undefined-symbol-pyfloat-type-in-embedd + https://man7.org/linux/man-pages/man3/dlopen.3.html + */ + } + +private: + std::vector createConnectionsMap(std::vector connection_names_1, std::vector connection_names_2) + { + std::vector connection_map; + // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is + // index in "connection_names_1" + for (auto name_2 : connection_names_2) { + int mapped_idx = -1; // -1 means that we cannot create a connection between some signals + for (size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { + if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same + // and we can create connection + mapped_idx = NAME_1_IDX; + break; + } + } + connection_map.push_back(mapped_idx); + } + return connection_map; + } + + void mapInputs(std::vector in_names) + { + // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index in "signals_vec_names" + map_in_to_sig_vec = createConnectionsMap(signals_vec_names, in_names); + } + + void mapOutputs(std::vector out_names) + { + // index in "map_sig_vec_to_out" is index in "out_names" and value in "map_sig_vec_to_out" is index in "signals_vec_names" + map_sig_vec_to_out = createConnectionsMap(signals_vec_names, out_names); + } + + void addNamesToSigVec(const std::vector & names) + { + for (char * name : names) { + if (std::find(signals_vec_names.begin(), signals_vec_names.end(), name) == signals_vec_names.end()) { + signals_vec_names.push_back(name); + } + } + } + + void getSignalNames() + { + for (auto & submodel : submodels) { + addNamesToSigVec(submodel->getInputNames()); + addNamesToSigVec(submodel->getStateNames()); + } + } + +public: + /** + * @brief automatically create connections between PSIM and all of the sub-models + * @param [in] in_names string names of inputs available from PSIM + * @param [in] out_names string names of outputs required by PSIM + */ + void generateConnections(std::vector in_names, std::vector out_names) + { + getSignalNames(); + num_signals = signals_vec_names.size(); + for (int i = 0; i < num_signals; i++) model_signals_vec.push_back(0); + + for (auto & submodel : submodels) { + submodel->mapInputs(signals_vec_names); + submodel->mapOutputs(signals_vec_names); + } + mapInputs(in_names); + mapOutputs(out_names); + } + + /** + * @brief add a sub-model consisting of base + error model + * @param [in] submodel_desc descriptor of the sub-model + */ + void addSubmodel(std::tuple submodel_desc) + { + const auto [lib_path, param_path, class_name] = submodel_desc; + submodels.push_back(std::unique_ptr( + new PymodelSimpleModel(lib_path, param_path, class_name))); + } + + /** + * @brief add a sub-model consisting of base + error sub-model + * @param [in] base_desc descriptor of base sub-model + * @param [in] error_desc descriptor of error sub-model + */ + void addSubmodelBaseError( + std::tuple base_desc, std::tuple error_desc) + { + // SimPymodelBaseError pymodel(base_desc, error_desc); + // submodels.push_back(pymodel); + std::cout << "HAHA111" << std::endl; + submodels.push_back( + std::unique_ptr(new SimPymodelBaseError(base_desc, error_desc))); + } + + /** + * @brief set a new model state if it was changed using PSIM interface (mainly position and orientation) + * @param [in] new_state new state set by PSIM + */ + void initState(std::vector new_state) + { + bool state_changed_externally = false; + + for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { + if ( + abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > + 1e-6) { + state_changed_externally = true; + break; + } + } + + if (state_changed_externally) { + std::cout << "Reseting model... ----***----" << std::endl; + + std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); + + for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { + model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] = new_state[PSIM_STATE_IDX]; + } + } + } + + /** + * @brief compute next step of the PSIM model using python sub-models + * @param [in] psim_input vector of input values provided by PSIM + */ + std::vector updatePymodel(std::vector psim_input) + { + // map input to vector of all variables + for (size_t PSIM_INPUT_IDX = 0; PSIM_INPUT_IDX < psim_input.size(); PSIM_INPUT_IDX++) { + model_signals_vec[map_in_to_sig_vec[PSIM_INPUT_IDX]] = psim_input[PSIM_INPUT_IDX]; + } + + // Compute forward pass through all models (order should not matter) + std::vector model_signals_vec_next(num_signals); + for (auto & submodel : submodels) { + model_signals_vec_next = submodel->getNextState(model_signals_vec, model_signals_vec_next); + } + + // Map vector of all variables to + std::vector psim_next_state; + for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < map_sig_vec_to_out.size(); PSIM_STATE_IDX++) { + psim_next_state.push_back(model_signals_vec_next[map_sig_vec_to_out[PSIM_STATE_IDX]]); + } + + // Update vector of all variables + model_signals_vec = model_signals_vec_next; + + return psim_next_state; + } +}; + +#endif // LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_interface.hpp b/simulator/learned_model/include/pymodel_interface.hpp new file mode 100644 index 0000000000000..6e57384aa4c9d --- /dev/null +++ b/simulator/learned_model/include/pymodel_interface.hpp @@ -0,0 +1,39 @@ +#ifndef LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ +#define LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ + +class PymodelInterface + { + public: + // virtual ~PymodelInterface() = 0; // <-- build fails after uncommenting this + + /** + * @brief get names of inputs of python model + */ + virtual std::vector getInputNames() = 0; + + /** + * @brief get names of states of python model + */ + virtual std::vector getStateNames() = 0; + + /** + * @brief create a map from model signal vector to python model inputs + * @param [in] signal_vec_names names of signals in model signal vector + */ + virtual void mapInputs(std::vector signals_vec_names) = 0; + + /** + * @brief create a map from python outputs to model signal vector + * @param [in] signal_vec_names names of signals in model signal vector + */ + virtual void mapOutputs(std::vector signals_vec_names) = 0; + + /** + * @brief calculate the next state of this submodule + * @param [in] model_signals_vec values of signals in model signal vector + * @param [in] model_signals_vec_next values of signals in model signal vector to update + */ + virtual std::vector getNextState(std::vector model_signals_vec, std::vector model_signals_vec_next) = 0; +}; + +#endif // LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_simple_model.hpp b/simulator/learned_model/include/pymodel_simple_model.hpp new file mode 100644 index 0000000000000..693cd12329292 --- /dev/null +++ b/simulator/learned_model/include/pymodel_simple_model.hpp @@ -0,0 +1,180 @@ +#ifndef LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ +#define LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ + +#include +#include + +#include "pymodel_interface.hpp" +#include "model_connections_helpers.hpp" + +namespace py = pybind11; + +/** + * @class SimModelPymodels + * @brief This class is an interface between C++ and python models. + */ +class PymodelSimpleModel : public PymodelInterface +{ +private: + char* pymodel_import_name = nullptr; + + int num_inputs_py; + int num_outputs_py; + + py::object py_model_class; + + std::vector map_sig_vec_to_pyin; // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is index in "signals_vec_names" + std::vector map_pyout_to_sig_vec; // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in "map_pyout_to_sig_vec" is index in "signals_vec_names" + + std::vector pymodel_input_names; + std::vector pymodel_state_names; + +public: + std::vector signals_w_delayed_input; + + /** + * @brief constructor + * @param [in] pymodel_import_name_ path to python model + * @param [in] param_file_path path to saved parameter file of the python sub-model + * @param [in] py_class_name name of the python class + */ + PymodelSimpleModel(char* pymodel_import_name_, char* param_file_path, char* py_class_name) + { + // Import model class + pymodel_import_name = pymodel_import_name_; + if (pymodel_import_name != nullptr) + { + // Import python module + py::module_ imported_module = py::module_::import(pymodel_import_name); + // Initialize model class from imported module + py_model_class = imported_module.attr(py_class_name)(nullptr); + } + else + { + // TODO throw exception/error + return; + } + + // Load model parameters and reset the model + if (param_file_path != nullptr) + { + py::object load_params_succ = py_model_class.attr("load_params")(param_file_path); + py_model_class.attr("reset")(); + std::cout << "Params loaded" << std::endl; + } + else + { + // TODO warning that using default model params + } + + py::list pymodel_state_names_ = py_model_class.attr("get_sig_state_names")(); + num_outputs_py = pymodel_state_names_.size(); + for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++){ + pymodel_state_names.push_back(PyBytes_AS_STRING(PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); + } + + py::list pymodel_input_names_ = py_model_class.attr("get_sig_action_names")(); + num_inputs_py = pymodel_input_names_.size(); + for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++){ + pymodel_input_names.push_back(PyBytes_AS_STRING(PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); + } + + std::cout << "PymodelInterface import name: " << pymodel_import_name << std::endl; + } + + /** + * @brief calculate the next state of a python model + * @param [in] model_signals_vec all available inputs from PSIM + */ + std::vector getNextState(std::vector model_signals_vec, std::vector model_signals_vec_next) override + { + + // get inputs and states of the python model from the vector of all signals + std::vector py_inputs(num_inputs_py); + std::vector py_state(num_outputs_py); + py_inputs = fillVectorUsingMap(py_inputs, model_signals_vec, map_sig_vec_to_pyin, true); + py_state = fillVectorUsingMap(py_state, model_signals_vec, map_pyout_to_sig_vec, true); + + // forward pass through the base model + py::tuple res = py_model_class.attr("forward")(py_inputs, py_state); + std::vector py_state_next = res[0].cast>>()[0]; + + // for (auto state : model_state) std::cout << "STATE AFTER : " << state << std::endl; + + std::vector action_delayed = res[1].cast>>()[0]; + + // map outputs from python model to required outputs + std::vector next_state = fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); + + signals_w_delayed_input = fillVectorUsingMap(action_delayed, model_signals_vec_next, map_sig_vec_to_pyin, false); + + return next_state; + } + + /** + * @brief get names of inputs of python model + */ + std::vector getInputNames() override + { + return pymodel_input_names; + } + + /** + * @brief get names of states of python model + */ + std::vector getStateNames() override + { + return pymodel_state_names; + } + + /** + * @brief create a map from model signal vector to python model inputs + * @param [in] signal_vec_names names of signals in model signal vector + */ + void mapInputs(std::vector signals_vec_names) override + { + // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is index in "signals_vec_names" + map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); + } + + /** + * @brief create a map from python outputs to model signal vector + * @param [in] signal_vec_names names of signals in model signal vector + */ + void mapOutputs(std::vector signals_vec_names) override + { + // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in "map_pyout_to_sig_vec" is index in "signals_vec_names" + map_pyout_to_sig_vec = createConnectionsMap(signals_vec_names, pymodel_state_names); + } + +private: + std::vector createConnectionsMap(std::vector connection_names_1, std::vector connection_names_2){ + std::vector connection_map; + // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is index in "connection_names_1" + for (auto name_2 : connection_names_2){ + int mapped_idx = -1; // -1 means that we cannot create a connection between some signals + for (size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++){ + if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0){ // 0 means strings are the same and we can create connection + mapped_idx = NAME_1_IDX; + break; + } + } + connection_map.push_back(mapped_idx); + } + return connection_map; + } + + std::vector fillVectorUsingMap(std::vector vector1, std::vector vector2, std::vector map, bool inverse){ + // index in "map" is index in "vector1" and value in "map" is index in "vector2" + // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 + for(size_t idx = 0; idx < map.size(); idx++) + { + if (map[idx] == -1) continue; + inverse ? vector1[idx] = vector2[map[idx]] : vector2[map[idx]] = vector1[idx]; + } + return inverse ? vector1 : vector2; + } + +}; + +#endif // LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/sim_pymodel_steer_vel.hpp b/simulator/learned_model/include/sim_pymodel_steer_vel.hpp deleted file mode 100644 index 5237d27acc833..0000000000000 --- a/simulator/learned_model/include/sim_pymodel_steer_vel.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef LEARNED_MODEL__SIM_PYMODEL_STEER_VEL_HPP_ -#define LEARNED_MODEL__SIM_PYMODEL_STEER_VEL_HPP_ - -#include -#include -namespace py = pybind11; - -class SimPymodelSteerVel -{ -public: - int test_variable = 123; - SimPymodelSteerVel() - { - py::scoped_interpreter guard{}; - py::print("Testing py::print LIB"); // use the Python API - } -}; - -#endif // LEARNED_MODEL__SIM_PYMODEL_STEER_VEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/src/model_connections_helpers.cpp b/simulator/learned_model/src/model_connections_helpers.cpp new file mode 100644 index 0000000000000..70349b80c9796 --- /dev/null +++ b/simulator/learned_model/src/model_connections_helpers.cpp @@ -0,0 +1 @@ +#include "../include/model_connections_helpers.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_base_error.cpp b/simulator/learned_model/src/pymodel_base_error.cpp new file mode 100644 index 0000000000000..f829bd20bbe64 --- /dev/null +++ b/simulator/learned_model/src/pymodel_base_error.cpp @@ -0,0 +1 @@ +#include "../include/pymodel_base_error.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_interconnected_model.cpp b/simulator/learned_model/src/pymodel_interconnected_model.cpp new file mode 100644 index 0000000000000..fac2a02ca5d68 --- /dev/null +++ b/simulator/learned_model/src/pymodel_interconnected_model.cpp @@ -0,0 +1 @@ +#include "../include/pymodel_interconnected_model.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_interface.cpp b/simulator/learned_model/src/pymodel_interface.cpp new file mode 100644 index 0000000000000..302ef331fd00a --- /dev/null +++ b/simulator/learned_model/src/pymodel_interface.cpp @@ -0,0 +1 @@ +#include "../include/pymodel_interface.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_simple_model.cpp b/simulator/learned_model/src/pymodel_simple_model.cpp new file mode 100644 index 0000000000000..f3a11c3a8aa7d --- /dev/null +++ b/simulator/learned_model/src/pymodel_simple_model.cpp @@ -0,0 +1 @@ +#include "../include/pymodel_simple_model.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/sim_pymodel_steer_vel.cpp b/simulator/learned_model/src/sim_pymodel_steer_vel.cpp deleted file mode 100644 index 98222cac22134..0000000000000 --- a/simulator/learned_model/src/sim_pymodel_steer_vel.cpp +++ /dev/null @@ -1 +0,0 @@ -#include "../include/sim_pymodel_steer_vel.hpp" \ No newline at end of file From 814ae25f6dab1694553dd3b0672b6ff2a285e265 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Thu, 18 Jan 2024 18:29:45 +0100 Subject: [PATCH 10/97] Update CMakeList --- simulator/learned_model/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/simulator/learned_model/CMakeLists.txt b/simulator/learned_model/CMakeLists.txt index be3e67d0cd114..ecee90775790a 100644 --- a/simulator/learned_model/CMakeLists.txt +++ b/simulator/learned_model/CMakeLists.txt @@ -8,11 +8,13 @@ find_package(Python3 COMPONENTS Interpreter Development) find_package(pybind11 CONFIG) ament_auto_add_library(${PROJECT_NAME} SHARED - src/sim_pymodel_steer_vel.cpp + src/pymodel_interconnected_model.cpp ) target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) target_include_directories(${PROJECT_NAME} PRIVATE ${Python3_INCLUDE_DIRS}) +target_compile_options(${PROJECT_NAME} PRIVATE -fvisibility=hidden) + install( DIRECTORY include/ DESTINATION include/${PROJECT_NAME} From 0f4715c54e08d44e0e69585b2d10d5c99985cbae Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Thu, 18 Jan 2024 18:30:18 +0100 Subject: [PATCH 11/97] Add python model to simulation models --- .../vehicle_model/sim_model_pymodels.hpp | 47 ++--- .../vehicle_model/sim_model_pymodels.cpp | 161 ++++++------------ 2 files changed, 67 insertions(+), 141 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index 3267221ac88d2..affdad9bf24cf 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -16,6 +16,7 @@ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "pymodel_interconnected_model.hpp" #include #include @@ -60,25 +61,23 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ ~SimModelPymodels() = default; private: - const double MIN_TIME_CONSTANT; //!< @brief minimum time constant --> TODO what is this? + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant /* Specify string names for states and inputs. So we can automatically map states and - inputs of this model to states and inputs of the python submodels. - In order not to compare strings each iteration, we compute mappings between the states - and inputs of this model and python submodels. + inputs of this model to states and inputs of the python model. */ - // const char* STATE_NAMES[5] = {"POS_X", "POS_Y", "YAW", "VX", "STEER"}; - // const char* INPUT_NAMES[2] = {"VX_DES", "STEER_DES"}; - /* - Index means state or input in this model. First 5 indexes are states, last 2 indexes are inputs. - ---------------------------- { STATES }{INPUTS} */ - // int MAP_TO_PYSTEER_INPUT[7] = {-1, -1, -1, -1, -1, -1, -1}; // <<- Value means index of the input in python steer model. -1 means not used. - /* - Index means state in this model. - ------------------------------- { STATES }*/ - // int MAP_FROM_PYSTEER_OUTPUT[5] = {-1, -1, -1, -1, -1}; // <<- Value means index of the output in python steer model. -1 means not used. + std::vector state_names = {(char*)"POS_X", + (char*)"POS_Y", + (char*)"YAW", + (char*)"VX", + (char*)"STEER" + }; + + std::vector input_names = {(char*)"VX_DES", + (char*)"STEER_DES" + }; enum IDX { X = 0, @@ -92,6 +91,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ STEER_DES, }; + InterconnectedModel vehicle; // py::scoped_interpreter guard{}; @@ -112,26 +112,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ const double steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics const double steer_dead_band_; //!< @brief dead band for steering angle [rad] - - - //PyObject *steer_base_model; - - // int num_states_steering; - // int num_inputs_steering; - - // Create vectors to store system input and state (C++) - // Eigen::VectorXd state_steering; - // Eigen::VectorXd input_steering; - - // Create vectors to store system input and state (python) - // PyObject *py_state_steering, *py_input_steering; - - // std::vector state_steering; - // std::vector input_steering; - - // py::object steering_module_class; - /** * @brief set queue buffer for input command * @param [in] dt delta time diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index d7f4f3635850b..860f2863a3b85 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -15,13 +15,8 @@ #include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" #include -// #include -// #include -// #include -// #include -// namespace py = pybind11; -#include "sim_pymodel_steer_vel.hpp" +#include "pymodel_interconnected_model.hpp" SimModelPymodels::SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, @@ -41,77 +36,52 @@ SimModelPymodels::SimModelPymodels( steer_dead_band_(steer_dead_band) { - // Initialize python library - // dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. - /* - More about the line above here: - https://stackoverflow.com/questions/60719987/embedding-python-which-uses-numpy-in-c-doesnt-work-in-library-dynamically-loa - https://mail.python.org/pipermail/new-bugs-announce/2008-November/003322.html - https://stackoverflow.com/questions/67891197/ctypes-cpython-39-x86-64-linux-gnu-so-undefined-symbol-pyfloat-type-in-embedd - https://man7.org/linux/man-pages/man3/dlopen.3.html - */ - // start the interpreter and keep it alive + // TODO this should be in config file not hardcoded here + // Think of a way how to differentiate between "simple" model and "base + error" model + std::vector> model_desc = { + { + (char*)"control_analysis_pipeline.model.base_model.kinematic_bicycle_steer_vel", + (char*)nullptr, + (char*)"KinematicBicycleSteerVel" + }, + // { + // (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", + // (char*)"$HOME/f1tenth/ws_testing/base_model_save", + // (char*)"SimpleSteeringHyst" + // }, + { + (char*)"control_analysis_pipeline.model.base_model.base_model_simple_velocity", + (char*)nullptr, + (char*)"SimpleVelocity" + } + }; - // char *model_save_path = (char*)"/home/tomas/f1tenth/ws_testing/base_model_save"; - - // // Import python module - // py::module_ steering_model_module = py::module_::import("control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis"); - - // steering_module_class = steering_model_module.attr("SimpleSteeringHyst")(nullptr); - - - // // TODO call function to get model signal names - // // From the names create a mapping table for correctly pass arguments to the python models - // py::object sig_state_names = steering_module_class.attr("get_sig_state_names")(); - - // char *my_result; - // PyObject *temp_bytes = PyUnicode_AsEncodedString(PyList_GetItem(sig_state_names.ptr(), 0), "UTF-8", "strict"); - // my_result = PyBytes_AS_STRING(temp_bytes); - // my_result = strdup(my_result); - // Py_DECREF(temp_bytes); - - // // wcstombs(buffer, PyList_GetItem(py_sig_state_names, 0), sizeof(buffer)); - - // // int len_ = PyList_Size(py_sig_state_names); - // std::cout << "STATE NAMES: -" << my_result << "-" << std::endl; - // // std::cout << "STEER: " << len_ << std::endl; - // // std::cout << "STEER: " << strcmp(my_result, "STEER") << std::endl; - // // std::cout << "STEER: " << strcmp(my_result, STATE_NAMES[4]) << std::endl; - // // std::cout << "STEERR: " << strcmp(my_result, "STEERR") << std::endl; - - - // // Load model params - // py::object load_params_succ = steering_module_class.attr("load_params")(model_save_path); - - - // // Reset the python model after loading the parameter file - // steering_module_class.attr("reset")(); + vehicle.addSubmodel(model_desc[0]); + vehicle.addSubmodel(model_desc[1]); - // // Set number model states and inputs - // num_states_steering = 1; - // num_inputs_steering = 1; - // // Initialize state and input vectors - // for (int i = 0; i < num_inputs_steering; i++){ - // input_steering.push_back(0.0); - // } - // for (int i = 0; i < num_states_steering; i++){ - // state_steering.push_back(0.0); - // } + std::vector> error_model_desc = { + { + (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", + (char*)"$HOME/f1tenth/ws_testing/base_model_save", + (char*)"SimpleSteeringHyst" + }, + { + (char*)"control_analysis_pipeline.model.error_model.error_debug_model", + (char*)nullptr, + (char*)"ErrorDebugModel" + }, + }; + + // vehicle.AddSubmodel(model_desc[1]); + vehicle.addSubmodelBaseError(error_model_desc[0], error_model_desc[1]); - // std::vector state_steering(num_states_steering, 0); - // std::vector input_steering(num_inputs_steering, 0); - - // std::cout << action_delayed[0]<< std::endl; - - SimPymodelSteerVel test_; - std::cout << "-----> " << test_.test_variable << std::endl; - // py::print("Testing py::print"); // use the Python API + vehicle.generateConnections(input_names, state_names); std::cout << dt << std::endl; std::cout << "OKAAAAAYYYYYYY" << std::endl; @@ -151,45 +121,19 @@ double SimModelPymodels::getSteer() } void SimModelPymodels::update(const double & dt) { - Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); - //std::cout << dt << std::endl; - - // Model the delay of speed - vx_input_queue_.push_back(input_(IDX_U::VX_DES)); - delayed_input(IDX_U::VX_DES) = vx_input_queue_.front(); - vx_input_queue_.pop_front(); - - // Simulate the system (car kinematics and dynamics) - // do not use deadzone_delta_steer (Steer IF does not exist in this model) - updateRungeKutta(dt, delayed_input); + // Eigen::VectorXd to std::vector for model input + std::vector vehicle_input_(input_.data(), input_.data() + input_.size()); - // Simulate the steering system using python model + // Eigen::VectorXd to std::vector for model state + std::vector new_state(state_.data(), state_.data() + state_.size()); + // set model state + vehicle.initState(new_state); - // TODO: define inputs to the model in a more general way - /* - Following inputs to the steering system can be used: + // model forward + std::vector vehicle_state_ = vehicle.updatePymodel(vehicle_input_); - - IDX_U::STEER_DES - - IDX::VX - */ - // input_steering[0] = input_(IDX_U::STEER_DES); - - - // // Call forward() of the python model - // py::tuple res = steering_module_class.attr("forward")(state_steering, input_steering); - - // std::vector next_state = res[0].cast>>()[0]; - // state_steering[0] = next_state[0]; - // std::vector action_delayed = res[1].cast>>()[0]; - // std::cout << next_state[0]<< std::endl; - - // TODO: define outputs of the model in a more general way - /* - Following outputs from the steering system can be used: - - IDX::STEER - */ - // state_(IDX::STEER) = next_state[0]; - state_(IDX::STEER) = 0; + // std::vector to Eigen::VectorXd + for (size_t i = 0; i < vehicle_state_.size(); i++) state_[i] = vehicle_state_[i]; // Calculate current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; @@ -199,6 +143,7 @@ void SimModelPymodels::update(const double & dt) Eigen::VectorXd SimModelPymodels::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { + // Not used for this model -> we can get rid of this later auto sat = [](double val, double u, double l) { return std::max(std::min(val, u), l); }; const double vx = sat(state(IDX::VX), vx_lim_, -vx_lim_); @@ -210,10 +155,10 @@ Eigen::VectorXd SimModelPymodels::calcModel( Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); - d_state(IDX::X) = vx * cos(yaw); - d_state(IDX::Y) = vx * sin(yaw); - d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; - d_state(IDX::VX) = vx_rate; + d_state(IDX::X) = 0 * vx * cos(yaw); + d_state(IDX::Y) = 0 * vx * sin(yaw); + d_state(IDX::YAW) = 0 * vx * std::tan(steer) / wheelbase_; + d_state(IDX::VX) = 0 * vx_rate; d_state(IDX::STEER) = 0.0; //Use python models to update steer return d_state; From aec7d7cd018b4ad14cbe0ea813ab33fc47fb2050 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Thu, 18 Jan 2024 18:51:14 +0100 Subject: [PATCH 12/97] Fix path to the python model parameters --- .../vehicle_model/sim_model_pymodels.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 860f2863a3b85..2fa8eb5766102 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -65,7 +65,7 @@ SimModelPymodels::SimModelPymodels( std::vector> error_model_desc = { { (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", - (char*)"$HOME/f1tenth/ws_testing/base_model_save", + (char*)"$HOME/autoware_model_params/base_model_save", (char*)"SimpleSteeringHyst" }, { From 903c9b5ce4149d1e3b2bef028e9ca38896d70e46 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 23 Jan 2024 11:29:16 +0100 Subject: [PATCH 13/97] Fix minor formatting --- .../include/pymodel_interconnected_model.hpp | 32 +++++++------------ 1 file changed, 12 insertions(+), 20 deletions(-) diff --git a/simulator/learned_model/include/pymodel_interconnected_model.hpp b/simulator/learned_model/include/pymodel_interconnected_model.hpp index 0dfbc23df70e7..ac24181787802 100644 --- a/simulator/learned_model/include/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/pymodel_interconnected_model.hpp @@ -9,11 +9,10 @@ #include #include #include - #include namespace py = pybind11; -// sim_pymodel_steer_vel + class InterconnectedModel { int num_signals; @@ -23,12 +22,11 @@ class InterconnectedModel std::vector> submodels; - std::vector - map_in_to_sig_vec; // index in "map_in_to_pyin" is index in "py_inputs" and value in - // "map_in_to_pyin" is index in "all_variables_names" - std::vector - map_sig_vec_to_out; // index in "map_pyout_to_out" is index in "pymodel_outputs" and value in - // "map_pyout_to_out" is index in "all_variables_names" + // index in "map_in_to_sig_vec" is index in "py_inputs" and value in "map_in_to_sig_vec" is index in "all_variables_names" + std::vector map_in_to_sig_vec; + + // index in "map_sig_vec_to_out" is index in "pymodel_outputs" and value in "map_sig_vec_to_out" is index in "all_variables_names" + std::vector map_sig_vec_to_out; public: py::scoped_interpreter guard{}; // start the interpreter and keep it alive @@ -39,9 +37,8 @@ class InterconnectedModel InterconnectedModel() { // Initialize python library - dlopen( - "libpython3.10.so", - RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. + // Manually load libpython3.10.so as we need it for python.h. + dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); /* More about the line above here: https://stackoverflow.com/questions/60719987/embedding-python-which-uses-numpy-in-c-doesnt-work-in-library-dynamically-loa @@ -127,8 +124,8 @@ class InterconnectedModel void addSubmodel(std::tuple submodel_desc) { const auto [lib_path, param_path, class_name] = submodel_desc; - submodels.push_back(std::unique_ptr( - new PymodelSimpleModel(lib_path, param_path, class_name))); + auto new_model = new PymodelSimpleModel(lib_path, param_path, class_name); + submodels.push_back(std::unique_ptr(new_model)); } /** @@ -139,9 +136,6 @@ class InterconnectedModel void addSubmodelBaseError( std::tuple base_desc, std::tuple error_desc) { - // SimPymodelBaseError pymodel(base_desc, error_desc); - // submodels.push_back(pymodel); - std::cout << "HAHA111" << std::endl; submodels.push_back( std::unique_ptr(new SimPymodelBaseError(base_desc, error_desc))); } @@ -155,16 +149,14 @@ class InterconnectedModel bool state_changed_externally = false; for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { - if ( - abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > - 1e-6) { + if (abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > 1e-6) { state_changed_externally = true; break; } } if (state_changed_externally) { - std::cout << "Reseting model... ----***----" << std::endl; + std::cout << "Reseting model" << std::endl; std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); From 0d42c69d0ff597511c9dcbb1ff3cc4c774ede812 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 23 Jan 2024 15:40:08 +0100 Subject: [PATCH 14/97] Add README (WIP) --- simulator/learned_model/README.md | 84 +++++++++++++++++- .../image/python_model_interface.png | Bin 0 -> 88467 bytes 2 files changed, 83 insertions(+), 1 deletion(-) create mode 100644 simulator/learned_model/image/python_model_interface.png diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md index d490d175585e6..16c2336a5f05c 100644 --- a/simulator/learned_model/README.md +++ b/simulator/learned_model/README.md @@ -1 +1,83 @@ -# learned_model +# Learned Model + +This is the design document for the Python learned model used in the `simple_planning_simulator` package. + +## Purpose / Use cases + + + + +This library creates an interface between models in Python and PSIM (C++). It is used to quickly deploy learned Python models in PSIM without a need for complex C++ implementation. + +## Design + + + + +Using this Python model interface a PSIM model consisting of sub-models implemented in Python can be created. Each sub-model has string names for all of its inputs/outputs which are used to automatically create model interconnections (see image below). + +![pymodel_interface](./image/python_model_interface.png "PyModel interface") + +## Assumptions / Known limits + + + +To use this package `python3` and `pybind11` need to be installed. The only assumption on Python sub-models is their interface (see below). + +```python +class CustomPythonSubmodel: + + def forward(self, action, state): # Required + """ + Calculate forward pass through the model. + Currently implemented in the way that the next_state + needs to be 2d array with size (1 x NUM_OF_STATES). + """ + return list(list()) # next state + + def get_sig_state_names(self): # Required + """ + Return list of string names of the model states (outputs). + """ + return list() + + def get_sig_action_names(self): # Required + """ + Return list of string names of the model actions (inputs). + """ + return list() + + def reset(self): # Required + """ + Reset model. This function is called after load_params(). + """ + pass + + def load_params(self, path): # Optional + """ + Load parameters of the model. + Inputs: + - path: Path to a parameter file to load by the model. + """ + pass +``` + +## API + + + + +### Parameter description + + + +## References / External links + + + +## Related issues + + diff --git a/simulator/learned_model/image/python_model_interface.png b/simulator/learned_model/image/python_model_interface.png new file mode 100644 index 0000000000000000000000000000000000000000..28b217801d57b9c3a020d7bf5444b4e42e641c22 GIT binary patch literal 88467 zcmeEu2Rzp8{y(~jq>Q47ZaZb~kr^Qwk(Is4-YaEiq_jwgic(}Gd)F<>NU}#(C@T^v zqyOh?l*Z|K&N^girI3k97gbWKy-P1)z%f;5i+{(rbi;?HR%5RL^oR;>^E{r_VjNIHt4i4<5 zRz?<1Mz+rEc4jVc2tK#9H?=aiGBaH{hntg|myMH)jhp8v7Y`$kB%Hz{$;ZRa&&zde z<$NOxGrQFrD%yKl+1MB{av$Vl=Y(7B(cH)_*1o;j4 zc~_1(xxsC@xVhLlS>O{{GZSljxC=KY7bpC1!J&gzPG;uzaM{7-vsNB$=3->AGE7NM zf#Yn}(ua*)?M!SXxp;VvxE@_O$IZ;i*~;E-^=-J>x!Aeke#ncucsiJ^9Ja8xx3Doo zj`GMba!SHy$Pdqf)yaWB?2K$z&sBlA}+}>vO^;XYu zfH(iVhi{;8uLB7@DA!o~(Sy@=FJsH+^Wt6r?YbUQBaJDou zwRc~+e)-V)Wm%cXwdr)Sw}<_v<9fn%Uc$xj1>kZPzYeTjEulX5->SPDdW>{?$adI0aS@TCTGXUjEgC zMyqyd@#Xf&^}lEB;GET^|Me>%(eXPA`T4S}y69hhS$H^CUu11rgam(F6@HA?KW>`YgGl{T?4a(1>d`PO5;b+3QuEM}(P1jS$5 z*-u?%og=J;%JpurcFf7l#>mCW?b}%UvsL&#L-;xn6?-el-d`M+o9{cz5ai@z7g)_h zYfp2wcXcu`TfOr4`Qpk0|KVeQFvy=h*2T!l!p!Agcr4F>KW1=Z5f?HQd$L{3f*W+W_8@ zGPQDJiMwcM{rncjPH?m35&xr`BB5*@zyc72jA|WJ9@N3Dlere@|w zuFDf-WMgH46dfi263qa0Ng=4p3c#}D>Ib$~rlts#`zdn!oyq5qL~1jP?60*V)C$1q!Es!*W%W)eN-$EvrTDp#5*%L0MH^M(L>J zQF&#hAMf+G@qfi0UvR(|)bo>w{+G7?HR9mc=C9(p7P{q;7-f#sj^G0TmY_D2`iBAh z-;a{31?XQbMDnc_L|>|=wX*fM;v?^B%v{4H>%kCWm%q(IECbj-5R!5W{(wo5to`l! z|Gm;{D-9gv>Jt9eQuF-jmYN5a5n0J~!9dN$$;!?Gjv=%rlK9Ml7~YgZf?eJ zf|TWdwMfCm{T-MRSVK+g&1cP2)*~qa&M%IXVSEv6H73z++B! zUMQvzHU`Nf4RzW96UUxboe2UOPC%^D%J-fY&6{dY(r zHo>*Tw-z4-IKgiJn>9F_;F?E$83NxghwzuJ!FhfxW&T0+OylyMesJ^Os)^V8*FW;ezgjdGT!U|4Ol{5f)_En@UniRX2>q7v zznu}HkgetMRgn^Nl{MxR90b?WQWl*qeH>VI_wAKvi*bu9}#U@tUH!3Y~w*$p1pVT-6_WzIR!)f8$x- z-*nb!)uSR&|4ID5%ImMM`~E9b|9>f|^RBwVs*Q39v9I?LAzq%9 z7_u$_t%3ISDSr(btmT0Jdo6(LXGs3HjZ#_t=pSvBUybx1+4bKRxxT%U`@69DS2W3= zIxp8M!}_cI{^u($1=iB&x)AWw%98*4(6JIZ{zM_+XY>Cd$@3 zeE$La`@ZPo=4AgOCL+J^t!Bk_MaZAdAb(!;{SM&ye_r(Quc?BoIg(9~7Zk-GTVK}A z{!dH5wcD(r!2f$1Xf2R^Q*-|i4>ZgytxQ0${a>j1*6i)?st8(Ir#gk zzF!*7f538otonXAg1@fmu1ZATnC=fL_6NxQhgN;dn=F4DbpLErs{-QBF#CUFf7$I$;vc9c{=FX)HX-~^`3v`VUs;JyY;`tfv@^8VHb(?AU`Qh7ozS&H}FSvH#A8e*s->vcc_RTCm@B8gHE12wu)?N6P zt>tg+_{E+WuGIa|!&G+x^0czH@}1en;1DdKLFFzW5hd z?sWlmtygw^`_kI6|8W28S0`fUTs7J?o95@|Wfu_oYSiD^^nbvs)*N?j1OgyS{bUQr zU&*U}x~-At57$%IrKUgOKF{wQ?XT`Eg5Q5XfdJPpw?zK&o+2ppZB@f2Ao!hk{)sZq z^7nlGgRhTV`^K=Z-_*I@IF>(M`)<&c4^}ULECOHYgM1rhiq_^gSXgK*d1=X`9{OYP z1Pj|=eE!@Mw(SC=>htIuA@~M5ff;;ZW3-*oa=ByoFR^*I+@HxlC9i#dTl;+u0|wL3 zYJIu;44ayJrswL1PZf3BPQ=HEI1jthY(3W$FXUW5T^lDf%=gxegeUfM>#4nTXq=z? z5G&w>qxBcmjoStJ4xywae)vImqsX;QNiKSvpYWC_p}-~~{_e+5i!9QEWVG+dz)!B0 zKr!gQruqJoUGM8bm!A|?K9CEf}ZITBC$5t)lNE72}1QJ94ZohU49@xsG?_(8+mE6x@#3!S#?_&*gd8 z$nz&(%(#yPERRN~nRV5&Lm7QNrrJ!-wWl1psk@s=`P!It$aYQL=fPWd+l6epY93kN^f446!BR$l(QGSBDH>5TwuMs^~9$)x9^!$oi)oos1+k%e{D1&3_rwo z`A1?ufn0IOaDIyV>Tp%maoYQFBMJu2ii|xZ5A+|7kR1{Nr#!#%Swor@eg*TV5Pn7KT))G91QUK`eMI&(2m^bmr2` zNOO(Yr`Pm4Hd&P;BNF?q+A^<`Nj_!MakTc2$an=>?VJkO?|Rxot6{+)47vqtvK zb7CE#+wDs~-pa9h@%SaoY7mByl`Z7@RG0I!g3%9S?Jq)i>fY44K90$F-CguS>h#b& zkvn;p9`Ub-RXxamA>+uYg{jk`bG-p>3sY4hd8DW_y8V^pIyQZn(-zk}9~Zign%|Cd z3L|583Z=jlqHp0E6AZq8p`2%MYSERphOkR6vkU$DmgZ9uj`aNgP4vEdrg(oj7ke1= zd!_DA6pnU$=s8s;5xj-lFA_Hp>q-G8oJ@KD^U4g5{!nI{PTRbkXJYfUcw1@W)QWeA zdOdiKr>nMSfm-vj{6(f_?SRR*_)Ml^1>?D$aWz%%-@F(tYYE^f2QuCs}EZE;Me#z&Dc zD?JdMZF`i=-ffZK;}k}bZ;CcNLnTn(k!Szb-;wkYUGyMsyJEr>2C-|%DF5rXf_FPGk`4VYPk)U7VveTL}5M&z` z_@Sk`2)BQk3J+3&g>@cS_8IF1sy{xr+n&lMQ&Our_3I^AFe-ek8+U)2P4ST_nrypk zzV?LmS;Fss^U@(ort*NBbl<=lc?Ql?OA8GAI<`tefG=3eu$va&;ZOEjHgcScuV0NX z%X5=LgMH5=wMt(z{R~Xq)44Zemq;6u5*qz9^JKojAaozlR({-l^AUs$!fj4j~+dBgZregNHRR|wo7lb?bnr+ z6NI6YiTSeFm14rTZ7nU-7sFQkG?@J&=Ii6oIL-8lt~N7mqfakGnmn>oJU(6(9;hZf zIg-1Uqvwyz6GzzjowxRJB6F52UKe}!gi4RfM=*$FpcwXctpPV|Dc zxkuxzSHO)!%CUFgcDC9fc;KOp`rYJTtluw?tU;K+Z^M=TA%$YOsovDlL9&J<12K_! zmamVaQxS!=(KuI+~qfhBe9B=%v)X-54dkICkl zcd*JF3GwxiNbwWOb6WMSt7yE_nllV~^wN&pJlIm;Y(XXLl9FT7&TYQ|;y1fUukQw% zwoIeko|E%z4qYAA8Mb9hpHh7QMwAL?E`1v9jCX%&6_=1PRysf7R!w_a>%&Ahh2X2} zr)lmx*9%p>JTYD8-&#CBQDpnF*s~?uLfOD}AWqM%y>ezW#^TzPYx)-C)%}u4;id1; z-*!_dZV*M|m=VJ(;F zOxTJ{<75DO9j-0GI7GRS3zT6)>_1xRi`TE%oYYxDR!#0=@lKOZ4pWgyF@tGgb%N>l; z>6mFXSjJJ8jXkGcdcWQI@TvIH)K>42JGXUrzkU`cc@+U0 zJ1=YMTh%r{=<5&XaQpns)$OIH#fMIq>o=b`&7N-SJUKr;-gkO;u626Bz!=K4Nf?U^aYaX?q>OzzWoz=blLb+m|&V}P`6*)Rf4$D zd|HA?jt=HYeivVkHmdOeTBmq!VfxuU%?$mPd`A=JW4qkD9CI5F?Q7p@5oh$`mSQ$v zi`FB8O#OB8a5^Xap@LDnq#F-t?`k8kuE`El05vee~9q)FZ{;c4` z*_Yn-%zL|BUMS{{rWbX14#!&zC!8uR0jrwuO3f%O%*Jbn#IEbJ`{dkpGNMDc*qCBZ3Hj=4NBpHG6ER2wcQ3H1cY0opWl@2)HLuJ z>^LVEP<{UGu{Q;sO=^fOL+s(VqQ5okh18Pz#D3iP;B_0WDv@B5gq*?Z$E8}pBcjZ= zHfG6!dPC&7(PecF$JmpBK@h9Mjzto=iwK$p$b;LL@CX1`g6kPg(|mB zTY(Sm{nGDWKYygHs{&R-qq!9aFA5)ERNB0I8Xd8ezR!&MhuyBuY@5ip-aD3afA^_{ z=LL4#!ae;4ap^YsW0TY7pJ>2Kljg1@9FYo?xN@C%>i|?0d5!~*g{F*C@8vo@L@}xr zmq!HV_8#LgdAymzdc!D5{pZ%Qr9sTrPl}%4`L@mKnyyu^iZo0E?h%F};YJyv(I?cc zMTci=`q}jeXJjV`ZP0>ZJlNu;>E{ z;=`BPY%7iSBk9MD+QF#}52dj5zHXiTHeKx;q_hiCkPed0T9_!ORpd|LubRK4{+q{R2!(m)-vV22)B`zJV9olAshmB*l+r6i7 zjK*fn@t#C+`MZZTU4D!Q!1~9>^LpGSs0XUfx$jio(e!3(hF&Gi!x2ie z{KU&zM~L11VP+=nqxIZk6cfa{3GncVuX*^H;nG!JR2H*>Ld`kWt4oY{a|oj&p!EVR zG73GH^D}RIdbDL(OWrrKn-IKFcKnPce+&@(l`iL@2+t#O#dCwv23<3map=h5D_kaN zQ2PX5@vEuz#on+fj6y5Okk&s4?~a<4I6DLWcFYHWskT+!L02f=$Fs73Out-Oq$CO{ zL2eTgkhxWlTku1ID`b{V+>C$^-&<<%j&#Ud`0+`Fle29m+0aL;Q+!3#JGZm=qI$p; z(wKf+E?#2BkZ?)K2Zm&2uWy8R0Or;!CSs+5GKQF`g-S~djTPrLa%UF8s|f9e@GY!6 z_HO40$&#c(sz-`g|6D_YcToQq@b`X_S~w>aMIpZUUM-$#lN}wZKS``!rdn|Dl3E<+ z!-Z#`y|G1q@7&WxhGRLhhKx_PTO$~9tc6-)N|ZNn_s0*B zxIjRhMAqk@cnMRi0zn7KZ(CtHU+}c#w5WwrS4Q&lqXu6}Uum=`f-s+HwH{Mu)NkqtxhsAivh{8Pb3JL4bRr)9 zE}%&cQZ_OvUfr?taJ8z`)8}{8ZXYXh8y_p4uFp=_D~jq?8@Ya#$}a!!Ku-NoC_iErnhO6Zef>qIM$dGH~TJ^61zrg9*{7{9t9Icpl=LDL6mF1Pb;F&#n zo8p;~qY39gu;iZp@5gEiWGrvV8kL zJ((!NgqxmAIA*{I6O@Y5kKPVzyBV96tzP`|2?R#y#(K|=y}_ITE^PbwBgh`MTBgKg z^hK}cBMR+{#`!xTe}%VXY(Ul%U58A|^lRp#x1<~wL{A>~xr{6~sfV5he8`0gag{tA z4t~&`^YI%tfa&lRLwJ^jUJoz=Zr!e$qc3is8wUs!YLpa=lo=9y2xcZ4DKsaB6o`^M z;+k|8=A?>-izh0pWhq@ESbY)@;IO8<2+Z@E|ZH)icXRuB~D=QoxpclLP?%G z4i+LKJEWmo*2UI$ z@<=(3Tk$qWSHt_9+nmD=Wq70t&P8VBTmW&;#g^s<3TY-kBu5>43Ypln?kb-%iyA@y z$6B6eZI7w!b=`G^q9{VBcb}N_BNJU^I^F>+*Kn6rg)&%fr20;`I^gZHyK}CN;HH7Z zmOZ5<=h<_>KGG{uV;CRPrj#QGC6tYi#lb|i|@k+G_miq5MA^N;+VQKL zA`)${BK2n-k=1cwhABF}1NYA2$-_yJyz0@&>#zPu90jL|2z!uD#tVr_l1$y_4jL-2 z?T^xwzE;>0bx8d=6wJ#+jyew|SjXBhgu0$N4I(+v7a<|)n3(NGU=y>M$}JoOi(oh)e?Seq2N zQDI5MNO_MmM{fd)QRAu|Nm`rEzs+jJvi>$J#JK()v#JBp_xea=D`DTs4*lvKdC@+p11f!oDm$T}N(8c}#s5 zH^ea74H`2!5b1!L>nP4{-bbPp={qFcO@agt6>oT})|XdqCCid>G;%kNSR^jr6#sp& zsu>-O|LQ0*{wUJ99KuE;=L#J_TO1PS8k*v)h*n2H=MiB(3pkXHWI*m z=S42188aaKXMvnG4^4RvKnl#!G#>!|{-DHT^#GXTN+auD#=v#0q#>KEH%>xKA zVC-JHryOI(sUU99rPwUnyv(XP(j^v%D&4%EI|sGT!>w%8`*1$f11jzhqQN7P+kjo? zYPG{`<_P3ETX?VAwixT%WdIc{Seq~6udG1d_$`G{iDp=VR7x*BEhJh?(Zfx7AKVo* zh1HV6OB=Di=*#2-bL?r6s!5lLhPxKhBPlR(7T?k?ql8OQ;T*(Qv!v!{5-T@-O_cmp z8phAtS_$K4UAA;z-=Wz}y>FWC0vUfqeWB1pb@|-$P2QU0l4x&*hq5#l0o7)}j&Q=Xzdg;WLo zGR1FIa+Fa1w21}mJI>yfLEfuh8YVXG-~rnysM>>e9~0IcbOfvMuv0Ob)bB1ygRUmu`@&^24?GAD zu(VWntcH~f+^p5y$^;Gs^F3X!ks)m;2~Tj5MbfIoIM}1Ux5?Eg8pv@qa;XV^D>rSR zI&?V{-o-LK9C?@JH^j!S8%Kyab%0Gb0Dw&acbK$0!egr8z}9x5;W!gSL_^fqErPvR zR04K!z`A+4=G7B0#~C8nn&;X67waYCi4VXmY7f`P+rDnN-g4he0nsJLKsL@jwJ@;T zb+`v8O#Tyd4x>F~Wwu4*xf4CZfYPUjpJk*D!y|L3?2DhXm(Cd%y}1=@^Z3lp+?He3 z+HYT$j3)+c*|f-ia<1?6IB0k|%3>ce8v34Qqa8V^dLDg28X5YcK<<1PJ1a7gslZ;q zkm*)#`|16g+*cPA@H8wUx8DP%)eS`D4nWHBc^weZy0-|uA|8EN;;kKB3QX7-B5gul z)Y>hgSLl*8{Y<><>=R(}-cD2nY!%B-UwU$px$~-Zx)mb-6}9f^l64-g&jHc6X|V)m zVzy%A?zT<)(hS8HCIh&lRd%$#P!{h#E7*@;)ay^!a@Kuu-B{kT#UcvLoq93}5Bbmptwulz#wo;2a{gsJNc+n4^D z-zNwY4$RU038nnS+0O1E!Ra1iA_ zQ6Pt-Y@sn!7Jp}m1ra50rg48z>)!bG?lK3)W&dbthA1&_1N+Zowl|ZFP z7fWzzna7xo-LGL!Eiysqz*v6UtJKc5TxMM|$?^WU zU8+pZUai2>r9Sf=c|9jbQTY75h+L0!H4um*Rq4G4jfdP2VziFi3@TneOw7%*!cSkG zoVU$uUxMj9lQE|ijlQ&hrb*3CZF9}3#ZiNaQL!i@j?%dswpo?r2%9(Bnqd$>6V}>N z^5JEt_G9#(EO}YWk0YrWag24)#Be;X+buSgVqYNev26%L<4sIF+jHuaCeg#qEHz-rO6lLw{=StY$q+?$)*l}5#7=O0QS-FZ`Zt7uCa23V#dy#`Ov{S}U zFR=s%JysT9G>xDVa8swZM3~ZU!ZSe$Hwx;P(*cB#CuoT0I|q_9*jFXuWu9*&gyMve z*xTaiuE!+qp+Hq`e^A_;>JD{O7n9wq>hs$}DWK&d0cpdqBda^;04nkwo@R~#^1UCo z@qe;T%L*PZdRwZ*ib%{|(Wf#@kLjQIy!b)MLt;;_AO^)M?I0pFaE52|^9|uR{Q3A) zCnBnnv9b(X_1s<4aWnhHm5UP-LYPlH*)8ahC_059n}wDOL2|{byjkE^O`%K=i2NQo zmps|Vk*VteTE;l=Lhc%S^YhVIy>5Q7xKg+pIG2D^sbErydp&a99-qx-#If~y#2N7k zDlN%L9hkv=`i9y8d-%+2uLL~~>6s|`pzr8-W5@4sPuyr!mJBQa zDIlJTPzo#7gB6{rTfL3Lu3(USm?ofHLNdv+`R?WH3+0FWdW2m^UY&e&1Lr(WG!B2E zmVlw71Wbp+3(0NQ&xP!^$#k^sy#h=BU_wERi>fzHp}4mv^U_DK{Chw_ri-P^dy_~J zSlB zW5%xuN5q{1$KLb<2_c>Sf*PztGSC>&u>j3Ng61I_nDBOwfr#!!|DZEHU}+p&)KHG2 zai*BT(w0n;$cLbml5!8mzl2nsu9bxR7N}a&=^D++THp8>E{^-Bb5*D4vP+{4PPQuv z4Q|UTsNa&N^_Wfl9Sc657ENYMRVA%jF`9it3z-&!DLE?*F;1q_q;b%shz4;}P$Z{S zBLju>1)y?!nJUcV{kLzlqsk9+U^NcZY(pEkJeLuwB&?Twk9J_}52E;tofUAh!5l-# zKc(L*pT?6Ze7z16Q*FB0IqhI(@RMc(43%`S69hPxer3bONIeJ1hwpH(+3=?xhI^j( zi9b{sUi-;mcg1nseR0rIRZXCOGHgg#{DDi_asUf}0A_mT;;{lwB!KN(4Pg89IZbW! zNw#u^TbN9kQL!-S6X#!y*pI!t#J_XHtxqQ^J{(H-jYl2C#QW|b)29{N&`uk2gjk~@ z{)W%|YES-8xbx=b7 zzFTI+>>>6ke~wnvX@*S3SXqBr8lU&i*)I~r)@5#rV$8qYz|?;fEk~Soac(zevHYF? zgDV-u@4}o}J`Rh2e#2jq?`^1F@%CELBVZRt{P{t9Xikn~T&mR2u^rR_|5fHxl%`K( zBR&~o;HRTd(2vU^n|0>8PY46V+??#Ey_7Z$1Y_->nYoKLb~NJgPk;QRlkQO==)^%1 zfWw6!PU=AtyF}eXVWd55__6dZ;y@-$)FBLEAj7Ai%v>OB^G6+@(M6HS zHDxRHQ9_H^Ljv1!ubURnt|^&5B*iU7`CJ~9(NexU0^H{ETnY0FnIuOY0@@3WvmLPi z?yXZesT=r@Cx=m(Vhjhub+EJ*@KH}RT{yH%p{HvHg}%d~H1g`+3cQ#)UMdVHp}}*4 zs8C!CD54X_6b(`QwjKGb)HazAd-Yl~5E<{z?Q6%O*(s+JYk=Dq7kg@Zb=X4+A8DMk zv|{*`8H2bFv3Dwk?2|leN*hmtdc=rKS>lHYozOpf} zmKDWh+mv!X`;%cUi8HrCu1AG^2y4~uQ^jP%4gwpE9o*gw}Dj=pjhX~6NDL*UeB zE>%KjvdF=piOrx@N!(NZH21{)O=G1kO%{8#`tf+lop~~Mm1s-x9Ctg&SNNbPHt~Pb z_o_cyk!J{3c2gWW*kivx3`DE3m%y>nLABC&B%(Co9Png zkzRg9CfDIJ9G%}Y&zm+JSDup3*HUY^nuw{+?2Tf0QzUqZC# zH)$?O_)R1sFOrn8DnK0;yY!qkkzSIv#6P5B=yY!=hHJ8jWK;6_yGNn;bO^0wev0jXOa zl9_U<{G`>yJocd!Mk<=|bi}*imdx=ks(Xm&JZx@xR4hpO9>KXjJJBt2$@;D;Qt=V{ zS^B4kXX6J-!f&(2R7)#kM zA-C_)F;!_|Rj&)_=eI@2+T0H%+R2SsEQ;1AGeSuw0ntA}d78#6rG`7JUU)R$<$O%g zX3C^XuNI`rl&|e?2go{8IDO~T$H8d#o!sfApGFF;2M6Ss&Vme4xJ3Hdq-3VWnM*=$ zP`&_K5yqhzRWF6cpX7oDpFx?`h8W?t;`1|s+oBP^U+YnUO36kVkHi6G^Q~n;W9&(X z#zEN_kc)~)>`)Q~QAEqKcrlkNWxzRt1e>`aTHd5)5hE&jtRsTIquT5}I+;D_L2g&g zwS7;aoG?eaCw3a&?^er?F1W1;?Hc}N7i|L%bHALHt;KJAc|rY_=k!q2XnSg)jFvOC z4_c?*zHD)5h(bD?r(W=pS@V$<79Bqtv9SDm8sl`6AC^qcmq{$-EL|wewZkXbb6J9t z$%j8#OTVG@@biZzM?vy=BN(5-+4js76 zwY?!DHtI-I)FwAQ-Z!Y@K8?&FGzQKjock5R$GG-kddIS#xoJ>g2GG_7pP$_d-h<1%sb9-VhUb6(`GJ z`s$xz)m()7Lgao%y~tQ*fb*iHmL!A3VM-yVq(x}0$bF$CQqVIe@|o7~NSi}~TZfg> znL?<3ZII@k&kMuiN>~**CeXDToU=uDBUF2=%$a+QrVnpwhE|n=tb>!14*fk@yovf9 zeP;V&Kmn$rV0FoKx#K38^pxf%(rPjjduFG;WN)W=tnCP^)7*zW5RaX~6N+jjLQb|n zqj{*s$atn;ivklS8ahGg?|YY$;t6Xa@#T1g#13cgdt9ogzST;Cx(|xY^={AsglaR= zbf=zoSA{6UMp3} zDSt1*87hS>BM7qffto_;6+scwnw&!bE&#GNLHZW_!}y0DY`Bi&PSqj9gs*F&amrSOM@qKU#9?McHKsf4UxNvv|pjfBC>*E}AE z>RmzvuTXP*%Vze1$FHxLoOLuN%0d%c_eP~k)9OcV$EQg4nQgC%IFfFWK^lB@bno@s z&da3JH6Wd;oOPS-_5*pd4KyQZLs}s37dLtvlT}lEgjU?PO0PBq^|rd{N)=yFF0S7G z0!Pi0zx$)HzDJoi7Y8N3&1?36m`~8*(Z^p~nZ_yQC^3aK zFB&hNeMj=B(Qj_EzAhdB1*Q&#oluR;(J0!FA)^7e-z7j3B5t&; z&ecdoJszokRP>)}Xk$SkGZMZ8@4stQ6zzh;!q9vEid9 ztv>y>hCwPRuShvU{QUSNS)wSC--|qz4b`H^-%)-Q)_Pyn5n1ZNrFtgHXkwf_bnHkI zMkvX~T@JpAae~jOFS6-RHt(jO52Gk2%ffdLu01T6pY$Hu=PM7;ax!b+Y{Q^c-+J|f zyXYE0gPv`CA}QnUvQ{blM>#{l%(xd5a~6{Z4dQ-4v(CSNqb3l1a$e=NN1f}M&_=u! zDdxDg91l7L8GnK2#ZEdE&KC!dP0o2vWgTG>?4dum|2`H?7Y>j@A%%%CM?xs{H)PJ0 zoB2h!U%#MGT)yb1Xp(h>B6(KisW+bH&NLYZ??kkI02}ZJ=-#0kZ}+vRZIB`4sH*;fly}!n`t#XjOB7Y{8EBk@!FaDHIT-B9ca#pZ6Z)bGn{farp(`c@)JZ!Kga^&f3bc^2J-)+2%@t*YD-GnT8o1 z(b6y9sIXl(3qO(4<+&~eNMp;r1kF$|mV7cvB)^1gF!1bh(Rd21@CRmnha$r}zE+|Y z+>Y|-nb*K_4)n8lB&*aCa+u2u-ILYD>!NIv8=8;5Le_pq^-h4trV~@LXZ9Q==BR!s zc?NrFAax#SzoXIWIQ#He1@Vu^Xoqa@+kSv$@$4r@DLFd5y?#eYI5N?_Z?$wHKbis$ zTK~?^Uj#>kGIRDR^g9gk*M4~EouTJ5(_H@$r^4?7i@9NeYyNzj4oWk+jm`8==r$r(|%f}Ksm6A>&lW^e#QJt$ijcAS@63v|eE~kso z-+@rZ;QFd*;x16e22}l(#h%k+&`(wG&5`A3|NMrG`r8~%cBL}MQM8T%P`I$229uB8 zhZjX^+Jc^z+lK2-k|uPO-PGW#h3ZeKP+g9A2Viku zs)Pk}ax?gsLt}Mph?`eLQ`fbWK2;MQaE6BQl@{r5G@w+Q>fY9yvge-eecleaDvOEJ z7=s@xe(Gde=ap{3y75p-c0P^eO^PXJmi4R4-IBsOs0)%3&U$783@4E$a=J(qj{J%G zi)}*;U4+L?=1``dBw{9d*YTFMsUrb-IfV>3gowt|u1NdyD?sUarlpwz;tp7BQ?Knk z_E1QbzHhNh_Tk{&9g#){qb|(FF=y3r?egNgvk9?U>_YY!WNQJVKEr2rjjhs&f?&0k zJGToS9Vo=P^A}Y;AqiiLI~oNH|s0nQ^!n zA~D1JLv7FYHKfX`qd_Oy)ZqH$)rxHR(LKuxjZ+Dl(!P4RD2mHmjqutoIP!7K@Y+c| z%*Ri0!k|!(j^re*%o9t|U~@NYwSbo{4>v$IOU&RS+k}w)e+#g4kG>Nq-?0k{>{$Tl zoB2kdh{%CHpO%rABFvUgQN- zxXy>S(?fO6RtHPGwuru0fsXb8WT#RVBG*tqvbWAC$wlICF6`TCL$=o;Ac6YCWQD6M zQsp5%%JZOEb>*-$iMUU66?(xQBNL>7Nfe44H;@K8KtC}CCFDMAQEX-}o|H<3HaG3^ zi_2?aMs!x`t`J~=<92e**Y`l{2#C>@KDVqxBywP)Qf*L}4A|OHB#gRt;+;ISge}@M zBuXxKV~@3GBi-Zfr%nxC)=abQ%vCD(7|W`%fd;7b^C24nC+6SF?+;T-giT>8Wtu5S zgZzWqQ;70qgD5RX8TzR7^O83e5Z{Pwck6JBF;oi&B&Qyx&V#0v2fcFL1eE%c#ciO( zb@P#^emmh*FOjkeFdFg58PjRR%x?CyX`IykZS}9@lgI~ z=#pLnEE88g3B977P?T%sdN=}a)DBB+20WUVUeC)@pM8pku3C=;Q`zkyJVBZ;&5&D= zR`FaOI+OZk>WJn0;yLK7i?4YMdQ-ocs%cbh z4{R4K#DyJYPOqC&qxn{LH||XFm;!CC9hXAa5X8(>pXoY*mSlx!kjtQwe2Q$!%nM!y zAR>o>*VyYHzdSAJmXR(_b)dH$;W=>UtTfWhbm@UB+j1^o0)M<9WM^eJ1A3^5ERAQ9 z52$T~M(C3$^6hH3m2&3ami1KLn&e-?q?c1~5QqdBFxhwt2Rj)d(}_+keY{P!i}-LZ z>{PtL#ZeqQ>_pWku(SwG(eCMc4NiVcc8;hp#)J>|xi74k*vTrHD$sv{0uo9eQLhHy zJq?O5vROrfh!_?I50A@C4;?ZHn&zmnB$PV6g?=f%jMBM-GW@p^yAN`Ya*$_cU-3Av zP0_-DwEv6Fk7cv_S_m?^|ML{Vx&l(vy$!s_VNLO{zqrbd2N~WPJanpp_{!wu2h((H-@#Dl<3}H{Ti$DGz zt>o41;C}s3WCy6k`JR=d#Ae-t@8k(v4LV`pL$<_pXaLvAg}Gub4(_c?4#VKTb|9pl z>=rlhdXqe{a9HyJ!Y}hA>E(OJ!A50Q@e-hD>iu!@NHc)Qp8HVt*>=1$FHcQLD0w1Z z$a$zuRe@~~>d%7*fo2NYlsJeS@#F|cbSJ%Bs<9V zw%HYpckSwHwN>bsA?|TOajf+MxciRCQTrWBc&{dqII_|NAR+NKj5zV`(*}Ywus_#_ zWUyuo=!1TK9{pSrX@8A-#|AgI#k6yBdt_SeN94EQ*RZWJ6VVLK-nT*68`;7pTa13q zRB?cS@~P$k#GV$U;g|W)W}5x^i5^pc-wNAy>AHh}>B7!}8hsTmY>-#OvYf`u#Z3Y{ zBu8`~&m~<{a{54kOXBN&VSWy17^nV9sJ(|LZX*=#Kvt7w$AddH_XDh(K*2Rc8jIuI zI-U^dYU%gdVMObFO2E$BArzZZ**-Wwm|a{xxG8+AH1?qcF&|wK;;!np02wmZlDYoy zWzlp@6lp1)P)yo*YXYiCZP*hPG5@(>_*yn7u7%zS{J=HOAWgpo)8~Xo_B^@~vSe~E z3;G7w0?+Q?ejnO|xr9(E`L3fa_3>f36q}r4+4L_xjUSVNK7k~YzT1F@4=7ZHm4C)> z`jnL6x3YUICC<0U9oPKHiSEa*t~`iD5(KEA^=xNL9=4j3x4axe$h~{>ciSLkjdm4e z6HyB{9m}=R=A4lDytJryS8kI*=SR9XKJliK5Lh$b3Us?%)M?7bn&>Wa*ETP`eDNsJ z5i*aNW=-32U+f9}S7Jjo`>1H_=`Zc;lryKYsJm*`Wn$BEOF6;LCa~?2(Z@(_Xb%=l zsH2p^#UU_gB`Eg<-LkdvN$-^tTpXOwnP)~Yp=7q;4DPW9(xf0?sgawPJ@eG2Aec*H z$Zg+>qg>oeBbidJ82C`MTJ!`$bWGt)Z8E|l#2V5bQcEKx#=jDtGMHvIP5}=Ey@o?YdF@HRX-kP-6t&B(Ra9pFpb<}BGao} zDs=wRm}pECMbehX!eOJveoe3uiQ*<+>lSI^PmiY^I;?KDLyyI9V8NKb%(j_}+9Akh z65MyN@#@$)8qv7qZEs_xB<-9-DJt{nDbKs@v?jeW#{$(f(&{uRj*}zy#ySyUR3uP) znR`Fz!M5+o8Selheg{I$!|p5~f7~i%HD)<+Z@?NID@16CpYyMwcc`Uo-AB-M7TM{2R5Je&DGqHz>@y(&wc^BX zL`T!ujejbHK51k9iU=!ECJtKkL`U3#*ia$ABN`gm0(?9xKGopSqqeV+965C%@uDI8oyy`^X zfVCcDKQ0}vYx?pL=w0fy$2plWE$E`=LX!FTZbi)v&Vq0oq z7YA|~tuxRILd+^h%uozg#<2VC7&;F&xf^$h-a@)zoPDOG6ICB;^~Fgc5g`7-f`f&{yLg~w+1!_-o z$=2749TB2!CKkZ4+HJxgb27YVxXXUgXjcM;f4(mgEK(+To@|=Vk1kZSFO;Iz8NcN7 zhf|-2?^aP_)ocVfA^7Os!_I|-6YrkbJyLd~G*%~`e6g?T+`f%mT!<-DOGHXI^m@U* z!vtg}J^Pk3%+Pd#4k4b$X(g|(2&3zK*AMu{VPaSQM%Bzq<$UHB75N>x1TtG5Ic;ba zKl|kDpuf)M7EdjGk7wSrQ#Z6CbxwMySl zEJm+J2m5H{2*p(9ipHjJ!CJNs8F=28}k#`b>iNW7+90%!ltX=(FCW|lTq{W#v3oQQHNmz>b% zQ%MF+iterhpE&3eNZ)TNV?b?v*LNe+$n!jhfxTD_A>C6_Gx8e3h07`r^O?jBO7}gM z4GY2!Ct?-g#O>cvdKK-_z;w2<>9qeyAeLVNvlJHYi^B_tKZ>xVT5_~@L(NJ~*}5_x z=JKes6@~s%ok!lkCZof%mF5fFXnrV1pZ87cC;Ze?%Ut-f*Gl!{-m7>Fejv7MS0+#!g!vlF|w(_ui(BhyC@vz#skNK!RQCf^8 zmByZ!pdHk61{OLsURAyGbytIOOx;*2Z$xF}q`lFiQcra_M@$&Rk@oRWX-1jkgBU(u z7Fv0qr@~h_s=L%MS`~*V6zsUSJDkc29;)FE%1G-Hh^vVnVb?3n=i`*z#$tM?ob?)4 z@|N0nA4S#n*EFB>r@AkWp1UzDHuLJDa^Vb5tMTHQWKwUe36pC9i4a)#zaEWNGa&8M zRT6hqI*B7o?Rv>skwrkpfzgJfA?6W#;cL2{W`;w>5}OUBx=E=b_Z+RW;ZGV61` znf!lry>~p-@%uN>LTRvAjM{I81H7eI3SKg1-)JqTqBUWrY$1jyIX35|$JI z{UYGc%%6*qAB|xWx+KCTU#?-cGj>O*ETBcKZ@xd~^)n>#)1d1pn@PF7=bmqlsH{-* z(|cw!@WoQ(Spg1}$n^J}rkRTnj?dK_EUwQo4XL+^yEy?A`$nesAsZ~#sL{d@g=*7? z7Ge(3L6C2_F^6wI>^!wk{QEiNwVqP2BAuqAD=L`r&SVnoZ*;>J7oPFKVzC!stp`kelQ)~n!GS9wWZnD%&wtE zD7|&BA0eRDqh{>tJRNK2+os_dObsS^OPt`#n>0^o^<Lu5m6l!ElGcii>T??00FRUA_P zGO&QsL<$r@8W;(NweyRDXEnruB+z)sXHr-Hlj-$Pw9Ks3aDe@Vv$$1vkR88Hc|MsQ z)%L?_fL#e6>pcT_TSe(5M&5j((Mz85gv=jc$33-0?@iy`#gopL5`xbYjx3Bf*;_P5 z&enZQeQqY`d;9Xb*zgxU#zH<0NnR8AlfJ-FtSQ<=pDLy-dJn{6T!f%~!f{;lEkU;CC$3gxApXk^;v=<;ugA&c5H>TiaAL%f*{ z>ytBk6YehJy7u7lRSi!+K9Kz8{jhp3o^RnU8U!NY76S$7+245ygV3aWxu~`dF6dBU zAKZmd)OQZme`G#j*lGA$jq2bgL{h3=^GL#N5!f&bv zwo~4}ON(C;;mKtP-5s2^RXg<=&;F}#03I?bb-h)77H=NpzRPk1iJ&9FIVk#x!wH9# z_4rYc?XCQ;)uk{UYI_6yJu{mym~WI!m&kBfQ{8qGT^wq0+I8Ev$Emq0)OrjN$?0eeX!zvZ|pSJvi07T_lU}zEPgEGHP91Esu0LkZRZ&CpZK zt7>cav7G@K2liP?s$2YbG3b&;uo&65xtH!d@Vsh2^y2DMO!3&oOS7f*Zor8pWNCr% zdtpR_<-r`mVHnvn`cE<9`U8C9z3{Thp#rcK7MdP7;J_;&uu%mS8mBEpK0Siwn6BB1 z-i=hW-af;*-}K5)n&FUqLnlU=2WgfqxY6dZq__q{Cm93th8?V{(eqfZrtq#vm7(5N z-r-#8;iq2cM#%s_8tw*|E6a;n&N&(>1D38n@_AWzpH=(KqQyFGmed~tt#^4b?tu21 zv_pY-pJ~v=2OVFOOI(?sJHG&wH!(q|9mDXi_jXUyt84&;+RbEt?w$_QaGq8isA~PN zbvb5*0WThR*2%)*6sZJ0CEWARNr6sh{CDJ$aRLuNB zT^E{DTH_SliK7U7fh%1JU&@E<*-m))gIlrmZ1r~p zC5(gOn68qf!<~VqTQReWV;uQj-@$=J6o}92R zcugy&_Gj_9^*xcZ@GtStRm6?(*w2{T$G-T@Sv#W}B8R`(cvt#N))}N3Wv#9JQq_-$i55SwIucZ1EX-tUzyD(2k2|=d z!|kQS^8k^Ia2bUP>*SXfz1=B}4DdwWc$mvvQ#rg4Y^XO@m zY}b8Ct|jAW?gPv8lIRMS*}KYSgQ`|Hd-JcVXo*$a)$sv7~v6hf(XD=B0O!LCN=dikCTq=~5^SH{rVSq%bU_eL3NfO>A31MEIdC z2r&;JJ3K%6X}{U$!lqyU09Uq?alT{)CvyXW67yn}11RhDfj11S`t=fomOyWUG7WH$ zv4S?_3lSWUkt}|rlo@ak4^CGEBe&}Y0uC)ULJ37I{22L*gqg6Iz+8soZG$ae{H8_JnUZO|!^{8jzH1Ju?36^XJ6nLuj`JVJ^`yZUR&5FQILX|yq7nLRTZO4@+FFVO_*!k-su8rbtV>%y(p1*d- za^NwdNo8Tz*Y5Qz8-7rdTwU>*|K3dutJ(GpRFg89C-DtA;HKwCK#T8ZEmQJ4skHb} z<3j=5K~b~Jc4leCNDr*|@{sAog7`&j6UAu{cKr5Wwjo*g)#an+@W)p=^os7L^H=DY zy8hDNn0ExHrWnZdD_b~6mU(`Q z-X*mL^)G9W-|u+wmd>}S;QB2-{JAGf zXQl`gJVyEdJces==1!uYd2a9+2WLhlZV2=m9)18dM|o5hw6MT_@MDDV#Y` z1DY{b-_atph)k3+YZmu;%gS)@<``OG0#`E)Up%`Ow6SeJ! z-6K!Nqqwd0`ff3WU89xZ+0EeLX@b!a25FPp*qDWpXSIs%Rf7{btHih!M=b&FkV<%N z!~=S-w46S<0}9@H?xB1*P}KE&7I*K})h5k~2yfW+OAD=NG3|&b`~nJ1K;}6d@auX5 zSKu;oJ~h_RT6nJ&0D~Aunn&+iufBTFnAMWVeT~mppA5c}X)b7Hi@TGo@3UaGY0Qqd z=k5k<0^qLmQJnSEw^AxrB{^mG4Hr9pj%sEv3rtsu^w--qP6L z6?L_4ThmkJ?lRT++JkrRvS9l7wS!{j-?`!#;&pydJu1qdH^a59eR9~Vm>sPCX!4+J z>AVeGx^H^xhT+wDoXo>d3IrOcLPL-uS@GbIi;$q1C#) zZ-2D^)gCWnz^iTJaZ4L)#u3&vA)8{&8PYf$@bOxKP=|THD9T;%QAe!bIVbV0obtju z^^Y=Q>*9Q#C;{k-mR0py78B4l@=4PB*kGdFqhtH;|Etj4fp~eL2BBLpjgksr6@oJ( z+ss(1;Z{~-q>pn4mLa-g>s?&hwL!NGD(#lJ1vX`XH=d`HSw_NoD@CvA1S9Yn2RU-JJ=pJfV^*7DC%BK4w`%KrdR(`Is%VlQK*5 z#jKZu#m4I;5&9by@wz}l_6We?uJP^pd-@|U0nXvLS2g#FF@F3`fJ;8Q);N_eM~v+CVWoAf*_!Pfm?+XUP_=Y9DF*wo`ScxbM?~JGY=+aB#tFG^n=%4Ur5xpO z*bVFHT;TVCI~?vnjH{lPeoFNjXSiu=nUoRe*8D%G1L~u@PA0~1)beXP`?5m%%-8!_ zPo-q?BY{A)yk}eLG(hp@w`Bbzobw-CZ}hbC6lVgJuYJt*=H06w8j_W-0rR22Fg0eg z6s+_(9pvaqrS@+tAAKuhm0bONoiVxviRM(jy!2z5G3awv7fybztMoScFsE_HZUHsVA14fKr9CO`I*qx3=_B5x5)(FK z^?A6~%EbBj$G;Lbf)Brj+M=v`x$SF7TaCG1O0tt@{QL2Cf#GTomz9oC*BgSB!EQi% zM`vi06=xKxCmK~TX|CT_WT8kLRLUxdZhI!9$~)-~8&bYYyGbItHl%W{_iWa-YX_J$ zyRM6g?g2c$HV-Zs;!@h$5!1=l*rtjL|@atEuhgG<_m&B5hnM1ItalTF+xC?wxxb zF|+7uh16_(CoyN!C3&JIQqgh1V*UJkJk`ddzLi%heCH8eCq#5Gzd!iP-Ab@^?B|<8 zdL)r~WIPZAKkU-$Ss5c*p9MK8*%m--nO{@f4_0Y^lm)QTIzl(=;U9WyQR;LuTvet& zUI5g899S%`ne#h!-MjW~v8Nf5J>-VbkkWv##d=0AqvNi#F|uVe5?R0uK3iDl_ERExyS@}i$G*Qxg! z!t|p_&c60aeO(>=RP`YH#HV$V@AktRuBMzp+TnCs*|Vau`!*2g9cGH+kdNW*EWqzh z&&iV54P1Lg49ctJGbwGoJNKyh5#HNaj*f3fkJv-_Z%8j>{-;24<64@V!qTP2T*Y`TGUp0b4?@ zU}P{&FdHBh_(eX*|9w12CDkymb?-aC8@o=&>{Kl4jvkJ`M;d-)x|m4E{==46W8dA-8v-ph!TLvYMS-nVTIOOwg18Gx~?fdbMI zOAyn105h#P=H7f*ZVAPJ*1tGhh61)=9DV*MvVyeQFoZEmf7EI3$NNCx6bT6DJe&HQ z%7ua5=SP|TN1~n3`||mZ*^=V@)`^@soL08YF(`E-;Lph?cZIg7!U-i|DuXsX0Ft6? zzK3k{gst}#pO-~<)mnVU+L4Zl9&0uee**HNVehUR?3lv2R#`eg!%hG=#RdQ{IVz$N zDCQ$WDY{*mh>9l_DY3~{LBU7BO?IFOZj|TFryu$Fu{P-|C4tgK(!Xz5A9enx6z61R zO?Z&SGDXI3dUalvB@JT9ktzWc0!S+M+XJrYq)MJaw|fABQr2f?#-pUN8GiYnv6K=N zD=g zO^4wm=B9 zFd4vwbX~K^pK9+3cG9)oneS#PYx(}45s-;Y`j=;9Kw<7Qt}GchP4wKg;Jt*Z z;I9N|GqXxm{u5ZU2chY#@qp;=}yz8C8md5tQi*_^@ z&v>k=nSNDD#5N%x@(|zsm!2_jB3i0gM-n7Op&I*C|8rnAG{^Lmu}#|_efH`T715#@ahp{ zjA-mAdtKQRJ(1?2Qr*TH`e#0hMegkhhaja=bECiiz-yBax&qwnJ`kyo$EowTo&%ER zkMq|yoHbccmmAc2L?$X;+TSfe!;6(O)=ECY)O%+QFsoR7y~J&EHfC}DhY(jYeP3md zi<6bA1iR-Z$Hh^@+V;gwK?gXu5@3)Q7m#i6#^+wdpv=FM0jQCdsvvu|Lgt8aY_pJ! zq;Mzb-LV0DOPp!x^PSt=(?R;BDURhz-Rm&??M4O)fJF!H>FzEv2a;TD-_RyTR#~SuMA3`YUzFN3}$j0DdE19J#rC zx}Uqui(y>r1p$>srn3jY33;8CHW&#UU;?RFpV9r~f2v5&`3Y%HI_-c(zin z08Lo*m0<&Lz`%Gb*tXM&f<* zQURQf0-HP=lSsh|+ul9u;%QflaPlpbnF}MDZ%KJlV>97c^?8q*w|M~g1Z*eEjtfIC z`6*JwRYtkXjDH0ULpr`a-!K??gNvmDN#KdUfXJGFBdf;i4Tm|ENiJ}p5i>E)8P43w ziv~ubxXd6e7n780nXH@>bS*QJ~^;-ysUBc#-uDT z&|c-a0qA0zUMdvG!nQUerl`wyAsq2e9(gMI2q2n*smsrDu^L!715??$@0LI#r_RN) zK|PF2;;7=!smODACgfRs2D0!;U9S8`W#hKoGkUo!@dX1mdxpkKtX|^` zOmgnJFkiG~PeaU`{Z#6FvXBjVQ;mR2apN3Oy@83o!8p@U!}SZm#~lVy`E6hq6CUY8 z%@bYJwKEw{QQt;cuWKn^cnTuosoS_32Zac2nAwHo*KNVeox=c-syMj8yrg%{lQ5dh z&7t@w>Z8`h*`9`OVS2ytFeDiMR| z6SW5QJ7xBc1P@O6e5T+?Cr~ZFT^`*f>%_z93X$+QsS10T3Vbl$0+)lTS|!&l%HzXmmDXF<5b-|KRs~4I#P$P{~7Fk%M&8l0FMuOn)E3Cx~RPP%P+&3COQ_b zG5@4i4`DxzwP=2QvnIkCZT8s@P;l#U7vH<~h_7f$j}hm`$v45UQgm?!__b@ca?}$z zm29{`ex&4FCl!AolMm1}KiyTVHa!nFjZb<705zqf^*A7Y?cS{3Q|~-02O-)N2^^4G z-G-arThG1Q+j{wDB80bzd|TP-xALlRYvF4vhhsn?uP7D6!BT82f;)v)IH#hwp6-Bp zZCfhtBH459t0`aSMFaG|VBt~T`wpn$>NXrT3V?_CP6?a*i-tc)!lumWrrCfU$T<7Q zxl4h74^L3IkbSn_QXqt$$=(*PM=_r<-QSe%DeX+kAmOgU%18Fc!Y z9H)hoNpYzsi`ZbxkW~EOy$W<;C`BgcfUgC2Gt&S#tGB+ph4Xa!b8%EV4EUNMNU5uL zo=Cn{h?;SCk>dLxXP4;P1ZVbFc}~&Q0_gYBjT?AbD>($2 ztiE#LBp2+kAl-I?JSa0TQ6B+?`=I2W zzp|!hfH2Q$yl`b?+NuiirfBoM=ce=O+*f$*6kzZ7Tww+9p^8aOl-v4nw?5IEA1z

$BaM$tc(3kxS)535m$7Qj~P-8zhx0>+CmLY{&iC|p^+Omd7QPLYAFf6izA(Cd2% zHHR5(j{ZX|hZk`4_IIE?OwB@f^TCOBRR(bRY9QXiDZX(AP1ooweJT}YIQhk-+%fN6 zIVC5RfFVzipDr+s*&b;;g3tNs(PmjPvT$AFN1k0Mtx7o0%Oibb$WC!a^B>$wy}4+G zVJhD}k1l6^0M%Loo9Y|?P_Wg#|@?DYoZ`eP|KtkteYb`T+=L_D6v`KeAEOd#Vf{K7k6b{-k)rJ*EZR1C}}6FcQ-zQr|wwc_XsH@%|x10{9=`j`t3zpOwn%?W+{$g|uqqzmR;>!?!N=@x~ zg-SQW>fZb4i2;If^ZFj*=aXCaUd+-Q*4>E=POgNE6L}N=*E1H2LT2XRef;aGiY6t2 zo)jF<#y58_pF7$eCa22AtUZl_;@r@VcEajXk_c7I&A*w&VViBA*QvkeAh=pEpm!9% zAPVmHRnSkt#q(|*uzlH1CogLJ`ktc^vN;Ady>>wa!eTAu)Umb~@yH<(0#O?vktvqk z>k{UhP%5BYbh)}HhRo#-ak%Us0U5sl3~b6iTHw4c2Hj|W(bDgkv98O00?Qw{d3oFsQ`AI6){vb|20UoXl&-YRT$6ssLu4J zl;P8*2ELq91+DKS$#~BFP9vxv*M@7LiH|LM{!lf&Nqbva9MN~V>pWxwx6U7e#!AUJ zqBy-Dz9T)rFO?QTHD!#%g|O~~f)JHU{A9L*I~lQ3U$LFk{1vogx0=IWVI0*Kx>Rs8r^!SiQ9ts4*pjk{nyYIG2|ygQ#kb3@KxzL7Bx*X zh>>jeaeo=yoqBsh%c$(Ue>i_IDwgtLxj$>2)dlc=cA?)zAe#ahbC0FEH#z(U$O1L@X zvmHZ>RI6tv&Aod#Fo<>k+04Xd@D6L6VY-r})}C2$lzs6$@|gmh1@kz&yZn#A<%$29 z3~TVYzox@j6fIalS^pMLD9MAm`;eQB9&j0p{tpDkPi9Exu(Hjvn*PI5p5@4bB8Bp}QYGO5aR(uR7ST zxIoJV$Yw?+qjKeNp)*1Zt2|AUI$8Vr{G!ow*t%I==VeITuilxOuQQLU&vmVeYyD`R zGuW$*U!`>Se3%ZN^d%(j&dS9X+AjKg_AUaF{1}z%^~QsAeTP8VlugB-);6O7?5995 zxe<58%EHC}00igP>V|G{;ux%**%oWbuv*1&0TFK?WbJ4E6jU4VUoWb#PQE zo9x#aTNPyH?N0}fYDSU0C{CWI*1V=>o)UI&J$vRqZ)?d2I&$0@hHm2;%4~qp3&_rf z$g*F^qkuu^eq&m$1P(w?D}zA~3XY#Gz8Ln(yPZF`yDkn4xuogM)W(158Ws+vB(n)@ z^_py?B)l%BHJs9M@$USksoCoh8x=FU%ymKy|2wjPmko$z4WPLfaI5S{xj#aQeG2yH zEq^RPH_#hYi9jm3;U<)Mm1w6RQTj=Nx4RvI;a*`SOR4scP@pld@mRI~Z8O08lH^JG z#K7kxk`-3L?3=+Wch(%)i@LRh2M&N!gd^Jy6SDNiGMJ}7ycp~IRC|mMyQ#3GBtON! zTrx$R`J@~sd;jhbEVDf#ysw}$EcA{SGDmrAF--k?@qO9LtmE?^3?epbRjPwecypIS&?uloe)vid<|AGG@_YA*ncwGhk z?5CN9*9$6|lnwRs9DDB$`C0Q*HMv}M2rGRRmRw$dWCD*}U({tdsKHGBwK+N8Q6<07 z8LB(Tk%5W=8F`;k6AHLYYe44}(Y+pzEFiT&5~=&(-h}?AF(ldV(O;)|Iu9b%X|dU` z5&%WrOg>tIB&67pISj-gC$941TpFz2Un#3oVNOmaPjQ@$azwUwWcjiw3)Y3;> zr)Tr*9@5htFL10EBRxPcdUm)&41FB(H9q&#pEr~|n(Ky@*IH2Q!Ql~NOh9IlecdhF-BiU5D+nI2JVOl!Z6MKuqkgqF4+r|%pUPtNMnYq!VUT4R z^r6JwtZn|o@{_si#+@cD7`_n_h)!T#rwhULYU2l?DW7W}9;isL^*R+3$?`*FF^M*t zR~PrNEXHTYrl_YW2xV#mwfypR z@{*T_D<;IX?4kL)$GjsXJ3O!eJpA7Kpt8q8(%e3D6RNYQT_9xY3Ml+zzxoc6s%NG0hZLI%4e+YeRy)zFUTiD0c z=_-2%4q-vpE$A^8M<#@sXW7v}xS!^UgQ=Th$_`cIMId%%31Vk3Datd|%-VilZ*FdL z@|B>`qTVe0C3=tNHZ))bvrA-M zTU#Dl&HUurr+IFX|9#Z0h~)DJg(|{y2U(q7Iv70VD}Tq@tmI(OXwKUNR{vFVShH{7 z-zl7xzN4qHwe=;diDSfeR?W!z|6Leem+nl)zM-T|(l~VfYWR_0{vkpDAL6DfalhF1 zaT{Om8p=qWv}iYZTsaaoOEaHm3ob%{Fx4Dy9{SYUb#_+#rl!UT?c zjauL0Yk3!ps!1W7va^Q+VzIf?-lWXe<3o5t>F=|Eugr;}zHPvUd+lIP=`k71t>q|} zF^B*CD@#(ramg3eS~R>CaUj0&E=*CTE&x`+8?L&V{DLYl(pompyn`CI`$?&rUvd0> z{~>9qDY+*FYUwXKHU{caD*7P!WBY0<82ho}z*m>AC?okXnK$i*HGvK4D(Iyad(%!Q zb@kWqXU66*WF!d2(ZY{=GJ!}@D*#4uyvL=Lp*hzOXaic~#fmwbhtdhMPJQ=Gh~ZRQ zbNS;(2)70um_$Is(Hr{3?0?RHo$mhcqbJO_0bTr4rOV0y=&oc9EbI4|yBUEHRT4Cg zeZ_FgF~500=%)qnrh;A)u-d0z$TzAK0^M%0zcx0`zmr!~zc>iZPzbZ?ulc=l2lR?5 z@j7|hktq0~zUJW<%sXIiqWmyV=STG)Zk_xzfW7sD-X(PFPnSCSfkD1kA&>DlkuQUw zx6kqZ2JoSd+P}LRMo_e7$&3W!=wevTy-7eePuB^g;obnWef?&yRQfM*@0!8iUB&J3GT(9R9lZ zxc7Y)ouIiU7$6&9txbK)aT2w_Ru1VP(-4ft*XGsERHCNyNN3ywo*rBeb04T$|GX82 z)N2Gf@60bIwYyBBwnM=MnD5HK^O-cNnoF-REiz}Z{mmZ3@cE?xqfqMW?hYb$!{Hfs zBRZf+`}g85Ei1n*e;I(=R4-H2?Okh;s3kK$vRQegbez` z1imcbo3vxN5fz05^3&!}1<>;BNM9NFLvmhZ_alItu0!_dXR- z3~qXTB?Vnm@?_>cpGi$ukX3bvMm!I_apluMr#=NaY8lYkTPzt`;W%BLM9HbD1XRf@ z1SK;%RnHw>zrfcI@IM5#`c*I|OdJFgVMxH}5I!9&FupC}G>dmc$c_fa2&0q0h(pX^ zd8squL7omTr;d{}EC2QuPzv>dUr4maw?SM}XDXIoY(D>elvDyBJIcs=wOCZP{#VWg zOXXeJ6@rVxS28Vx)E*QFkDXs*!m2cFZO@hGX&fPb55_@Wuck;%T-3EQ)EPH z@~w)3ao2t5-OGb?)ZBi)goxvvG5@QMT{1aFlf1LWMntg|~qYRC=ld}{&WRFk); zUg7W8ZI&$l=hX6OhgaXbD&-}{i~tj_PD05Ji>j`h*ofyf+%D(>}RN81*H2p;_T5o<`*O^W+n~v%@E1aa zY?g9d1~~7CJXbp~Nc*3D-XDTpi5Mj+`$G}O7jsgQ)B3Nphrin?e2*#k38D_;VOeVh zbQ_$^YO}NR>!Nc88%N#?ga3vHx1>F=jXW8UZBd`4j<}c6NLZNSu@u8J&w_&ptLXNy zID0g*Fo@vRvzwQ%-f2rFMBPnOoSmIbYu?=*31ZpJ01k4o+`4>g{U3;#^(f|i+weuM zc;Ks1$bUqs;S;;e1`s@rVZSHV{Sl3YcuAH>I*|`t+GlKoBPfFXP7XI1?d+?4&Xdo1 zjVhvEUzO~z5$)5(qlcUmiOBn!N)2S*Ys?WtN%tYX6eP)z#NOxgKO|MvZ&{%kveF{4FmJ@8~5Lr&=cZHd=(O$PH*gb5ws8;dR>Zi62KLw;5Xf>n-f{Le7&Gv8|& zcMz+7o)p;h8MKuzS6^~tM0SG93N2O9_#9WB4WocI3AqG zPRSbob}0Aa$cw3D4U;>)nhgzmvW}`eFbJ*u?AMj+yLW!Pi_sA`tH%V}$$G!TL5dBORm~wg-_`f2!eBQ@e_+T%xx~`sDi}aKFyWRe3Umd@K+<@2B=+fxo zN1achB&<3>mo1DSs%vU*4+$dtPAvcRC+oFm7B^o3zyd!dSQ+hlSwx-!r9iky$cm4k z@q3j`iqPs?LJtw0`YH=rqoh$hA=U^LYs{=KIfWYR@tSU)%uFbfzlR1B#Rq`UE9x#X z&!P&_XOzP5?`7Jxz@DEP-_L&PcMuj#Z!GtWqKzdAU3Y)vjP$v=n#K7khlyqcvE8V& z+8f{Dl3%XlJ`0ksOCNm>(_1yLTi2^5fpFfPee+v}Lq}RUf+03zn2fAC1pYTCyS)q93n5^UJ&gWlcF6<3UyX?&{j;zC&y&3s z65?&p$HE1QEjUrc%#z)Nva@)F$gx5>Z$JB47pLl!UGiX01!L-kB1SyQ$P@Jwxh%Ix z*u?NjHL2kREciKE&a&~7Myg(0|J2mG&TiR1kW;$TZoOT|Be{O&QGZ8;BJ*ak8w8nD zm&Xu?`44g^foEs3%CH}*4$EZ2FP8nG;ZlZFRGI%t8Po&oY!M;j4;jCrREa0rw~x0< zP)ta{&6SmF7{;L-JG;+m9ckGXg$-dahF|u;ibFY3lc;>smwKJ3eiVCdm_3%fc9Afc zY9GI!k&lnh$N{RwlgJpW9@hp}@%E3{`|af7$T2LgNI@qaQ6*DA3C*X*-)fy`>vWJ3 zPdH&FvCA?hdovZYGAbaxAu4E%+mS6>TU!GQQ#4$U-c9_^eNnd93@+Rgv=xuX&d!3O z6w`ucWG)9HtZCxuyEIGmBB2PMlXcS^bu{c-E>g^C0cD*-vV%^{I|~8~UZhZ(0d?hRZhc zL;6B-DOJ~`+m_mPgma z$4v6!gbpwE4h}e;N)Nn^(XCC3klSH}B~oDB%4r9#^-{%VZ+i*9pct%IsAKZ z>t2?rGxDtRc)_hhi2cKY%X{+XRvv47s}+Q2JwHheP9bt^4y31m!BZ|k=S{Ii<{Uhw z1azpMREXJ)OZ$6A-C~b8g(FHRZOX#X)`pQX??TcFOzwO4o*xPqo^xa`6bG(LzAk6` zEjGrbb1GUUz(g-cr}*FMNNB3(13JY9iJ&J1-=`!LxxW{4fZ|S{spv}|Z@Ht9kLg1Z z7@L1d;!THBMC|_lzDQ@Gj2`+(X5<-r3%Jhpj`_2RS-w8g4kv~9`Hms#FvL@2E+xK) z$l5qe*WApwJqV!`LtJ~b&EHk#f3_Q7mO-__s_)ziZkQOmKWR_k=k;aM)CZcsDn5Mn zfrh;7Z_?bg{AG*z=ce*w<-x^w|7KWk@M9z?-EyuUHYOg#GJAyAVV8o100MgtRM2Qs za22#*yh90?Px1%Cl3G4$6s#Dh!I zHQp-1R$XZToF82KkyqJ42=-YxTZh)#IJm7VBw#l*L_mn|opBmakbZ}w!#d-w6_65; zso%Qs(0=OMkCo&Sn?a#fNUb%<48d_H%8{_0i@*TaxkFskFbYpS4|P!4GHGX#2Z zU#RLncU>LGvIi+tg#piZ)Ct;Vh)2H|q3l6aiq9PG0NioHX_^eR#)qiMvJA8`9tLGy zw>Joj0KczQ?Y;w}d8%pa25@7e7j6L5t@##QoXbTOrpyOy$d)OyL5tUAyR@7B2W23g zRF37MP?VazjTOrNrYvX>aVvXfFKwSY?VN+_x&7SumnVF0s_gEA5F>;4l41)A22yZQ zCIxD4V{Vy7QVM~UX~)3EIKD9iZaf8(YHQw1m=i0HoaNC@Hw6*lcF#-DD}5ix;I&Jv z`@N!RCEWc(>j9*DEa3KQRFD>*P#m9?AT#*Fa2S{`^}oJyyTa>QaClde@E{oK9K9lQ zuV8M?G`x3`p7H3r&>=L&*K)Nloq$T9f;eaiBm}QRUNEPBjM1);FO%x|a*_DTRfrY&Tee%~(MBk(l+VgG}=*=*$y)+szS| z88PR|n9rI<-0A~fM)OLSOXi!yT$%`j$x49tx?rXA&b$Jy`0yk5VHn79D1NH0zd?Qr z+o5HG5^0-(l|+H^=+Aw~4J3KA5zNxlts{UJrf&l6X9UT`k{rk7PhcqL8$aGG1<2m6 zq33My&isZCfVD|vL&U@g2Ep&`GM)pMDj^^dko^S6;ka_gRXgzaa$f$qcmTo^T8HL2 z`sZfM6jx9$og|lO7>1OuyJA*eEVwXI*(;tGzEc|cL)7QBtGg%q|@<%|SR)M&>HC59>R z8fwfslo;T~=E_?be*mb-^wL|O~?9P;~zj>C8>g9WRB>&rJ8|g&&4!5c4jqW>F z^hUAx%gFot*MqmtR*GiL{LT0zEt=M^QJD1xxaSn^#jwvcie7H8Mi4gRYw8|AgBpAX z6rNo7&k+3CFi8_iLRHPQ3!4R6j znQTF-J4rYQQ#=qlTA|(FiaGm9%gMun!@==W>>YKrY-n%cF+YeX7Pgr51@;Ar#Db z1n)VTOuQg0tEIDOj4z-lQa4OiqlY8hi}QGz+XvocaPaZ^>ajqU$0t5JWe;#L%7L6M zf^&aZ)>C#E@lxq5iR}qvfD(%+s}qqU*U4Kp2|T4wH9BsrrDIdMlbSgJwrF znvNAeGOGEJxC}Dh--w%q-oKx73Z4yCXKSWIm__=XV6F93b7ypRz=ku&q}FF~cGgrC z$@9D$BqbG*%+fxKazbrzYY#L^Ys5`n3%FPMzkU1G8l1PP@+bv-V*BdVqZCny5EM$e zS^IQ}w6db2kcj9RqZ5Kq+;6zX+fzLUUmeA-ldmTOG)r^6OjoWX&H75Ufvr|)-W^2F z%=7Av&TAP`te5-g{`GY;Q@+BF6d{>2QLKS5j!dpqsz(L~gVNF;Pv09py_8;GUOfvV z`^xTpi4+o9M#I^9$AU}Q&rhaOt>6a?RGV)?3?>tqKu)H5Ip+`2lk44KB4m=+-x@!N zxD(3gf`d<1+u+sMR>sFa4Z+j12i8P=;LDUUsqylvYr)b#`AT5N5>dTv#~e)n+8vmm zLw=g-x%k&Wwq+$QGqk~@$WJ+3cMh2sWx-u3{t%zTIYq>#VG+Tqb{2zFV#FR2CaV(= ztO}v{!Lc&uGJ4mN`0{G-fUkEKlJ$1qv-vj?&i6qKM{cK5DcCv*)=PYQ1~~e57)JPI zF1=KW^qYpaab_l>=35y+sbo2okYZg#x@BI*6MEIYLq=VJR(rAHr8Z@jJ`z$~ZaCtl z^dwvnER2OB(*C&egqDvF4z6k+D*@5AqGQ=*Q0~74QcC*3O~{RCbT;B4L)7{gyWlSL z3xeF6zLRw_?jYg9@bMuJI)4BJ_D))KN_i* z_pL1qgt*QI5Qgm>kPdumnbnL4rb5u8qS2QL*++BuXG83wZ(#SCMVP+HXrstL;#I1p7p3M|j$%;3(9o-Hit3 zo3xoOKSlV_>S+d~`x0m5@-Bo}2F(WDEI18{c_^;%y#E3*F^~@jtN*i|ROYP~ zT$O)EFYsr_ZEi}fP=*99*;wf(sH{Lo+=Afsz<&xkx#&~udzgzJ!t;6M|JDa4IY6O_ z4-GOohmqBClY(jE-R5K@mJhy`QB2qd7h$S%HVrrooFS6Yh@u8nwFN1bCgvdZ(I&>d zY^~4I=Wq(vpZo#LG8mLGel#yCB+U^2lGbhXQbZ7h zbghZ&g@fnJ5_F(A`ltc2hqYytFplwUgU^!ALu@$BvREWZY2?82ToATInAPE}YkgAm zYfAz?nZS5ZIjKM~Ma_J)GI{;9GUv-qJ>~z3h$+EjpbI6_Z zf0|;%4b!^`afGwo|MqcOevZw+Rl3o3#qtSxo-p%iEAL|9T^IK!YNX2Ab0`2RseGD*!PhKcx1>KIp$qJUh$Dop0 z^NhJ`c31y6MUO72t|Q;vGI3ux4l&pcl)GXiNM?C3k?ow6yY zXS)Hl5ziK7?FQrYy{G#Bu=SQS^!l1FNxVrJmvrel}1El6@&BK8hWjOzs-n zOuT)Sija3xhd-Py{a?fm%H%Uug$v6NZbrpAK|qF(UVoLj84Yl(Dev<*0FG5WWdWBu zUHUKY%<>7o-$z#(;1L%t4zlO-e?}x!P05imxKw!HjaIbjQ6zPZvD7t8dbJWV*N;&v zRJj%2kXT)w7W=I^d)zdm75Idy$eQXUm; zyQ#kN5%^62q0GK>i?bXYNQqeB^V5Go8vB*gY{o?VGzQLQzC`_WD-XJL!+a)^Ht+3* zlq~H=ZjhOkwUY?FnI2Z%Gx#3~tw|ZnzOJl|{qR5fOE_S(DYI35@*Dbsjnu0sS! z5BQ`N4HW(paBQ1l0`nOQ1c>kw>!I|}KIZx_$Gg++ndC0Tn1Pg;`)cyQR} zn&%3El*ZeL^ zavFO7_gy7>&R3pYTDqZ^lzWWMm(;Q)6!OiNCZv2z9av-%hoU?Cj@*E-cE z-Fqd~iqTg6Vzc@GJgNr|r4!)vVY#q3 zxKV4+r0W?gx;;t~9$)Xs(Uv5$C|E#7`RL^-)^=|e&1}NDLRXRG8Nv_nQ79Gbx z#`xaa4$gIiFABoH&2{^_T@4HPuHA>ceHG7_@~Lz0<|n4M*ymLjm%py0E{_EAP_wOU zM>G;mm{U+nQWy@eQF56ec@38WM--n$axa*|gd0BpZT$2(wXznXiD7iUUGe1gppg}~ zmUzd6gQ>Z~{7g3UH#uBHca(v~hAJd7_-%jfbb#g22Us4>Gb+P=w?cgSz1Ao!=jEiB zkuJ*Y@=Ca@r2khRr4-jw8ACmIux($`Tql)w%t}-aUOa#gV;CTe)U^WWWG=qI&blxw zi|d#WhOGDg(V1-$A2WqCUuwlzSpez|f%j&Lim=$maX6LMw;#~&0m`la2oydd1f7uQ zLmQyVfV=@WLwkXSJzgp6j%CdVna{w z%2T;aD<=~RR>`B3encaMd&9fg@7G#K zv96~?{t=UCObPqtl*ZC#;#hSr5%b7xU?Qq5hFH1Dl)L@ZLyyvYYM>q$E^RTJ)EZP#J;K^(Y(+3>MKa+le@Pg*C(i)P*JVhgnzd2MV(L~)dFkryOI^NX3-vh z(+Ak5PBY?L(MbXIK5j5$DZH#$42HmOqtJUaSPDhCVZuQV4!orNz2D1z*3p%iqlG6k zBh{b!T897jLFhs!vVowu`*WAdG4g@>S(Z+9iQ*I{vqTT@B6l@Bj4m}%c#D*o_=P0i z9gzOUhQp&mD`7cRKUghUg`$i-VJ10eCiXuXH(D6Gh@An7D}VVKxQ2nX?3MrEWyR>{ zDq61LPLMqT{n&tycrh5m!9_5ApO(EBH}9j6FDTG#qZ>jzUo#+S*bK%5Fr63!+teq! z-S<|kg*r~1*7pUh1R-0;L1?K>Jyg-JXoK|bey^W$LFYQ@L9B)!ACIPgRDHgP)3Kik z^JK{$5{0gIV9r+Rhudnu==4shCz}DzPTu>tT#WXyCE)t*U9#-^c30FNUWF9;zds)P zgO^V|6-QP@$dyKPW=9h3Co`_k#yN*dNoK``3q(RoBOlEvN+rc)jaYP>!%Eo zO6@bJrmB4s%Jmyxb6bkhL$zyO#pQGa0VIDVeAMJTKd~uN{%q>{dl|5m zsFAL<6j(~YAwr>Zc7697coC`(ZF;{9CDwI+RmtTe!c9{Jp0t0mdk<*dq@+z*Xg^tx zm3m-(s}f$)Pb588E?|h)2&0Tu?o$6cn?zXc{is-PCUcE1T<>`xa^`y(u0i`0k>yS_ zbam@tJ5*sfZrlm6YduzP2*_b*zeRvYNFt0x8%P%6d86ML_u2PhsPDVG$3y}aCA?u5 zI3vrGOkw=3k*m~qGmp`8#ZNa}4+Nr`jQeZtM9NvM$yBn@a;y401usYUCoB5y0ZG;F9=hsz&||@sG;tT7NPDkD&dQBiXg? zrvlbD2h)M9qc#Jf?QgZ4!kL&X%-62o<}K{SOb17Wl>2qpyMvz;6tF~V65g1QfiA(n zY9ufA&Iy@}JD-gbEhl_Wr}bmnhklFUbv|Udt-AHu`GUJt`YngiP(iTUN1)xER4#M6 z1Y#JWGJ$(dNQwj>dvthc{cwfAEw*~SeI&$`%!WKlHlM%IA0^F2SY5)5_7pSkI9XIS z&S)I3#O%JHLM+;B|OLtYy8usmquuQs6pZ%CWfNm2U zNMUk^B6hwp?WRM=0Z0gyPa80s9>lY4LR$%zG_y)i7!Cf&S)Nf?N^-#qp20`_AP^1* zU}ujC$23k~wYjIK#)fIyeQa#a z)Dyo-D-A^aK{cqO&;D!;_H3ng`jY2~T!y1h%E4`W)6Q?kcw(YTh)hSTOJR8KbIlH| zLF@8p;yqDsy45zne1{MzMvbC0;_rxba5Bp8}g> zoKW|+1{Ky`!Z-3y!pJ6IOsp6dKufW~*N$P9dU@$MoEUV8i6|>LmfVkOD=`p%3jSG&vMfR35 zDptu0mAO3V$)B~E)=8o)VXSbY!uQ~ats&F@D%$P##8JYi^_2n~gy5A4x%s32eWYY> z`=IHa)QBbCnn6aEt3mXMg9Bs+&#G5V zY9)Q7p!e^%!6>wYpd{_e5dWYRy+zOLA4PcWSZPdYOiH627)2%1;p*a-nT6b#5B1uP zQ*qzt>MW63!sPIa*ADe~LIz_b54N4r;@6&^QN{1iF9hY@6rt;^mdnH#xnHi^D&!Qw z%NBk3@VQiCR)3-q{Zkv8|6mL^W3p>1GV!rTg?jvJ5wHs1$=iY(rVRuPbuM{V9mx1~ z!ZZs{Ei>;?DQ@&{v9C`qNAA)!1vaNuENnj?_0i5k)Peyvv(Ox3Uy=yi3BA6A}X$^_eQQ;3CS2{ni83W~Q{w)aRrhUFu{dJEWRY!(Jp4 z@()l~BfL>>X>dPUC%&zq641XUmfx8*6$Nox5uhHGPszJ)O245s(bGZIqe4R53{#yz*M zEdF9<+5Cds&(3lBSEPk` zjY>W?HQ=nbgh*z+e~?w7iYpE0;E;wL75256S011fAOx!snt6vradd2Ay)+~G>8yu(Dk`YI8msRT@-&Jgu?41R%%p_qlva*K zif@mQ)t*#FeNo`a82qW()Gj4;f-JtqBShmZ4A_uUupPf%lX6M>5ehrFj)xyv!lOO) zN*1*=)JawG2H73H*s9pI`yYs4sMAPQP=Rl<0uhBg0Iq;-;`)j1UcS+%dVs8qbW7mO z>?--PN*zKi8QPhMA&$lVk5FErh8qsZ79XEUaZmz_797}2@%yeUy4PAlRHXY4^)3-l zQuD>g{_UytJ4)9PE|rpHyt~uT6KsDnv#~&ZvRVjM}vZ*0A@T{M>f# zK!)lR^)H=NXoR<+7N=z^a7)s*;S$qr@Qs3JQHhd!A(=(^m_+Ds8w-iG(VVKwbJ$H6 zEGDd43a-o-D?>p3(*tOOjG9jP!iz*KD>>SkCntMn-!(k;FDmj`*G>)TzO`VGu1`?1 z*!S8*iKO#jy2)bjJ2Oj)o)QZquy&?sF3M4)x79D>jPZLeV`RVVl^XAOn}9>_TI|gv zNSmopDUO7jD0sun_ns4Jk2+F!GKp~F;hD{^RxRl96PChg*|Bh)C4pV~vtRQHZEmAE zlT~2K%`gw&onpd5Ot_d7!g>7j1-Cz1&FY3_uO`VDYRw!d%ZuL2UfMCKxAMH-I&^K%ogf#NcKv>E4(iFjq)n@fI9YZFb!=%| z>S0c2=>sSKq5XZ_)rw=wEMw$Y!@|Fr(+IzYyk-6nZd~&R>Cf*G-m!{{W#Gsk9}fBf zH!@Jhv%%-DU^+9&AHE=CbZikVoSpKecD7T;Q6MOTnLzpHtUK$z=Cfn0UTBL^ z43bDG^uF0p(k|c(KZrV?Dfs$qf3mbtP)!zv6RfUT;-1s~Auh{{T%5d9R3o7UqoP$o z@#bv3&-kIYu_ypguUNf?PJN@e{j=8?jWyV_gom3NNY^Ls^CH_GfP;*pTs%qH1&h@9fn2VFyE%2{) zlB#<}#pW)G3`Vph?p4%$d?#(?_2#l23G6>`9a50EHh`YG0Pli0*DY1J;)|JDAEBD3 zYNdrYTH?nHAbF=5pb=CcrBPrJWBs{$d?~C`L6h{$yf8FuJ2ad%yDQ=3gq8lJ%txSE zCX2!Z1+E&1U*yYS%>-QhPLl7oL5zp*z!{#pEGRo@krfdTPJ$O5jNY_(@odQF{cH~c{VrDDhe zc8zSc1N*{(7b(p=hMRy>ZLV1xrqjpoeibu+FmHmq=QE?{x7Yp12GeAWODgPa6sL{P6lt}c)r^oHAWhSO)YIW zP4qG>-6%&Z4acll%`7)yh(iGv@m^9)@l3*U9TQ&eU4)5Mbn)0{iZ5K8My!xC)S(&M z#eXmw{3Wb9elXkvq7<$Ds!u2@^dj=Df*z3|dSo)Jw%MJxnWJZWF{}8E2UEpq>#G29 z6LETZClA08KG~X5M_^!6sYyJZZ84YWUl4PEP_GY7&)}P(>P5uWVrkn-QUxQpcqxi= zHsha3yk3u}<39-T?Hp)3{PS4K3c2Kk?U94)fE+XksgiTv$e6UY1ENGpQR)m%6C5f%9-7r5IK&ApD8gS3tIT6Lkpl70iNlP}iQ4HZ zRii&TY&UBnM~wd48U~em^JiMt5=FB?E23w;-TDgvZDjJ+avLYQ9Kt+Rs-ysPN>`L@ z7qh`hgnTkljQCo8=#kH`a4FcL=QRQFdgBe&_GnO{^hOX=T00JgEL?$3Crm>bK`ZT!13=j}H1Lv6?DUb!VdzZ3^0r zC=H!4&3gGnAb%K2;31?YOsX9aLDD0?wfX*fxiydzUnIEAx+Zs??gTkt!jSI1i30+d zj~JWfcB>&+!if8TH& zOQyxh476pp!06V$;ZvuK_;&n)5>SltqkR>5B=4em=LBeN0XP_gb`Jb?E`sS7Iweq$ zg<^@3v>OhflVr*fg#+XQ1Ni69n`lnFN=a%^XsBYGtAT_0$medTY%r*6uKeJ6eD3HZari)t&^0+p z70FTSoEMN5a*^XThx4ed2WL#RXq-H6PtWONJXqK2Vg~V^09M*4Fmll7TNtFgT~Yya z$_@Y`hrK|5RfH5KxG3xF2@EgpKD0BKe#zwJC=Gmm`gn{+JJrhe#|WO!h6 z3gHKW(9g@mc6SJ*40#CTc|Wc);tf;rkoM9#2q!yA9q~y#ZVoKUEByX6Td}+rKZ-~s z6qi=2!Bj-iP3?R((Rv~bsgb0cx?y&_IJ)y1`bdX_aWt8wP%%Vo4Qz_K0}TVvceA|( z;U|8}jc|4sI~pq4?OPmXVUKE%yTzE5xsT}-TawEV$8DQyX`~VtWcG{9jG0$=0fA`R z73?_Z91zoFVNiivhC}f$LnG8yft{C}&?Li8NXLP{~k2+^HlcU8-kp5Pi3yP{R-~ zuPME3^v`M^j3Z%Z*bLK_iy;?Z4?xk)>14k)f050fpcqS_mh@6J4aLHr78tMuvN{VE!gB<90#6fCcbpiucT6|9Lp)YX8Jrx+W^(21-Ur`n9x@C|xzZT{>fr zz{vxKEA-4D-nu~|&GR`8ezD|Qju$~_QE3@x{^ion(W@d3YU}mu%5TmyyV|JELWgnG z8o0~lad&DcgrxbK91K$EMvS&`!9ng@gXh{BZO(mc&%VFdEq)}6H#WNg!aL@p|DV3x zrHj}7C3@H7!bMDeG^Of(Lce3gQ(2{HuD%&Y?522gXn+0ym&MgWE>M19j)iCZI_Ec~ zDAwT5YDi#Z>#p>2>hFM3cgDT@l<0X2f9Vp;uW<&%;{)Vr8HyMS?q_7iwpcU$K8+P}x4_ z<*~;;XsX!bJ}?;(H{!0+3!=j~k`azjEtHQA9oyAw5$U7b_|mT=+b-U6 zCEr&08`^wG2#W?{l==^*Qhy4Uy8(K^TAav;uA=N6uKxm6Wq$Jx%LzO?bT5u=6FbFd zB3OYhkjw)cV_MzO$CTEdgOl#0Uxw4{T5)W5T=0p{;*m*(HlY9|N5f-Tv(_`N+^D%h z=`Th%n2Wnls@|X?B%MrJlCo1v-1jvV9mY+7<9W7Xqu#v1t^=P*D6;qK{4eq0Kfr+p z&iF(WHC}Fm*6%OwXuPs$Owxm%J$oVyP&kVt$>sXbJQ- zr_)vdT=cfgYMueW>1SgUpNJuxfZb@c51k3I!~In`8j@B4Z=5YBxZZ5LTdUMspxyqXZHV+NE0a5>eJlp>uVq0E5I?FZ$4wv zFSH1OvbY;pXqnnVs0b%kgpD-jG%hnc8H_41gg+eCOsPX|S2BQ$|Hw#SmEjCtozzj5 z?{`-#08HpB_^8MlnUANYFY#56=7YQL#Nzc7rdwsp9;w(4QwR$Zyb@fAqdsrSckU49AB+Klv<43#-BqTcJIlGR{z;Ikh0(5j#mJloj+GMhTV@p>(p z;hkPbX=GXp(^MbU>rw+20EWO(uD7s*UH*1lwp+g@0j38Y-n6P&Ob>uZg2i-Y){icU zF3wh-(eAc~0`xR`ni{|imgn%scr4+-Ko2z}OdG0>^mZx3ETI}jls8BtWyu}pN+Z;?Yq8wcWu4bK_>Fk?MuA{BkBD?B>{=6GdtS8 zaKj!lK1aI|0PP|M6;S^^xI?DhT@2m5R;^LM5z!baAgNy`OK2ga{BBUBg(pu5qpu?f z5Lv{H+GZGJ_o8PYa7+(HHmgHQx)6&0ue@tJ zTm82*Ft2_2M8So||NRT&B?jw~?I!acjlgD4_8h`IV1|@ypgy>g$b4_wqS&_uwGw3!WrEuqJ zfy9gB{>T~?^cGOQRcinCZ;l=NxG;qStA+=-fZ>lqT*al1)S$(2Ko5G${;jmdX&{er z5Pg&IB`II_%L-%CFAm$iKLPq{L>sxp2B1?*M89-t^FFLo(F2wbSU2d72srJ3L1|9i zUSNP|Wr5;T#)h~X;*1SJ^qX*%{V3T3oFxs3nw7S%jM%Nm!W$V*6EOpsw8FnIlg0<3 z8z|7Yi@k}^-&3bw_NrlSZ{iZf(;Z+QRX%9zWu(W(XE~@~1fbi4kM68%jOn2n#0AmT z6+_`UoeTo-g@5+2or~$dJ^#I#N~om__VpB!HPX`2C*7%8DfUtLgs_D7RJ|Z2>i<~W zxyUuyD<3i_ALX^*8pCARSMrM@SVrd9p6KI;%mQv96~}i{0%@-!0ydE@SYP}!9qNa| z$eiz4uHX97(_t5VU>j!d2o~71oEux=t%C+_3Jk>ddi}PPWl!^52C)|b`J8PZOBkRI zzF}lTufMByqVnun5CM)RbUCPCSIG8xB})e3Zo0{e0#4^uGUb{ z@A46vnwj7ILE0og<>8kUK(5e9mNeYI8-QWzzCq^yy!8Cv*L zmkj94Z6KFIFV!Ub<7=Lrs)lNr6^cL~iT61rCl|25aGa8&8wI}tU6{~*AuE&pNWqGq zbJ(RB%b#|lpv4mO^4!5u$bF0ILB^Ssxh23Pg|R7}@dbA;_Le1tX;G`lg3YqzXPNFO zZz{8$H|tmL-+%OgFFrHxOx!4wi04r(<#0UgZs#IvX{1>Qezt(yLA{kV3LKe8=zJFS z-iB1nh+@ZSI`~wJ#`xuu*Jjff4LzV?D9n`#XZj1_%AolAxn*j~Lj>{55O3)0HhFd( zf2_Te|B>L8ta-X6-mQ0xk^&@IaRUdbff$3-nk}bBe`KIKXYMpxiADMMdr+bu|N1YFm-jZA0`x7B@byMr|nc6o9`rsZkX@edibTh)T0L)Dy&>k1qBwss?RJ6 z(#wY!!AP3%dSa#GHgB$sYPT?$;O z0IsF@>)F}H)x&oY=qOy;5YAaI%bFg?Kif<3`w8X#cDr94(6B74Lm-L_zEa4QU$!T& zAb=0bn08KY!Sjxf@dJ+K zo8c$pE{g;W>_-LayOy1CfK?xwd;!p3B&{c!8YSAK{UV%X089(zIT3f1|aGzweFyb zpMY)s8lePGO2@E4*ysLjN&qP8We_GN-{kpMWd{G_NjxD*Bm@<%zLKI=GrHNihR>9K9DH8u(0UtRvXy3|sCtYYJI~Qv&Hu z3-ZYGqd&D04lNA$Ari7i9|G=`itfC$Cb10}Q<$Q-%ik$#vyB;k=AaxxfTVQrN@CMw zdwy~S)24!Yh2E$`+eOZ)a(}Yjk2q^AdSzWxbGxU6V7M<6k)EDP zfh!%?iOrk_d?TO^Vks~)Z{a&xKDGO{`U?9H6@v{BA$U?#+Ena8w+w75PYUSz|2Ye; z#`eI-puZK7)OvrE%EjV2jjX9ueD0(f7Lf3O23lL)EZ33XAcX&=ABwfIk=8#N6=M5> zn#v6C@@Jf$COam}?3ZyTKYTQy@Uy#H%@1%{-xf{R7Z#gr=j7%l)tST!{-Xtm!RHdx z(d3Xux0w|OG*BGqW?!|jQFu1{8!m+y0Uk>be2iwNU9co42tq;(mChC^+8wBfF~F&S zKnH?Q&=U!3=*KieULussW`}FF=!)3RIq+u$MOX8d`{!o43`GDAk%*3Q9gv{h{^k=q z>XhVcgv=hjS3<_~L39*9iCEl{(gsTnykEO4Fw?v0|{8)3lNqx$(4{5@l>hjVQynnNm2Z5de;MrqyfgX^Mtf; z|Bd$~FTP9SzmgFP7NdxLEmma!*4hm5jVNul5z8Q`>e@Dz?i@v@S35>i9yp&Rf$Fy~ zyvgN_6}%uI|QU3&Ht*UNU=2QRnFH{?(&f88n)r@Pztfmi2-`J)iC z=y?%j-L%9mLLq?7=<_z3SL5t(AHTxLv{UvO5Lf(ONXuVi6A& z>4yL6X<}#p=qU<#`1HpH=;^2ug0cUvisv_&$fjKML;ZZ`opZ;BBZ4iBG?ZU`(c)oe z=646QrS%8;|1_4Kyqkh{FpH91cG9z3>dmlArF8q-IWEE}+`p~W3(DW;2X;WM{IPdg z8mIz)Qb|oBR()TNQSUVP%4pk5mGuXgz9mS^c`%07u65enBXF(JRWJqbe>5H;n_=+_ z6VgV-th%{4-x3s;)0Im3j84j>jy$;~OJu0^0T8{nw)`HTcCBZc!rV=c?7gqQWH+Qk zRjs^OA1ZsvsnU`p;x6`O_b2itOMKQuo29YwoXQKL!^N~*b);W)>hNk*h!l$6xLIzC zP;LFo-nLS_!>_^+&^ie6dJ^|!1j14;s;DCGcI#D@<@iZRcxc%jzm#;+? z0VHi}Kir?XF3zw7aInOr8V~|Af$=wyra1GuKlFR#e|2p{A_YW*&AQaUjS|!V7;<6Y z5I2hNhq}DVWSRj!K+EuS=TK+Uc|Sf>-9MS8TNZ(H5Nl~WZ8jc`jlv36ZPqf1 zsv_}(7riGwsn5C!0XTmEowRK@gJFP|u0k%HneRlc^7(8mKmk()eiTD|4xtBz{+ygB zb$S1ieyLvM?2bHD9mBZV#wC>Qyw$VOygbD$Ex7(84Ei+Dy8OwTZW(;Fl9IexBW2_m z!O-#b>9Y=2cf`P>8h%$$xLlI$s1GMNiAG0SU)To^U+qC$vQO4MqMX~*dH&fTkm{aF zIHk`|h2t|7r$@gY@C$zJeq0f4&z!=qR(7f(FJMqVyvdsakd9{8e&Bxnc>;Yg>WUJI zcGm0qJJL!MCQ_`x#`pP)U!cH$qgr?A6!rsqqeNuXb}=+6=m~gU zaptPcUWplT@3JQXW2U!;Bb0N#L+$}x+igd{2{iLF6VjaHY6MJCqr9)RaZsQhQH}}p zfCI<*omB~hu;Df&6!--(gld~eh!07Y!RdcncxNf&`-Aa|03R-!^YsgDa`I}QxZ)}z z4RkQ2v5k1o3s^t@T&=&@6evg%XaA8IP?*Rge(robTtPFLqFwR_{Ow~W^T|oIwps4G zG~FGAlx2dDmN*~=(WQ)L08OO36uSPxjQZZz#?-rn`QT?mg>@+5}flB+!>V z(6W_2EyoPuv-R+*86K}}WzV#zW9(N;Z6z=V!#UJX&5CJ77@uuKDZm9`lHRT!i)dAl zB9wRIe6KZZPpy?#eAh;Id9aW=_?%QiW4l&C2-J_8NQfbXVb+V}FgAse^T^?J|LpMz zFd^;&(kKyiJi#IW{4Q%#Kt|{wvR=L>v)}}bX{e3qCFnlsDVn{1?iU|qRT(w(H{TNyhh zfuf5K4oYN6W!2|%vJZ6Dg>+({Aq(+VN&<{3r#H0cxzh?blE^q!vVU8@QbT_1yixPU zpMhf*P$j^llTmjdY>nn6ougwvhKD!Qnsiczh8jqPP8Q}s{ol1ZhysGG3<6c6{jY)~ zt6xkEB1PWl^i~jo7WE{nJJ9Cspu7$j%TBmwRIdt#-~4-&>41>}X)YJr`!udhP(mAq6_D%R%%+%62U&REW4*zf)i8jk|%g4XFX&_j$9xNV-Uz`Aavwb4ZIfoy;aG z72DD^iDbS=fN7zsSDL?3o44XzgIHJ+;PJhuW;6M&77z%^vnR6A>VE02R8pNi;Lw%nfoBnMa(vf)Cn(_>tCs^0(?nkHb8+fo8msz_`@S^-z zn|XC1us9;UM~=&uRGf3Lsw387E9;|PK96ZAIq+mRJstU)5?vvAM|9bHc)EB*3J%Ip z{tHIX^SU)B-Odh84Jm)x`&BkRQ>v~^FfMgaM8yWssFDnlOR#Fo`P&&;i7|fust{ejFD4D3RS(phZw~l7{qYu^mCnGYqh{I|1Y6Lm3sz z#UT1h$j&>1yOnc%ozst^E{WY+N`>QaUo^Bx_NEI_<8s?Wr}<4t7@2$wR9&>>POjo^ z+DYD&Zz3775btAh?_nLmVI))$O|vg1E2SCYpHUt9dRY`NQNko2w6TJiSp_c-BlrWV zJJCR&p8gQY8hz~O#D29m2t7=SRx8rSSDg9?m&ndf0s3jvGun+t) z;EQSsjLA!4$Q};zPYjfWUabaxp%K4_%N&>exqXyG>EwZ`eBs4(snWitH`=Qb7PYtl z8w!I7woe#Ob&bjnkL8)db$009P{f2e1~_2yKmRFP7M^_WO@^vazY00Oceq*QnF@u@ z?-3`LN9{--mR)yRYTemgD^T^79p?DZ{XqRvPh8nF2vjC%Xl>=xZ6P+NaM&d%wfMcwFHKAlsEV zO=3VbOZk479;;2)|5qc&GK zG0eVis%9FsjMGS!VIV<_1fnUgt$SUqLzpSG7A9fWPU3y#^;IQAuMb5EX~C>XAey@! zyaSh}#WXnJAT?Np$3*Dz7#gHiqJ&K>0v3r=3&2u|7=Mvg2Xw}eH5(V!zrY7fsQxy- zxe%C-<)Swm$?-_IWnI<&Kht-<{h$ISx{3f75@ie=?C$~4MaBk71L5(ba0;F3nZxD` zlKbAR109=^-jDL-33*?&6M!hV6W?9#6BurP-O?EC&tBS@WDKzEoC<=~Xk;sqzJdB& z65CiHYyk}#Xf=dzyL?45w6@Tan@XbPSXhD(q#w;{T%(p}Z{N!X4sumxIBzoFSBv+x zp0YqmVIQ75;g&P{SNJLq7$4~wl)AM`aWxc0DGU*%l+JuXyI1vbn-B#>BA{c7Rx(V}I0LlwbZqXl z_32fsW8f$(6eKX*DILpm?oBii5H<;gZz4Ksu;ly{9#lM=AhzsM!DCSkl{t}|YJq4W zc6jIC>k06M9Viy<9mQn=x|G?9Vs%6OdX*xR#`-F|4dH9)sfM*g5U>rLH?6J)4=T`auoPHNPBN|lgN%b3KpV{Q+o?%oUwQ0N2N zU$NI!t1Sth8S1tIZsjs59Uyq_=+fS~tn@)sQKWL`5rWB0wJzkO-2*aToZ z9n39aKZMX}f#lP%!qCYJu(1zb2!3<@{sgmwY9qk@&IM7Jyn>5KCj6SZjda1l%a&7s z7bFVBk*Q|TnY-|%-~)mg;rrNs__9ohSh?W%&CFu`n9gG_H}#uK^${6pVbeB~lnnLv3HhpKg>l zDDmt@QgMkHeQfAd&spHG0h%jruSJ>LDh<{GTn@!q-*`MmDp~JWnm-0+HjxN=jsSF51-D56isr(oCtdi)>t;?UrODF^Au}B-4Y#@w--DoKkM(`=%ny{@zR5n!2 zv6@s%{bsA%2!9QSFQQywU$aozxlRMk|4_c~Y^B)(@L_$SumfLP}Tv_>47JjEXbp0o}mB-=qJ5!r(keb5Phmc2%N3aqRF zbS-q2WHuq`NG!^}0X+n;x;|)$WT81smFZKV_@1$seAJQTp@>nAF6B3|wLrk6c1CJ9 z#fbZ!7-nJ(H;G|(IQ!GoqsQy_MvVX%Fb?c0pgyf-ji+F1y-}EOW0Y3bN(DxSn>obt zXZq+mm6cd&$BBt;O792HR;X-EaXa0})acF^+BgtTlpyh(h~zYsTL;xt6aQWSG$#G( z-$oBWg@O*Z_1h%SL-FAk6+^UWTia+KB%aZ(-xv=3l9$?gbF_;;Y z))_gYym(XqLBHqSEdd++D59cUsZcNb63SquH5Ot-42eWe$d^pW2RoZfbgPV7_kS~F z8A?Ej5yE_3DN>_pEA2^<;Nu%-mF3wpV+8B+Xa`HSQ?sY=ldot}vu0LzzS-(p6@L z0}AJVbDO4sKUU-~l0#Rn5OzBsr+(e!pp^n3#pa@7rU}-Te{zIsL{o^vefOE}Z(1pU zHekQD!hNh4);s+~MX@_>2Kw$EEnpDVe^;FkUQ|Q`K#JlRA(9%fiQ^kvuc{(r&E{5Z z_Ht*buwH?a*!>NbTFg1SPQ7JmZCiguyT}RUHM%1l849O-PlJ)9McgB1{jn;5fY8F! z7nnao_aJ+hkW>2p9RZfqFZ!p}q$Ty`cdv#g^T45&QPMDz^wIROjI;>*K(4-6w>cZh=$%3qY`x}rfSK|#|iS? zkv-!wMMh-Plur`xzj4`xbI;hUe>i*m6QwH&RBP_P` zU&#H>`3`-F_+wR1!Ko&;%zSoCqYdS%uxNTpu~L$P{mg7BGEeKXl=$&=aXYgX-WM`D zdYqkdv2nH|I?VBTt#W;IZ%>Fv zBoI_BN%&}wngEumW6tPEF}D$eEXUVIvi{8bz|0cg`dVo$h9nf?pe0SI#%J&DboTdyso&-`RqkTr}L+~#G{dX{>UTm&)#1}MAt;e zL|)u*H$7|qE0U?ayH$*cg_%XEgOZ1pLIMd$8f?6Y>+lyZ{$=(9+p#{hbn{N0tZj3$ zc_P70Qxv@X8l_mx6<4ihLJO?&FeEBfJnB;sV8a`7y1cH=l(yLSSiguB!pxgkjZkg# zCY@qQ1V;@UiI2``F`z)10=0hPwz3a+v_EdZa34MT0nj=F5p!~e z?fWtnscV3*^p&yXW>}?yW>gIdl@`Yg4@#DZaBD~uo}>uI8~MNL0A4(Mlqu-|*g$83 z@KTs1*p7qZf7hyqa**-6wOc0Pr{F+&Ea&ctgsJ8iLNxD8fJwkCFbrgYYIXy_K$`4t zo#Urxsv zn9H=Rh0XGlXuxA}sNV3|Q|pQ7&?XY}rMAsdt|R&KZlgbIu$bH<=7H5KNuxu-55cVE zKHG8RJ-k>yUA8wom=cAUQ-tU7DiB&yfVPKD1_&%%1g7ng;K1s)T(8hoOg5$c zXkWE!*zo$^Ut|_Nd#vp`53GSM{P=Udi?Z+xfSTdp_PXW^0!^%gp1~CpcLZZ)xufu0 zu)dk{!O$qaW&(+$Xh;c8Y5k_Ww;I2rXa2I{EPpI~z2f${aqeBhW*%4LwbMA+j_+cl zYy*qAFqKX`r#}N!W&9&gNA06P%LuxFL-BmwAc|q9%-V8+r3GT;w2ah+v(dq(SGEqK(`>?Da8>c{S_>E$O5A=(=9^tdyYXG z!#wWS5BftU5Jx}M?WZp}qlh`pNH!^el+VeaM6S+!5a=WlqP9m zDE;X3vB)hF@HFd0@OW%*zvgy*eg#)qMU-=S3w-mbqy5(-2$KqO^#8PXp3!iAZKEfV zgot1W5(I;2VWNvfZ=*yPo#+w-(I$G2UL$G{qIY75j2bO^uZc2cCL3pK9Sl6{d@YFv0Y@mqOT*sEhQYMjnJv>`|cPOHv=_pY?OE$uz)T~4#xOjuMjgUibJh0&|=-j*jix(qn6)}V! z*Nt>eG45MurJ**Yyep+y_raNwuwvKL(OvGXuTj;9RF#!(9qaG z%pOs=FWyfXUzq$BwAfRy$Ld2in^`NYe8;BDI=g1nP~~r&pRJ^ll2wwv zb{Z1&=Q*Xvg*Z_9SsBOe`q20`7<)yn^*=7ie3cC&5%7rk))O|iW%Iae;jCd4v9$F5 z_2%|g{f$obtn07Sjz!13BPu%*`eY-ucaId&xx3tLia!h|gY>5>vHgaV!_t^7Hcum{ zjtn~VJZ|YuJ_)0I9#u-<#cL`l&|;^2BCBdi$0UAb3reDmfjYKl9dEaJDW@K+49>z+ z_(F$|mdeFt>It3vO_bU-{~9-FHp~In!x?YZ8vjs zL+lMCC;WHES!lq1I3`pB9L?hL$ouRMO{Il5W&CwFsf(YJF4NhK$?sy=oLn+W?v1yH z$XB`iZu|Z&dZ1z(-brwKbp{@woFORA*jQLG#dvYum8iXcnJX(~_cC=}B$mNT_}zKN zQ`)VVj=Xg0+3P1~b~mWsG{DHbS-b)t>Fvn*T$h-sm^ zOr0CcRBN=3tT757hi_;{myRb>DvsGo_J1OCJU#lRFu>}|_T8PncfAaXykPP9Mjp`K zr|Na)_oj`|G#!1WzH2@hoWHX^V1xI(z!7SeXSxba5G+AcI+txEoqfUF7--L1-}_l{ zT~?>w@y65vO)yz1bUg zrl9?xLbp|ZIe`d4?8t1BY#39A5m#ACmQWa~K2o`T3;z|79LndL>(!-mJ(KkzHpKa8 zqFVi`_D<1SK$#wY1Vx#qZnrfCy>_6_dM0mHoFRDot=JjPX5;;+8$M^DU&*e&!a#E` zrUm!paUcMTj{LI>i`<3uuLtG{`~g+7h`*TZ$4_XP!u^Djp16j`5_1 z`1k4q2dAVs_ZBTPrgWFIn1^k#ka~jXqJi!c(w!lS1l(A1d`F#SVJ-IHq-^bt1&1Lu zLw%$4^#o?s24xOIV+kI5rr}KSoZwoQH)QI$2>I}<7wjB!wGiRgGYtXn*~#d^X{thp z3wPh|5w>2GtD1N38Wp31@aZN;MWFt8Y|V3>?Qf@o+iBgu=V_9Dq=kB={i=uZT&H8_ zmb%y#v;QOf`MK_O^#!R?!_(+Y*{VK0RHsfpJB89sS%L(9Xrg)0XD~~$tc|7+7;^J* zpt6_TYNGpBrg)R0laJ=fLTi^xnrt+?B$yYamG(!4ckAjyt0)%ocpEdbyKo#^^?8i9 z3T#7SL}?FrTaNmEold>WH`NG4Fx({{`pwrSS-LK0&uBLP>UjMp*DqtGbK=#>`>TYS z&C&_=>O~4~nG`?pwO4TJ-UX+yjJhaDw-Q*NR(Bcv!SjoJW#g?+p(5L0FiA3fTyUti z+KG%rgz^q2YBnSqaHO(9(<|CSs(HpM(nC2tBE@jOw}yKxQ?$?OAzu(&fi-S#A9yWTxBj7=fvJ7?_0ftKOV>4vbnayo^x%H= z*0@XYmSe2rV1YW8RZ5LBj0wJz(Yb6s9bqoo%4F0zmXKqTTA^g2CtcstFuPjr^KzDy zm=qROw8<6!?r@;@S#rImofyIR87<~RapKR%l`F*`tjdyF!y6bhViiK+^;(unt;}st zeMG2oCtF7@g%UU`Y&R|2gf)GP3T(OS6X3`W>3V7WKL$uhfzpSEuWUnvMoVA*f@9Fj zAG>1&4s1f~&W>f-?%4=p{B11sRk{b$+>Z2;*L9-!?U`RBA?8bbtpy2MrG4W?JFWqE z5hO#TBN(P{pB4EO8%D^ZgmUokXCHi|hh>wF6YsX9umzkHomlYM&MJnJy$MCkk5_Vd zPc=wi(rBS#Y$LZeQ>+;zevkIF6`7R?_b$FortBw!G3{lgLk!JR;jIbZ{Kaxv)x?bk zG#U>T%iGQHri8YYhMGwPjM#u2KGD*?Pqg(5KODmz0zRf_yC8>rFKysT2Fq$*BxahEy<*T&N6MAz$^mHgbz66P ziGpu@gBGJ=BfRN89-7hUsZf=a{a%diwLeAf;n(?{O_WI=auqbryvw*8eTjiavO~XG zcadnaYUx-!vK@jQbO>+`B;!?#qQDH0suuB7CSs)eZ}$$gUTSdaIBQKSO7~279M=}U z9VF9FR2t-mt>xQspWe8v|2)J{R#?@uFf}a z4WNTs6$1l9>=@%-s&5SEPJFJ^l3t9xl3Fw3niv#+V?BkEJCp^=6^f{Qy(MIL_C=#n zEoHbz%`3~xXY*!DB>zK`qf8S!EL(>>zZF-%)7_v|=QG(}>Ah~mIRJ0&580%O+8~{ml8$x? zlk?W<2PdjntVd*Dm~~4#GTrY)5|LgtMRQ*CBsD^=cARZ9#i(eh_55IWVw8_-4Y^mA zpBMR1uUaibdH3|AM`7(tQLm{G+vSiFA2!RA-k(c|vwktw6fsK6hz;80PY_IX%torO zYyD0~hEGAfmFNgdUqsSVIZB^96;Mf3%&FPlJ9LoDgL`!*dO+n;1o-$i9s~S$2#FD= z!FL#nzRD*GlA{;*t~?!W;`@}zP(d<>B)v@xaUjMmHT(5Fp`514jUS!e9=KK zB_*Vi-ZpcKUkN6g6CQI|+!&An-KfQ9O0A#-*B*um)-&T4fmt-E4$&K?vozA-E#2?h z!>f%t@VsR$@e^=W2(_T7h0~R^eLGQ{6;T4jV_-T)_r+6_R4^?M{Bl$O&n-U+K&Vv@ zQsSqIv~OB}D_T&b9w{!4Hcrrx!G_uS`Ub?j_Y!%+1KQL1cSKIymQ$W-jn3DyPh8yR zT^Iep6ImvGL)AJ@U95}yzs0M0>a?1FsS>Tne*{i-kqkeI^Tl@6CH>AeA0_D4@30#s z7zpTUJ(DV5JRY20_KGD53}W>#?9%Ax@*%$0;_P)}pGg{vaLU#oKjfBPT66z#6FXefon%^n0*?V?x#&grSyJ# zaS*)OnIOV@yCgLrqu)^c#dz~EX4$Fp!yiMXdq0W(I9Xv)yxgUqAx-E`l#QT$wtG$` zxP70*zoV~pI8TLiD37u5`NJTv&Gsj$GE$wKDbM2-QuYsA{*z{>X03~lg{RSxd}T!$ zrzYF;WVP-KANo)v+|Y8H^0iZF*kX66eejWV@cyc%tW3`Z2zQJ92}IpvuiL|t8KxSy zYqq7vB^Dg4&(o|cqRQ?6xaseVrM^_n@gzw5hS0S#GN|*s!};x2%O^822KzqZN3|aM zahS;VT%6bayjt6tqBFrp&=Kt{G}FB!@W%XYKbs{(*fYFv z;S4vKbQ)c5r}b>|Ux zfM}xwLEhC*RF+uD;-@MxLJM*w`34d|GPAmnD?@dpYoqyr`N@=|unhn(%{OvhnV8Q; z>A|to-Pff(+Iivl2p-48J#|Erd!`(jZp;@=@d@xkg~hl; zu_m-*^NE`p(v=>hT{&&xkcQqbtc-`HGqqPYHK|zyV67amya;n=MLU-t>Xo_3GDP#w zlpm`*ggX2-pDAD`k!P~Jq-tj9;x=V&kLM_%tkyJpFg0*RlHKI}i?xMMAk``#SA_36qn0pX5V}+>JP_p@n zXe-q|7|H2($=GKWI6_^h5OyPYyPr?dme4m3na|MxdClD6Fd9+sn@aaXQ(W6tYK*&fsNf%dwzEx$_7G9jp1J$wc>eLb}r#|x%Yx(6tOmoQL-LW{l zw}wD|9-mfrj?Z+T8^~pTeQ|~it9uJi?s@2TdEQneQ3a0)Mi#I7gt_sNqh&fYWR@(@)fULX_!#$Yk?WBWzO?S)Y3Y= zKD<>H!(UbTc#S!kp@E&6(O*q0;VZXMC?sx_^kC|d&lQT2nf#gtCH$>IF$kvRu4R81 zWp#oBV>0`jQTtz4y+=LwU1{ace`5+LvsK7pLaQ;pf3RNjf@-iO36${WI_dp{NajA% z-LUoy1cfj@JOef(c&_-7>$4r*U$0`maywzxbc&QoLpxE&yq<}DT`_u)OHI_nu@{ap_E=G;McA_pN85Z0%`t{P9vjHT- zB{uqJC@0-TMzIi_c($wR;4T?9Bc%)1fe{fvA5oVT3v63yXQPeLT7}JP$({f=Bes(61 zg22p0n42((AM$vwOn$nXa_m!B!`4|3wNy&2X<@^(lv$9xvFwiUes$WuTD7cJ>F#96 z_r;6Xr`}FlI#gjPj1f95z7?-`XIOUMKe|%?6+E!Wj#XW%C00zGuw_uKpwe)$Q9(5( z+n6w4GP~a>?s0|Pwn=dk_m!iqOTQt0{VdfP8Xcjxk9MCIm+?N*EAeEp%~IAh=ypM>HnGYBAj*?Gh*3 z{Pb~JA2h}`2&dX&AzJKGoyEm7B#xa?Lit|omG^`RR6K9i3=C6@tL@IHnt?QPj&vWl@fEX9wd__0_UB-*o+80U5K&HwbU*15*9P z&k1H?A0+qzl2qS02%pd^aHdJ|}Rt_~Vw^olSHp9R#G zDg(tV9sSWkx=@KH#j?N`70HZKHmIA+h#R3{UkC+U&27bKeh3YDVU?Kzr80u@&;f6Vg`L^cl)!YXl zcPRKSwP1?<@9zP4XmQ_oZ<0Pi@dH0z|V2tenE&@$b9SJjGNyyh!2{x#59Y|u#L^r3W=#cyZ#AsBS}QvXbe zBIw10CyvNE}#G;Ljo>}lF&dsvL+E8Bx)mMp~p~QF}l=j0Vgs# z$scrHBhXJ4K@G5DhJt%a@d^`3f^SU-Wm)|R4{}R1aEGw5B~Q~tiFS;vtRe1Zs@r|> zJnw80?+a~I@}W!M{%XitLKiy2=R#uozElODZIx@yftB~lY)-<(5mMwG7MS3%y*zC< zUkuH?MgU~m*_mf_Ug5ve-8k)EGouv1zN9$!v#tBG3x%y+olc1dTGRuQCu^yb2%x9LxC|J4c?qU+Op5QT_-xOS&KU2K}CX6b8JCA%-zk_Pu4r* zMROMsm5{rz@}^E0vyDJO;>q2KFV>K)86RKNo%MyLDf?KxP;y;`vDUQ#HP%vOmNVh~ z=;3+Fa`4F2K2dTQ=eOIYOFbk%8=NRDY;I)f_!(S6V3L$2&H47fP0mq{dgb{b$Yux@ zhkn52uSTeqN=x);XEkjFbj)eg@B>cw`z#+X9p0FITFNfL>~UAj!iKfdvMU638fa}| zx;t=R4@KhP^Ro^=6v8a|0m45Ab^!y0)+qT;isjM-`~Vu8L4A0zZu*6-zVMwgR2pRT z-lfXGOPz0Fzg?`)X+EnYQY0k&dbaj_*fsS$3t@7IKPYMsOt2Vh^%|%-s{a}8?)K%bePdqVH)K6kLDxV#P0UQV zEaQ(yuwTXdyKDs75qynozkk>B@7VKx$1d3MYQUFc`f&hHj*E-^KuV(L#f$)X)J>aM zP8wv21WfB*xt*V3^iWApOz1_q^p$)LWw)YthLLUX^OK@M)2)>TO*W0WzWP%HWAA!x z2@gzK^{eB$TTcdgaa%apo#ln!Ne!sMsiwB)t9_;Elb2QaC2(vp%g(noKs~9ID;s^g zci(GIyI8Js@v+{Sym98u2Zc)UdRkqKx|N@E@4oBj4Mex?;bWX2A=Fr_Y>!lVn+~k9 zzr&XT!2qn%h>9;o30S3klYVRX+1JWX&*Furww}0+c6+5q?;HiyCw+Z!Cs+wz#xx-` zhI8*5`}BN11&m2wf(K@q-|wS+)IAnOTHn|@=g13?2M4T|(AtsPCW+y`^<0~utD8g| zeXQBH+WSy8tPfA;oY1XRFsqhXP^@2)F7GSP9JxnEn- z%nb>lTsj}QG(ICLP;Ve#H9K*(fAO)4TsIdlonb-B@oYp_Q~gSJtsgiN+)H@ahY>6% zic0ehC<4}6&+*Hi{3*}^GJQwDpkiDj$F~5<>*6E8%ILJn=hp!Sa#{k?k(M7TJVtMs za~T=h2Xw~rlW`d~gh92$hS5Q1Pe8p}4@b!5@F`ex2))0Vx+V;X`^xQtQ28|mj@YcY zd>7C92^?_?$}@i|3n>B(KAuL8Wi?d+vI5VRTyS4`PY)d*Du0&52fbRS*4UnBKB}8yy!{AmU7qOQc4JHt$GW0Ho7}gqPJ@*Sifdl9GM;B z%b-i6JKGfE8qwNapuVyxXxRwHD0M?0(xv zJfn@4uy|iO37P+d9p4<5}Wc8&r?n@43D7CLN>rjQA|$aMlCt71JdF!LBh1=Tb0{8UYlC3T`487ZLY1H|Wu|D{j>(zUtutyMAn5Be%6&`zTj}b5FSsBA5GMC@_*%_C{))II@&qXaB*S_C1>rY2OiN31 zj?ES$AZ#z&Ngkm8|DXmEAAp(Y{us^}aU~F@mTGsNgAY*rZ@x@tgUY1`x!4mHTu2BD zpaBeCX9p2LlNOSmjn-graka?+Cz_{?FdQ%LHI*VL7gwMrsX^N|SR&z6LH@pvMOaX> zi^I~})qg{8;sD-o{?synUQ_uef1HQ`7Yp2~lQGCen8b8e3D zYCo{(I}wAXw&uH@%LC|&!nMc#Aa*82uyuAhOqCOa-VXtCAyO4z&z~efaJQO!lx^YQ zV(RO^pj;ZLayb2Ja0QqX>h9F~SR8`d4`5v5yXH1q*f{|t$PTQ(n|rPVJN-6`)-`fK za7V1(uF4<=s9Pa-}*@4Hp0V z>pv~w{eLf~QS-o59Gt5z^3oET|IvuQSk7+SJjh%BVG!WW418{NIB&*Uzz&IBe-NO< zanAUcS^d+#dX$0TPcllM{_~T6y=nRlT*iba&VUy%{{Q-V^aQkO8BMYY{O@1BO(g@y zx`!)%1gd)fYVUu(Z`uH@VzDIpFJJlM*gZzJ= oNk2HIWg~d#zmqFH?kVJ0tY>_pgs5r~7YF>w%P32iO1|*_AI&f7i~s-t literal 0 HcmV?d00001 From c62c915bca2ce8469d4f289260ebd2932d96f417 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 23 Jan 2024 16:38:05 +0100 Subject: [PATCH 15/97] Minor bugfix --- simulator/learned_model/include/pymodel_interface.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/simulator/learned_model/include/pymodel_interface.hpp b/simulator/learned_model/include/pymodel_interface.hpp index 6e57384aa4c9d..e1c7fe694d056 100644 --- a/simulator/learned_model/include/pymodel_interface.hpp +++ b/simulator/learned_model/include/pymodel_interface.hpp @@ -1,6 +1,8 @@ #ifndef LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ #define LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ +#include + class PymodelInterface { public: From 3c72983c88835b25c3ce947ae02b0c0015a3956f Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 23 Jan 2024 16:39:23 +0100 Subject: [PATCH 16/97] Remove explicit error-model. All sub-models should be properly enclosed. --- .../include/pymodel_base_error.hpp | 81 ------------------- .../include/pymodel_interconnected_model.hpp | 24 +++--- .../include/pymodel_simple_model.hpp | 6 +- .../vehicle_model/sim_model_pymodels.cpp | 35 ++------ 4 files changed, 20 insertions(+), 126 deletions(-) delete mode 100644 simulator/learned_model/include/pymodel_base_error.hpp diff --git a/simulator/learned_model/include/pymodel_base_error.hpp b/simulator/learned_model/include/pymodel_base_error.hpp deleted file mode 100644 index ef2e6116ea072..0000000000000 --- a/simulator/learned_model/include/pymodel_base_error.hpp +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef LEARNED_MODEL__SIM_PYMODEL_BASE_ERROR_HPP_ -#define LEARNED_MODEL__SIM_PYMODEL_BASE_ERROR_HPP_ - -#include "pymodel_simple_model.hpp" -#include "pymodel_interface.hpp" - - -class SimPymodelBaseError : public PymodelInterface -{ -private: - PymodelSimpleModel base_model; - PymodelSimpleModel error_model; - -public: - SimPymodelBaseError(std::tuple base_desc, std::tuple error_desc) - : base_model(std::get<0>(base_desc), std::get<1>(base_desc), std::get<2>(base_desc)), - error_model(std::get<0>(error_desc), std::get<1>(error_desc), std::get<2>(error_desc)) - { - } - - /** - * @brief create a map from model signal vector to python model inputs - * @param [in] signal_vec_names names of signals in model signal vector - */ - void mapInputs(std::vector signals_vec_names) override - { - base_model.mapInputs(signals_vec_names); - error_model.mapInputs(signals_vec_names); - } - - /** - * @brief create a map from python outputs to model signal vector - * @param [in] signal_vec_names names of signals in model signal vector - */ - void mapOutputs(std::vector signals_vec_names) override - { - base_model.mapOutputs(signals_vec_names); - error_model.mapOutputs(signals_vec_names); - } - - /** - * @brief get names of inputs of python model - */ - std::vector getInputNames() override - { - std::vector base_input_name = base_model.getInputNames(); - std::vector error_input_name = error_model.getInputNames(); - base_input_name.insert(base_input_name.end(), error_input_name.begin(), error_input_name.end()); - return base_input_name; - } - - /** - * @brief get names of states of python model - */ - std::vector getStateNames() override - { - std::vector base_state_name = base_model.getStateNames(); - std::vector error_state_name = error_model.getStateNames(); - base_state_name.insert(base_state_name.end(), error_state_name.begin(), error_state_name.end()); - return base_state_name; - } - - /** - * @brief calculate the next state of this submodule - * @param [in] model_signals_vec values of signals in model signal vector - * @param [in] model_signals_vec_next values of signals in model signal vector to update - */ - std::vector getNextState(std::vector model_signals_vec, std::vector model_signals_vec_next) override - { - std::vector base_next_state = base_model.getNextState(model_signals_vec, model_signals_vec_next); - std::vector base_next_state_delayed = base_model.signals_w_delayed_input; - std::vector next_state_error(model_signals_vec.size()); - std::fill(next_state_error.begin(), next_state_error.end(), 0.0); - std::vector error_correction = error_model.getNextState(base_next_state_delayed, next_state_error); // need to change input in all_variable input to delayed one from base model - for (size_t i = 0; i < base_next_state.size(); i++) base_next_state[i] += error_correction[i]; - return base_next_state; - } -}; - - -#endif // LEARNED_MODEL__SIM_PYMODEL_BASE_ERROR_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_interconnected_model.hpp b/simulator/learned_model/include/pymodel_interconnected_model.hpp index ac24181787802..14830ce36721e 100644 --- a/simulator/learned_model/include/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/pymodel_interconnected_model.hpp @@ -1,7 +1,7 @@ #ifndef LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ #define LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ -#include "pymodel_base_error.hpp" +// #include "pymodel_base_error.hpp" #include "pymodel_interface.hpp" #include "pymodel_simple_model.hpp" #include "model_connections_helpers.hpp" @@ -128,17 +128,17 @@ class InterconnectedModel submodels.push_back(std::unique_ptr(new_model)); } - /** - * @brief add a sub-model consisting of base + error sub-model - * @param [in] base_desc descriptor of base sub-model - * @param [in] error_desc descriptor of error sub-model - */ - void addSubmodelBaseError( - std::tuple base_desc, std::tuple error_desc) - { - submodels.push_back( - std::unique_ptr(new SimPymodelBaseError(base_desc, error_desc))); - } + // /** + // * @brief add a sub-model consisting of base + error sub-model + // * @param [in] base_desc descriptor of base sub-model + // * @param [in] error_desc descriptor of error sub-model + // */ + // void addSubmodelBaseError( + // std::tuple base_desc, std::tuple error_desc) + // { + // submodels.push_back( + // std::unique_ptr(new SimPymodelBaseError(base_desc, error_desc))); + // } /** * @brief set a new model state if it was changed using PSIM interface (mainly position and orientation) diff --git a/simulator/learned_model/include/pymodel_simple_model.hpp b/simulator/learned_model/include/pymodel_simple_model.hpp index 693cd12329292..1aebd6c455207 100644 --- a/simulator/learned_model/include/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/pymodel_simple_model.hpp @@ -30,8 +30,6 @@ class PymodelSimpleModel : public PymodelInterface std::vector pymodel_state_names; public: - std::vector signals_w_delayed_input; - /** * @brief constructor * @param [in] pymodel_import_name_ path to python model @@ -105,9 +103,7 @@ class PymodelSimpleModel : public PymodelInterface // map outputs from python model to required outputs std::vector next_state = fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); - - signals_w_delayed_input = fillVectorUsingMap(action_delayed, model_signals_vec_next, map_sig_vec_to_pyin, false); - + return next_state; } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 2fa8eb5766102..4fbd5bc91cf8b 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -44,47 +44,26 @@ SimModelPymodels::SimModelPymodels( (char*)nullptr, (char*)"KinematicBicycleSteerVel" }, - // { - // (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", - // (char*)"$HOME/f1tenth/ws_testing/base_model_save", - // (char*)"SimpleSteeringHyst" - // }, - { - (char*)"control_analysis_pipeline.model.base_model.base_model_simple_velocity", - (char*)nullptr, - (char*)"SimpleVelocity" - } - }; - - vehicle.addSubmodel(model_desc[0]); - vehicle.addSubmodel(model_desc[1]); - - - - - std::vector> error_model_desc = { { (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", (char*)"$HOME/autoware_model_params/base_model_save", (char*)"SimpleSteeringHyst" }, { - (char*)"control_analysis_pipeline.model.error_model.error_debug_model", + (char*)"control_analysis_pipeline.model.base_model.base_model_simple_velocity", (char*)nullptr, - (char*)"ErrorDebugModel" - }, + (char*)"SimpleVelocity" + } }; - // vehicle.AddSubmodel(model_desc[1]); - vehicle.addSubmodelBaseError(error_model_desc[0], error_model_desc[1]); - - - + vehicle.addSubmodel(model_desc[0]); + vehicle.addSubmodel(model_desc[1]); + vehicle.addSubmodel(model_desc[2]); vehicle.generateConnections(input_names, state_names); std::cout << dt << std::endl; - std::cout << "OKAAAAAYYYYYYY" << std::endl; + std::cout << "Python model loaded successfully " << std::endl; } double SimModelPymodels::getX() From cd4b82b272fa24317943bfc41e56219dee72c63e Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 23 Jan 2024 18:16:01 +0100 Subject: [PATCH 17/97] Remove explicit error-model. All sub-models should be properly enclosed. --- .../include/pymodel_interconnected_model.hpp | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/simulator/learned_model/include/pymodel_interconnected_model.hpp b/simulator/learned_model/include/pymodel_interconnected_model.hpp index 14830ce36721e..505448729f403 100644 --- a/simulator/learned_model/include/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/pymodel_interconnected_model.hpp @@ -1,7 +1,6 @@ #ifndef LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ #define LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ -// #include "pymodel_base_error.hpp" #include "pymodel_interface.hpp" #include "pymodel_simple_model.hpp" #include "model_connections_helpers.hpp" @@ -128,18 +127,6 @@ class InterconnectedModel submodels.push_back(std::unique_ptr(new_model)); } - // /** - // * @brief add a sub-model consisting of base + error sub-model - // * @param [in] base_desc descriptor of base sub-model - // * @param [in] error_desc descriptor of error sub-model - // */ - // void addSubmodelBaseError( - // std::tuple base_desc, std::tuple error_desc) - // { - // submodels.push_back( - // std::unique_ptr(new SimPymodelBaseError(base_desc, error_desc))); - // } - /** * @brief set a new model state if it was changed using PSIM interface (mainly position and orientation) * @param [in] new_state new state set by PSIM From 1fae8c698de71573669846a23e4ba5636bf3d53b Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 23 Jan 2024 18:16:46 +0100 Subject: [PATCH 18/97] Simplify pymodel interface --- simulator/learned_model/README.md | 10 +++---- .../include/pymodel_simple_model.hpp | 14 ++++------ .../vehicle_model/sim_model_pymodels.cpp | 27 ++++++++++++++----- 3 files changed, 30 insertions(+), 21 deletions(-) diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md index 16c2336a5f05c..57984a200468c 100644 --- a/simulator/learned_model/README.md +++ b/simulator/learned_model/README.md @@ -31,19 +31,17 @@ class CustomPythonSubmodel: def forward(self, action, state): # Required """ - Calculate forward pass through the model. - Currently implemented in the way that the next_state - needs to be 2d array with size (1 x NUM_OF_STATES). + Calculate forward pass through the model and returns next_state. """ - return list(list()) # next state + return list() - def get_sig_state_names(self): # Required + def get_state_names(self): # Required """ Return list of string names of the model states (outputs). """ return list() - def get_sig_action_names(self): # Required + def get_action_names(self): # Required """ Return list of string names of the model actions (inputs). """ diff --git a/simulator/learned_model/include/pymodel_simple_model.hpp b/simulator/learned_model/include/pymodel_simple_model.hpp index 1aebd6c455207..a0a49e0e0723e 100644 --- a/simulator/learned_model/include/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/pymodel_simple_model.hpp @@ -45,7 +45,7 @@ class PymodelSimpleModel : public PymodelInterface // Import python module py::module_ imported_module = py::module_::import(pymodel_import_name); // Initialize model class from imported module - py_model_class = imported_module.attr(py_class_name)(nullptr); + py_model_class = imported_module.attr(py_class_name)(); } else { @@ -65,13 +65,13 @@ class PymodelSimpleModel : public PymodelInterface // TODO warning that using default model params } - py::list pymodel_state_names_ = py_model_class.attr("get_sig_state_names")(); + py::list pymodel_state_names_ = py_model_class.attr("get_state_names")(); num_outputs_py = pymodel_state_names_.size(); for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++){ pymodel_state_names.push_back(PyBytes_AS_STRING(PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); } - py::list pymodel_input_names_ = py_model_class.attr("get_sig_action_names")(); + py::list pymodel_input_names_ = py_model_class.attr("get_action_names")(); num_inputs_py = pymodel_input_names_.size(); for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++){ pymodel_input_names.push_back(PyBytes_AS_STRING(PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); @@ -95,15 +95,11 @@ class PymodelSimpleModel : public PymodelInterface // forward pass through the base model py::tuple res = py_model_class.attr("forward")(py_inputs, py_state); - std::vector py_state_next = res[0].cast>>()[0]; - - // for (auto state : model_state) std::cout << "STATE AFTER : " << state << std::endl; - - std::vector action_delayed = res[1].cast>>()[0]; + std::vector py_state_next = res.cast>(); // map outputs from python model to required outputs std::vector next_state = fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); - + return next_state; } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 4fbd5bc91cf8b..5a615c61362c2 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -39,20 +39,35 @@ SimModelPymodels::SimModelPymodels( // TODO this should be in config file not hardcoded here // Think of a way how to differentiate between "simple" model and "base + error" model std::vector> model_desc = { + // { + // (char*)"control_analysis_pipeline.model.base_model.kinematic_bicycle_steer_vel", + // (char*)nullptr, + // (char*)"KinematicBicycleSteerVel" + // }, { - (char*)"control_analysis_pipeline.model.base_model.kinematic_bicycle_steer_vel", + (char*)"control_analysis_pipeline.autoware_models.vehicle.kinematic", (char*)nullptr, - (char*)"KinematicBicycleSteerVel" + (char*)"KinematicModel" }, + // { + // (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", + // (char*)"$HOME/autoware_model_params/base_model_save", + // (char*)"SimpleSteeringHyst" + // }, { - (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", + (char*)"control_analysis_pipeline.autoware_models.steering.example_base_error", (char*)"$HOME/autoware_model_params/base_model_save", - (char*)"SimpleSteeringHyst" + (char*)"BaseError" }, + // { + // (char*)"control_analysis_pipeline.model.base_model.base_model_simple_velocity", + // (char*)nullptr, + // (char*)"SimpleVelocity" + // } { - (char*)"control_analysis_pipeline.model.base_model.base_model_simple_velocity", + (char*)"control_analysis_pipeline.autoware_models.drive.drive_example", (char*)nullptr, - (char*)"SimpleVelocity" + (char*)"DriveExample" } }; From 3f24a262ecd9a6f8ff156659aaca6ba47fd2ebda Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 23 Jan 2024 19:18:19 +0100 Subject: [PATCH 19/97] Fix formatting --- .../vehicle_model/sim_model_pymodels.cpp | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 5a615c61362c2..f3ce0ac444d19 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -39,31 +39,16 @@ SimModelPymodels::SimModelPymodels( // TODO this should be in config file not hardcoded here // Think of a way how to differentiate between "simple" model and "base + error" model std::vector> model_desc = { - // { - // (char*)"control_analysis_pipeline.model.base_model.kinematic_bicycle_steer_vel", - // (char*)nullptr, - // (char*)"KinematicBicycleSteerVel" - // }, { (char*)"control_analysis_pipeline.autoware_models.vehicle.kinematic", (char*)nullptr, (char*)"KinematicModel" }, - // { - // (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", - // (char*)"$HOME/autoware_model_params/base_model_save", - // (char*)"SimpleSteeringHyst" - // }, { (char*)"control_analysis_pipeline.autoware_models.steering.example_base_error", (char*)"$HOME/autoware_model_params/base_model_save", (char*)"BaseError" }, - // { - // (char*)"control_analysis_pipeline.model.base_model.base_model_simple_velocity", - // (char*)nullptr, - // (char*)"SimpleVelocity" - // } { (char*)"control_analysis_pipeline.autoware_models.drive.drive_example", (char*)nullptr, From 88043aa14f53ad4407b6c55724c0099b8a58d4d1 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 24 Jan 2024 11:50:38 +0100 Subject: [PATCH 20/97] Fix formatting and add comments --- .../include/pymodel_interconnected_model.hpp | 30 +++++++++++++++++-- .../include/pymodel_simple_model.hpp | 4 ++- .../vehicle_model/sim_model_pymodels.hpp | 6 ++-- .../vehicle_model/sim_model_pymodels.cpp | 2 +- 4 files changed, 33 insertions(+), 9 deletions(-) diff --git a/simulator/learned_model/include/pymodel_interconnected_model.hpp b/simulator/learned_model/include/pymodel_interconnected_model.hpp index 505448729f403..7c1836685841a 100644 --- a/simulator/learned_model/include/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/pymodel_interconnected_model.hpp @@ -14,10 +14,10 @@ namespace py = pybind11; class InterconnectedModel { - int num_signals; - - std::vector model_signals_vec; + // Vector of unique names of inputs and outputs of sub-models std::vector signals_vec_names; + std::vector model_signals_vec; + int num_signals; std::vector> submodels; @@ -67,20 +67,33 @@ class InterconnectedModel return connection_map; } + /** + * @brief create a mapping between vector of signal input names from PSIM to vector of signals + * @param [in] in_names vector of signal input names from PSIM + */ void mapInputs(std::vector in_names) { // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index in "signals_vec_names" map_in_to_sig_vec = createConnectionsMap(signals_vec_names, in_names); } + /** + * @brief create a mapping between vector of signal output names from PSIM to vector of signals + * @param [in] out_names vector of signal output names from PSIM + */ void mapOutputs(std::vector out_names) { // index in "map_sig_vec_to_out" is index in "out_names" and value in "map_sig_vec_to_out" is index in "signals_vec_names" map_sig_vec_to_out = createConnectionsMap(signals_vec_names, out_names); } + /** + * @brief add unique names to the vector of signal names + * @param [in] names vector of signal names + */ void addNamesToSigVec(const std::vector & names) { + // Check if the name is already in the vector. If not add it. for (char * name : names) { if (std::find(signals_vec_names.begin(), signals_vec_names.end(), name) == signals_vec_names.end()) { signals_vec_names.push_back(name); @@ -88,6 +101,9 @@ class InterconnectedModel } } + /** + * @brief create of signal names from all submodels + */ void getSignalNames() { for (auto & submodel : submodels) { @@ -104,14 +120,19 @@ class InterconnectedModel */ void generateConnections(std::vector in_names, std::vector out_names) { + // Create vector of signal names getSignalNames(); num_signals = signals_vec_names.size(); + // Init vector of signal values for (int i = 0; i < num_signals; i++) model_signals_vec.push_back(0); + // For every sub-model create mapping from vector of signals to inputs and outputs for (auto & submodel : submodels) { submodel->mapInputs(signals_vec_names); submodel->mapOutputs(signals_vec_names); } + + // Create mapping from vector of signals to inputs and outputs of PSIM mapInputs(in_names); mapOutputs(out_names); } @@ -135,6 +156,7 @@ class InterconnectedModel { bool state_changed_externally = false; + // Check if some state was changed externally for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { if (abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > 1e-6) { state_changed_externally = true; @@ -143,8 +165,10 @@ class InterconnectedModel } if (state_changed_externally) { + // Reinitialize model std::cout << "Reseting model" << std::endl; + // Currently initializing model to zero -> TODO find a way how to initialize them to some other default values std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { diff --git a/simulator/learned_model/include/pymodel_simple_model.hpp b/simulator/learned_model/include/pymodel_simple_model.hpp index a0a49e0e0723e..d674876d2b2ba 100644 --- a/simulator/learned_model/include/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/pymodel_simple_model.hpp @@ -65,12 +65,14 @@ class PymodelSimpleModel : public PymodelInterface // TODO warning that using default model params } + // Get string names of states of python model, convert them to C++ string and store them in pymodel_state_names py::list pymodel_state_names_ = py_model_class.attr("get_state_names")(); num_outputs_py = pymodel_state_names_.size(); for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++){ pymodel_state_names.push_back(PyBytes_AS_STRING(PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); } + // Get string names of actions (inputs) of python model, convert them to C++ string and store them in pymodel_input_names py::list pymodel_input_names_ = py_model_class.attr("get_action_names")(); num_inputs_py = pymodel_input_names_.size(); for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++){ @@ -87,7 +89,7 @@ class PymodelSimpleModel : public PymodelInterface std::vector getNextState(std::vector model_signals_vec, std::vector model_signals_vec_next) override { - // get inputs and states of the python model from the vector of all signals + // get inputs and states of the python model from the vector of signals std::vector py_inputs(num_inputs_py); std::vector py_state(num_outputs_py); py_inputs = fillVectorUsingMap(py_inputs, model_signals_vec, map_sig_vec_to_pyin, true); diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index affdad9bf24cf..60f0ad9e1f938 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -106,11 +106,9 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ std::deque vx_input_queue_; //!< @brief buffer for velocity command std::deque steer_input_queue_; //!< @brief buffer for angular velocity command const double vx_delay_; //!< @brief time delay for velocity command [s] - const double vx_time_constant_; - //!< @brief time constant for 1D model of velocity dynamics + const double vx_time_constant_; //!< @brief time constant for 1D model of velocity dynamics const double steer_delay_; //!< @brief time delay for angular-velocity command [s] - const double - steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics + const double steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics const double steer_dead_band_; //!< @brief dead band for steering angle [rad] /** diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index f3ce0ac444d19..585a661777cf3 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -38,7 +38,7 @@ SimModelPymodels::SimModelPymodels( // TODO this should be in config file not hardcoded here // Think of a way how to differentiate between "simple" model and "base + error" model - std::vector> model_desc = { + std::vector> model_descriptors = { { (char*)"control_analysis_pipeline.autoware_models.vehicle.kinematic", (char*)nullptr, From 19f7d407a901d24e8a8f31c3f3f4c6bf589cc097 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 24 Jan 2024 12:54:42 +0100 Subject: [PATCH 21/97] Remove unnecessary model parameters --- .../vehicle_model/sim_model_pymodels.hpp | 28 +---------------- .../simple_planning_simulator_core.cpp | 4 +-- .../vehicle_model/sim_model_pymodels.cpp | 30 +++++-------------- 3 files changed, 10 insertions(+), 52 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index 60f0ad9e1f938..ded9550107381 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -37,23 +37,9 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ public: /** * @brief constructor - * @param [in] vx_lim velocity limit [m/s] - * @param [in] steer_lim steering limit [rad] - * @param [in] vx_rate_lim acceleration limit [m/ss] - * @param [in] steer_rate_lim steering angular velocity limit [rad/ss] - * @param [in] wheelbase vehicle wheelbase length [m] * @param [in] dt delta time information to set input buffer for delay - * @param [in] vx_delay time delay for velocity command [s] - * @param [in] vx_time_constant time constant for 1D model of velocity dynamics - * @param [in] steer_delay time delay for steering command [s] - * @param [in] steer_time_constant time constant for 1D model of steering dynamics - * @param [in] steer_dead_band dead band for steering angle [rad] - * @param [in] HELLO_FROM_CODING_GOD PRAISE THE MESSIAH!!!!! <3 */ - SimModelPymodels( - double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band); + SimModelPymodels(double dt); /** * @brief destructor @@ -61,7 +47,6 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ ~SimModelPymodels() = default; private: - const double MIN_TIME_CONSTANT; //!< @brief minimum time constant /* Specify string names for states and inputs. So we can automatically map states and @@ -93,23 +78,12 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ InterconnectedModel vehicle; // py::scoped_interpreter guard{}; - - const double vx_lim_; //!< @brief velocity limit - const double vx_rate_lim_; //!< @brief acceleration limit - const double steer_lim_; //!< @brief steering limit [rad] - const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] - const double wheelbase_; //!< @brief vehicle wheelbase length [m] double prev_vx_ = 0.0; double current_ax_ = 0.0; std::deque vx_input_queue_; //!< @brief buffer for velocity command std::deque steer_input_queue_; //!< @brief buffer for angular velocity command - const double vx_delay_; //!< @brief time delay for velocity command [s] - const double vx_time_constant_; //!< @brief time constant for 1D model of velocity dynamics - const double steer_delay_; //!< @brief time delay for angular-velocity command [s] - const double steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics - const double steer_dead_band_; //!< @brief dead band for steering angle [rad] /** * @brief set queue buffer for input command diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 63c3aa9922a65..bcb7032de964d 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -280,9 +280,7 @@ void SimplePlanningSimulator::initialize_vehicle_model() acceleration_map_path); } else if (vehicle_model_type_str == "PYMODELS"){ vehicle_model_type_ = VehicleModelType::PYMODELS; - vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, steer_dead_band); + vehicle_model_ptr_ = std::make_shared(timer_sampling_time_ms_ / 1000.0); }else{ throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 585a661777cf3..8efcdf94fec33 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -18,22 +18,8 @@ #include "pymodel_interconnected_model.hpp" -SimModelPymodels::SimModelPymodels( - double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band) -: SimModelInterface(5 /* dim x */, 2 /* dim u */), - MIN_TIME_CONSTANT(0.03), - vx_lim_(vx_lim), - vx_rate_lim_(vx_rate_lim), - steer_lim_(steer_lim), - steer_rate_lim_(steer_rate_lim), - wheelbase_(wheelbase), - vx_delay_(vx_delay), - vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), - steer_delay_(steer_delay), - steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), - steer_dead_band_(steer_dead_band) +SimModelPymodels::SimModelPymodels(double dt) +: SimModelInterface(5 /* dim x */, 2 /* dim u */) { // TODO this should be in config file not hardcoded here @@ -92,7 +78,7 @@ double SimModelPymodels::getAx() } double SimModelPymodels::getWz() { - return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / 1.5; } double SimModelPymodels::getSteer() { @@ -125,18 +111,18 @@ Eigen::VectorXd SimModelPymodels::calcModel( // Not used for this model -> we can get rid of this later auto sat = [](double val, double u, double l) { return std::max(std::min(val, u), l); }; - const double vx = sat(state(IDX::VX), vx_lim_, -vx_lim_); - const double steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_); + const double vx = sat(state(IDX::VX), 50, -50); + const double steer = sat(state(IDX::STEER), 0, -0); const double yaw = state(IDX::YAW); const double delay_input_vx = input(IDX_U::VX_DES); - const double delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); - const double vx_rate = sat(-(vx - delay_vx_des) / vx_time_constant_, vx_rate_lim_, -vx_rate_lim_); + const double delay_vx_des = sat(delay_input_vx, 50, -50); + const double vx_rate = sat(-(vx - delay_vx_des), 0, -0); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = 0 * vx * cos(yaw); d_state(IDX::Y) = 0 * vx * sin(yaw); - d_state(IDX::YAW) = 0 * vx * std::tan(steer) / wheelbase_; + d_state(IDX::YAW) = 0 * vx * std::tan(steer) / 1.5; d_state(IDX::VX) = 0 * vx_rate; d_state(IDX::STEER) = 0.0; //Use python models to update steer From a6bdfc300f35b9186d1cc32ec2299453ee2c58dd Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 24 Jan 2024 15:53:05 +0100 Subject: [PATCH 22/97] Remove unused functions --- .../vehicle_model/sim_model_pymodels.hpp | 4 ++-- .../vehicle_model/sim_model_pymodels.cpp | 24 ------------------- 2 files changed, 2 insertions(+), 26 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index ded9550107381..a0e8f5ae73c3d 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -138,11 +138,11 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ void update(const double & dt) override; /** - * @brief calculate derivative of states with delay steering model + * @brief not used for this model. calculate derivative of states with delay steering model * @param [in] state current model state * @param [in] input input vector to model */ - Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; + Eigen::VectorXd calcModel([[maybe_unused]]const Eigen::VectorXd & state, [[maybe_unused]]const Eigen::VectorXd & input) override {return Eigen::VectorXd::Zero(dim_x_);} }; #endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 8efcdf94fec33..2289730185b44 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -104,27 +104,3 @@ void SimModelPymodels::update(const double & dt) current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; prev_vx_ = input_(IDX_U::VX_DES); } - -Eigen::VectorXd SimModelPymodels::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - // Not used for this model -> we can get rid of this later - auto sat = [](double val, double u, double l) { return std::max(std::min(val, u), l); }; - - const double vx = sat(state(IDX::VX), 50, -50); - const double steer = sat(state(IDX::STEER), 0, -0); - const double yaw = state(IDX::YAW); - const double delay_input_vx = input(IDX_U::VX_DES); - const double delay_vx_des = sat(delay_input_vx, 50, -50); - const double vx_rate = sat(-(vx - delay_vx_des), 0, -0); - - - Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); - d_state(IDX::X) = 0 * vx * cos(yaw); - d_state(IDX::Y) = 0 * vx * sin(yaw); - d_state(IDX::YAW) = 0 * vx * std::tan(steer) / 1.5; - d_state(IDX::VX) = 0 * vx_rate; - d_state(IDX::STEER) = 0.0; //Use python models to update steer - - return d_state; -} From 474dc37484ea3b7e1067c9f2e0b9fc799f2e4435 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 24 Jan 2024 16:44:24 +0100 Subject: [PATCH 23/97] Minor bugfixes --- .../include/pymodel_interconnected_model.hpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/simulator/learned_model/include/pymodel_interconnected_model.hpp b/simulator/learned_model/include/pymodel_interconnected_model.hpp index 7c1836685841a..ba5959850bd0c 100644 --- a/simulator/learned_model/include/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/pymodel_interconnected_model.hpp @@ -102,10 +102,12 @@ class InterconnectedModel } /** - * @brief create of signal names from all submodels + * @brief create of signal names from all sub-models and PSIM signal names */ - void getSignalNames() + void getSignalNames(std::vector in_names, std::vector out_names) { + addNamesToSigVec(in_names); + addNamesToSigVec(out_names); for (auto & submodel : submodels) { addNamesToSigVec(submodel->getInputNames()); addNamesToSigVec(submodel->getStateNames()); @@ -121,7 +123,7 @@ class InterconnectedModel void generateConnections(std::vector in_names, std::vector out_names) { // Create vector of signal names - getSignalNames(); + getSignalNames(in_names, out_names); num_signals = signals_vec_names.size(); // Init vector of signal values for (int i = 0; i < num_signals; i++) model_signals_vec.push_back(0); @@ -189,7 +191,7 @@ class InterconnectedModel } // Compute forward pass through all models (order should not matter) - std::vector model_signals_vec_next(num_signals); + std::vector model_signals_vec_next = model_signals_vec; for (auto & submodel : submodels) { model_signals_vec_next = submodel->getNextState(model_signals_vec, model_signals_vec_next); } From c3a9164e3f4fda7969367ada7a51b9ba2ba3c82c Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 24 Jan 2024 17:20:16 +0100 Subject: [PATCH 24/97] Add vy and yaw rate to model states --- .../vehicle_model/sim_model_pymodels.hpp | 8 ++++++-- .../simple_planning_simulator_core.cpp | 8 ++++++-- .../vehicle_model/sim_model_pymodels.cpp | 6 +++--- 3 files changed, 15 insertions(+), 7 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index a0e8f5ae73c3d..f590f5be5e48e 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -55,8 +55,10 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ std::vector state_names = {(char*)"POS_X", (char*)"POS_Y", - (char*)"YAW", - (char*)"VX", + (char*)"YAW", + (char*)"YAW_RATE", + (char*)"VX", + (char*)"VY", (char*)"STEER" }; @@ -68,7 +70,9 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ X = 0, Y, YAW, + YAW_RATE, VX, + VY, STEER, }; enum IDX_U { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index bcb7032de964d..651158004f2c1 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -565,7 +565,9 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & const double x = pose.position.x; const double y = pose.position.y; const double yaw = tf2::getYaw(pose.orientation); + const double yaw_rate = 0.0; const double vx = twist.linear.x; + const double vy = 0.0; const double steer = 0.0; const double accx = 0.0; @@ -578,9 +580,11 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED) { state << x, y, yaw, vx; } else if ( // NOLINT - vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL || - vehicle_model_type_ == VehicleModelType::PYMODELS) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL) + { state << x, y, yaw, vx, steer; + } else if (vehicle_model_type_ == VehicleModelType::PYMODELS){ + state << x, y, yaw, yaw_rate, vx, vy, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC || vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED || diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 2289730185b44..7abb76c30b905 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -19,7 +19,7 @@ #include "pymodel_interconnected_model.hpp" SimModelPymodels::SimModelPymodels(double dt) -: SimModelInterface(5 /* dim x */, 2 /* dim u */) +: SimModelInterface(7 /* dim x */, 2 /* dim u */) { // TODO this should be in config file not hardcoded here @@ -70,7 +70,7 @@ double SimModelPymodels::getVx() } double SimModelPymodels::getVy() { - return 0.0; + return state_(IDX::VY); } double SimModelPymodels::getAx() { @@ -78,7 +78,7 @@ double SimModelPymodels::getAx() } double SimModelPymodels::getWz() { - return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / 1.5; + return state_(IDX::YAW_RATE); } double SimModelPymodels::getSteer() { From 01bbf0010317ca74179387cf797945fe1ab13c78 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 24 Jan 2024 20:15:49 +0100 Subject: [PATCH 25/97] Add example to README and API doc (WIP) --- simulator/learned_model/README.md | 59 ++++++++++++++++++++++++++++++- 1 file changed, 58 insertions(+), 1 deletion(-) diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md index 57984a200468c..1605e330384a1 100644 --- a/simulator/learned_model/README.md +++ b/simulator/learned_model/README.md @@ -68,9 +68,66 @@ class CustomPythonSubmodel: -### Parameter description +To successfully create a vehicle model an InterconnectedModel class needs to be set up correctly. +### InterconnectedModel class +#### ```Constructor``` +Constructor takes no arguments. + +#### ```addSubmodel(model_desc)``` + +#### ```generateConnections(input_names, state_names)``` + +#### ```initState(current_state)``` + +#### ```updatePymodel(vehicle_input)``` + +### Example +Firstly we need to set up the model. +```C++ +InterconnectedModel vehicle; + +// Example of model descriptors +std::tuple model_desc_1 = { + (char*)"path_to_file_with_model_class_1", + (char*)nullptr, // If no param file is needed you can pass 'nullptr' + (char*)"ModelClass1" + }; + +std::tuple model_desc_2 = { + (char*)"path_to_file_with_model_class_2", + (char*)"/path_to/param_file", + (char*)"ModelClass2" // Name of the python class. Needs to use the interface from 'Assumptions' + } + +// Create sub-models based on descriptors +vehicle.addSubmodel(model_desc_1); +vehicle.addSubmodel(model_desc_2); + +// Define STATE and INPUT names of the system +std::vector state_names = {(char*)"STATE_NAME_1", (char*)"STATE_NAME_2"}; +std::vector input_names = {(char*)"INPUT_NAME_1", (char*)"INPUT_NAME_2"}; + +// Automatically connect sub-systems with model input +vehicle.generateConnections(input_names, state_names); +``` + +After the model is correctly set up, we can use it the following way. + +```C++ +// Example of an model input +std::vector vehicle_input = {0.0, 1.0}; // INPUT_NAME_1, INPUT_NAME_2 + +// Example of an model state +std::vector current_state = {0.2, 0.5}; // STATE_NAME_1, STATE_NAME_2 + +// Set model state +vehicle.initState(current_state); + +// Calculate the next state of the model +std::vector next_state = vehicle.updatePymodel(vehicle_input); +``` ## References / External links From dbe3aab78b9828c090445c954828afdbae118443 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Fri, 26 Jan 2024 15:57:19 +0100 Subject: [PATCH 26/97] Fix bug in CMakeList --- simulator/learned_model/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/learned_model/CMakeLists.txt b/simulator/learned_model/CMakeLists.txt index ecee90775790a..e6461f17f56f3 100644 --- a/simulator/learned_model/CMakeLists.txt +++ b/simulator/learned_model/CMakeLists.txt @@ -8,7 +8,7 @@ find_package(Python3 COMPONENTS Interpreter Development) find_package(pybind11 CONFIG) ament_auto_add_library(${PROJECT_NAME} SHARED - src/pymodel_interconnected_model.cpp + DIRECTORY src ) target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) target_include_directories(${PROJECT_NAME} PRIVATE ${Python3_INCLUDE_DIRS}) From e41d9558eae6d68792d43fefcfad2f9cb4a5e74e Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Fri, 26 Jan 2024 15:58:47 +0100 Subject: [PATCH 27/97] Fix import bugs and fix formatting --- .../model_connections_helpers.hpp | 4 + .../pymodel_interconnected_model.hpp | 50 ++++--- .../learned_model/pymodel_interface.hpp | 42 ++++++ .../pymodel_simple_model.hpp | 129 +++++++++--------- .../include/model_connections_helpers.hpp | 5 - .../include/pymodel_interface.hpp | 41 ------ .../src/model_connections_helpers.cpp | 2 +- .../learned_model/src/pymodel_base_error.cpp | 1 - .../src/pymodel_interconnected_model.cpp | 2 +- .../learned_model/src/pymodel_interface.cpp | 2 +- .../src/pymodel_simple_model.cpp | 2 +- .../vehicle_model/sim_model_pymodels.hpp | 33 ++--- .../vehicle_model/sim_model_pymodels.cpp | 42 ++---- 13 files changed, 177 insertions(+), 178 deletions(-) create mode 100644 simulator/learned_model/include/learned_model/model_connections_helpers.hpp rename simulator/learned_model/include/{ => learned_model}/pymodel_interconnected_model.hpp (85%) create mode 100644 simulator/learned_model/include/learned_model/pymodel_interface.hpp rename simulator/learned_model/include/{ => learned_model}/pymodel_simple_model.hpp (55%) delete mode 100644 simulator/learned_model/include/model_connections_helpers.hpp delete mode 100644 simulator/learned_model/include/pymodel_interface.hpp delete mode 100644 simulator/learned_model/src/pymodel_base_error.cpp diff --git a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp new file mode 100644 index 0000000000000..d43a69266dc37 --- /dev/null +++ b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp @@ -0,0 +1,4 @@ +#ifndef LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ +#define LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ + +#endif // LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_interconnected_model.hpp b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp similarity index 85% rename from simulator/learned_model/include/pymodel_interconnected_model.hpp rename to simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp index ba5959850bd0c..8c153c30c7605 100644 --- a/simulator/learned_model/include/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp @@ -1,13 +1,14 @@ -#ifndef LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ -#define LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ +#ifndef LEARNED_MODEL__PYMODEL_INTERCONNECTED_MODEL_HPP_ +#define LEARNED_MODEL__PYMODEL_INTERCONNECTED_MODEL_HPP_ -#include "pymodel_interface.hpp" -#include "pymodel_simple_model.hpp" -#include "model_connections_helpers.hpp" +#include "learned_model/model_connections_helpers.hpp" +#include "learned_model/pymodel_interface.hpp" +#include "learned_model/pymodel_simple_model.hpp" #include #include #include + #include namespace py = pybind11; @@ -21,11 +22,13 @@ class InterconnectedModel std::vector> submodels; - // index in "map_in_to_sig_vec" is index in "py_inputs" and value in "map_in_to_sig_vec" is index in "all_variables_names" - std::vector map_in_to_sig_vec; + // index in "map_in_to_sig_vec" is index in "py_inputs" and value in "map_in_to_sig_vec" is index + // in "all_variables_names" + std::vector map_in_to_sig_vec; - // index in "map_sig_vec_to_out" is index in "pymodel_outputs" and value in "map_sig_vec_to_out" is index in "all_variables_names" - std::vector map_sig_vec_to_out; + // index in "map_sig_vec_to_out" is index in "pymodel_outputs" and value in "map_sig_vec_to_out" + // is index in "all_variables_names" + std::vector map_sig_vec_to_out; public: py::scoped_interpreter guard{}; // start the interpreter and keep it alive @@ -48,7 +51,8 @@ class InterconnectedModel } private: - std::vector createConnectionsMap(std::vector connection_names_1, std::vector connection_names_2) + std::vector createConnectionsMap( + std::vector connection_names_1, std::vector connection_names_2) { std::vector connection_map; // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is @@ -73,7 +77,8 @@ class InterconnectedModel */ void mapInputs(std::vector in_names) { - // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index in "signals_vec_names" + // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index + // in "signals_vec_names" map_in_to_sig_vec = createConnectionsMap(signals_vec_names, in_names); } @@ -83,7 +88,8 @@ class InterconnectedModel */ void mapOutputs(std::vector out_names) { - // index in "map_sig_vec_to_out" is index in "out_names" and value in "map_sig_vec_to_out" is index in "signals_vec_names" + // index in "map_sig_vec_to_out" is index in "out_names" and value in "map_sig_vec_to_out" is + // index in "signals_vec_names" map_sig_vec_to_out = createConnectionsMap(signals_vec_names, out_names); } @@ -95,14 +101,16 @@ class InterconnectedModel { // Check if the name is already in the vector. If not add it. for (char * name : names) { - if (std::find(signals_vec_names.begin(), signals_vec_names.end(), name) == signals_vec_names.end()) { + if ( + std::find(signals_vec_names.begin(), signals_vec_names.end(), name) == + signals_vec_names.end()) { signals_vec_names.push_back(name); } } } - + /** - * @brief create of signal names from all sub-models and PSIM signal names + * @brief create of signal names from all sub-models and PSIM signal names */ void getSignalNames(std::vector in_names, std::vector out_names) { @@ -151,7 +159,8 @@ class InterconnectedModel } /** - * @brief set a new model state if it was changed using PSIM interface (mainly position and orientation) + * @brief set a new model state if it was changed using PSIM interface (mainly position and + * orientation) * @param [in] new_state new state set by PSIM */ void initState(std::vector new_state) @@ -160,7 +169,9 @@ class InterconnectedModel // Check if some state was changed externally for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { - if (abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > 1e-6) { + if ( + abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > + 1e-6) { state_changed_externally = true; break; } @@ -170,7 +181,8 @@ class InterconnectedModel // Reinitialize model std::cout << "Reseting model" << std::endl; - // Currently initializing model to zero -> TODO find a way how to initialize them to some other default values + // Currently initializing model to zero -> TODO find a way how to initialize them to some + // other default values std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { @@ -209,4 +221,4 @@ class InterconnectedModel } }; -#endif // LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ \ No newline at end of file +#endif // LEARNED_MODEL__PYMODEL_INTERCONNECTED_MODEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/learned_model/pymodel_interface.hpp b/simulator/learned_model/include/learned_model/pymodel_interface.hpp new file mode 100644 index 0000000000000..0f0df0ea5be95 --- /dev/null +++ b/simulator/learned_model/include/learned_model/pymodel_interface.hpp @@ -0,0 +1,42 @@ +#ifndef LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ +#define LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ + +#include + +class PymodelInterface +{ +public: + // virtual ~PymodelInterface() = 0; // <-- build fails after uncommenting this + + /** + * @brief get names of inputs of python model + */ + virtual std::vector getInputNames() = 0; + + /** + * @brief get names of states of python model + */ + virtual std::vector getStateNames() = 0; + + /** + * @brief create a map from model signal vector to python model inputs + * @param [in] signal_vec_names names of signals in model signal vector + */ + virtual void mapInputs(std::vector signals_vec_names) = 0; + + /** + * @brief create a map from python outputs to model signal vector + * @param [in] signal_vec_names names of signals in model signal vector + */ + virtual void mapOutputs(std::vector signals_vec_names) = 0; + + /** + * @brief calculate the next state of this submodule + * @param [in] model_signals_vec values of signals in model signal vector + * @param [in] model_signals_vec_next values of signals in model signal vector to update + */ + virtual std::vector getNextState( + std::vector model_signals_vec, std::vector model_signals_vec_next) = 0; +}; + +#endif // LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_simple_model.hpp b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp similarity index 55% rename from simulator/learned_model/include/pymodel_simple_model.hpp rename to simulator/learned_model/include/learned_model/pymodel_simple_model.hpp index d674876d2b2ba..3993cb44b066d 100644 --- a/simulator/learned_model/include/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp @@ -1,12 +1,12 @@ #ifndef LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ #define LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ +#include "learned_model/model_connections_helpers.hpp" +#include "learned_model/pymodel_interface.hpp" + #include #include -#include "pymodel_interface.hpp" -#include "model_connections_helpers.hpp" - namespace py = pybind11; /** @@ -16,18 +16,22 @@ namespace py = pybind11; class PymodelSimpleModel : public PymodelInterface { private: - char* pymodel_import_name = nullptr; - + char * pymodel_import_name = nullptr; + int num_inputs_py; int num_outputs_py; py::object py_model_class; - std::vector map_sig_vec_to_pyin; // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is index in "signals_vec_names" - std::vector map_pyout_to_sig_vec; // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in "map_pyout_to_sig_vec" is index in "signals_vec_names" + std::vector + map_sig_vec_to_pyin; // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in + // "map_sig_vec_to_pyin" is index in "signals_vec_names" + std::vector + map_pyout_to_sig_vec; // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and + // value in "map_pyout_to_sig_vec" is index in "signals_vec_names" - std::vector pymodel_input_names; - std::vector pymodel_state_names; + std::vector pymodel_input_names; + std::vector pymodel_state_names; public: /** @@ -36,47 +40,45 @@ class PymodelSimpleModel : public PymodelInterface * @param [in] param_file_path path to saved parameter file of the python sub-model * @param [in] py_class_name name of the python class */ - PymodelSimpleModel(char* pymodel_import_name_, char* param_file_path, char* py_class_name) + PymodelSimpleModel(char * pymodel_import_name_, char * param_file_path, char * py_class_name) { // Import model class pymodel_import_name = pymodel_import_name_; - if (pymodel_import_name != nullptr) - { - // Import python module - py::module_ imported_module = py::module_::import(pymodel_import_name); - // Initialize model class from imported module - py_model_class = imported_module.attr(py_class_name)(); - } - else - { - // TODO throw exception/error - return; + if (pymodel_import_name != nullptr) { + // Import python module + py::module_ imported_module = py::module_::import(pymodel_import_name); + // Initialize model class from imported module + py_model_class = imported_module.attr(py_class_name)(); + } else { + // TODO throw exception/error + return; } // Load model parameters and reset the model - if (param_file_path != nullptr) - { - py::object load_params_succ = py_model_class.attr("load_params")(param_file_path); - py_model_class.attr("reset")(); - std::cout << "Params loaded" << std::endl; - } - else - { - // TODO warning that using default model params + if (param_file_path != nullptr) { + py::object load_params_succ = py_model_class.attr("load_params")(param_file_path); + py_model_class.attr("reset")(); + std::cout << "Params loaded" << std::endl; + } else { + // TODO warning that using default model params } - - // Get string names of states of python model, convert them to C++ string and store them in pymodel_state_names + + // Get string names of states of python model, convert them to C++ string and store them in + // pymodel_state_names py::list pymodel_state_names_ = py_model_class.attr("get_state_names")(); num_outputs_py = pymodel_state_names_.size(); - for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++){ - pymodel_state_names.push_back(PyBytes_AS_STRING(PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); + for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++) { + pymodel_state_names.push_back(PyBytes_AS_STRING( + PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); } - // Get string names of actions (inputs) of python model, convert them to C++ string and store them in pymodel_input_names + // Get string names of actions (inputs) of python model, convert them to C++ string and store + // them in pymodel_input_names py::list pymodel_input_names_ = py_model_class.attr("get_action_names")(); num_inputs_py = pymodel_input_names_.size(); - for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++){ - pymodel_input_names.push_back(PyBytes_AS_STRING(PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); + for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++) { + pymodel_input_names.push_back(PyBytes_AS_STRING( + PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); } std::cout << "PymodelInterface import name: " << pymodel_import_name << std::endl; @@ -86,21 +88,22 @@ class PymodelSimpleModel : public PymodelInterface * @brief calculate the next state of a python model * @param [in] model_signals_vec all available inputs from PSIM */ - std::vector getNextState(std::vector model_signals_vec, std::vector model_signals_vec_next) override + std::vector getNextState( + std::vector model_signals_vec, std::vector model_signals_vec_next) override { - // get inputs and states of the python model from the vector of signals std::vector py_inputs(num_inputs_py); std::vector py_state(num_outputs_py); py_inputs = fillVectorUsingMap(py_inputs, model_signals_vec, map_sig_vec_to_pyin, true); py_state = fillVectorUsingMap(py_state, model_signals_vec, map_pyout_to_sig_vec, true); - + // forward pass through the base model py::tuple res = py_model_class.attr("forward")(py_inputs, py_state); std::vector py_state_next = res.cast>(); // map outputs from python model to required outputs - std::vector next_state = fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); + std::vector next_state = + fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); return next_state; } @@ -108,47 +111,47 @@ class PymodelSimpleModel : public PymodelInterface /** * @brief get names of inputs of python model */ - std::vector getInputNames() override - { - return pymodel_input_names; - } + std::vector getInputNames() override { return pymodel_input_names; } /** * @brief get names of states of python model */ - std::vector getStateNames() override - { - return pymodel_state_names; - } + std::vector getStateNames() override { return pymodel_state_names; } /** * @brief create a map from model signal vector to python model inputs * @param [in] signal_vec_names names of signals in model signal vector */ - void mapInputs(std::vector signals_vec_names) override + void mapInputs(std::vector signals_vec_names) override { - // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is index in "signals_vec_names" + // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is + // index in "signals_vec_names" map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); } - + /** * @brief create a map from python outputs to model signal vector * @param [in] signal_vec_names names of signals in model signal vector */ - void mapOutputs(std::vector signals_vec_names) override + void mapOutputs(std::vector signals_vec_names) override { - // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in "map_pyout_to_sig_vec" is index in "signals_vec_names" + // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in + // "map_pyout_to_sig_vec" is index in "signals_vec_names" map_pyout_to_sig_vec = createConnectionsMap(signals_vec_names, pymodel_state_names); } private: - std::vector createConnectionsMap(std::vector connection_names_1, std::vector connection_names_2){ + std::vector createConnectionsMap( + std::vector connection_names_1, std::vector connection_names_2) + { std::vector connection_map; - // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is index in "connection_names_1" - for (auto name_2 : connection_names_2){ + // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is + // index in "connection_names_1" + for (auto name_2 : connection_names_2) { int mapped_idx = -1; // -1 means that we cannot create a connection between some signals - for (size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++){ - if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0){ // 0 means strings are the same and we can create connection + for (size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { + if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same + // and we can create connection mapped_idx = NAME_1_IDX; break; } @@ -158,17 +161,17 @@ class PymodelSimpleModel : public PymodelInterface return connection_map; } - std::vector fillVectorUsingMap(std::vector vector1, std::vector vector2, std::vector map, bool inverse){ + std::vector fillVectorUsingMap( + std::vector vector1, std::vector vector2, std::vector map, bool inverse) + { // index in "map" is index in "vector1" and value in "map" is index in "vector2" // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 - for(size_t idx = 0; idx < map.size(); idx++) - { + for (size_t idx = 0; idx < map.size(); idx++) { if (map[idx] == -1) continue; inverse ? vector1[idx] = vector2[map[idx]] : vector2[map[idx]] = vector1[idx]; } return inverse ? vector1 : vector2; } - }; #endif // LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/model_connections_helpers.hpp b/simulator/learned_model/include/model_connections_helpers.hpp deleted file mode 100644 index 0acdc60333e20..0000000000000 --- a/simulator/learned_model/include/model_connections_helpers.hpp +++ /dev/null @@ -1,5 +0,0 @@ -#ifndef LEARNED_MODEL__MODEL_CONNECTION_HELPERS_ -#define LEARNED_MODEL__MODEL_CONNECTION_HELPERS_ - - -#endif // LEARNED_MODEL__MODEL_CONNECTION_HELPERS_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_interface.hpp b/simulator/learned_model/include/pymodel_interface.hpp deleted file mode 100644 index e1c7fe694d056..0000000000000 --- a/simulator/learned_model/include/pymodel_interface.hpp +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ -#define LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ - -#include - -class PymodelInterface - { - public: - // virtual ~PymodelInterface() = 0; // <-- build fails after uncommenting this - - /** - * @brief get names of inputs of python model - */ - virtual std::vector getInputNames() = 0; - - /** - * @brief get names of states of python model - */ - virtual std::vector getStateNames() = 0; - - /** - * @brief create a map from model signal vector to python model inputs - * @param [in] signal_vec_names names of signals in model signal vector - */ - virtual void mapInputs(std::vector signals_vec_names) = 0; - - /** - * @brief create a map from python outputs to model signal vector - * @param [in] signal_vec_names names of signals in model signal vector - */ - virtual void mapOutputs(std::vector signals_vec_names) = 0; - - /** - * @brief calculate the next state of this submodule - * @param [in] model_signals_vec values of signals in model signal vector - * @param [in] model_signals_vec_next values of signals in model signal vector to update - */ - virtual std::vector getNextState(std::vector model_signals_vec, std::vector model_signals_vec_next) = 0; -}; - -#endif // LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/src/model_connections_helpers.cpp b/simulator/learned_model/src/model_connections_helpers.cpp index 70349b80c9796..05b14720b8395 100644 --- a/simulator/learned_model/src/model_connections_helpers.cpp +++ b/simulator/learned_model/src/model_connections_helpers.cpp @@ -1 +1 @@ -#include "../include/model_connections_helpers.hpp" \ No newline at end of file +#include "learned_model/model_connections_helpers.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_base_error.cpp b/simulator/learned_model/src/pymodel_base_error.cpp deleted file mode 100644 index f829bd20bbe64..0000000000000 --- a/simulator/learned_model/src/pymodel_base_error.cpp +++ /dev/null @@ -1 +0,0 @@ -#include "../include/pymodel_base_error.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_interconnected_model.cpp b/simulator/learned_model/src/pymodel_interconnected_model.cpp index fac2a02ca5d68..00fdc4eae75e4 100644 --- a/simulator/learned_model/src/pymodel_interconnected_model.cpp +++ b/simulator/learned_model/src/pymodel_interconnected_model.cpp @@ -1 +1 @@ -#include "../include/pymodel_interconnected_model.hpp" \ No newline at end of file +#include "learned_model/pymodel_interconnected_model.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_interface.cpp b/simulator/learned_model/src/pymodel_interface.cpp index 302ef331fd00a..edd0c0d2483d6 100644 --- a/simulator/learned_model/src/pymodel_interface.cpp +++ b/simulator/learned_model/src/pymodel_interface.cpp @@ -1 +1 @@ -#include "../include/pymodel_interface.hpp" \ No newline at end of file +#include "learned_model/pymodel_interface.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_simple_model.cpp b/simulator/learned_model/src/pymodel_simple_model.cpp index f3a11c3a8aa7d..26f1b0963fa89 100644 --- a/simulator/learned_model/src/pymodel_simple_model.cpp +++ b/simulator/learned_model/src/pymodel_simple_model.cpp @@ -1 +1 @@ -#include "../include/pymodel_simple_model.hpp" \ No newline at end of file +#include "learned_model/pymodel_simple_model.hpp" \ No newline at end of file diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index f590f5be5e48e..d49f7f727c555 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -15,8 +15,8 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ +#include "learned_model/pymodel_interconnected_model.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include "pymodel_interconnected_model.hpp" #include #include @@ -47,24 +47,16 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ ~SimModelPymodels() = default; private: - - /* - Specify string names for states and inputs. So we can automatically map states and + /* + Specify string names for states and inputs. So we can automatically map states and inputs of this model to states and inputs of the python model. */ - std::vector state_names = {(char*)"POS_X", - (char*)"POS_Y", - (char*)"YAW", - (char*)"YAW_RATE", - (char*)"VX", - (char*)"VY", - (char*)"STEER" - }; + std::vector state_names = {(char *)"POS_X", (char *)"POS_Y", (char *)"YAW", + (char *)"YAW_RATE", (char *)"VX", (char *)"VY", + (char *)"STEER"}; - std::vector input_names = {(char*)"VX_DES", - (char*)"STEER_DES" - }; + std::vector input_names = {(char *)"VX_DES", (char *)"STEER_DES"}; enum IDX { X = 0, @@ -88,7 +80,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ std::deque vx_input_queue_; //!< @brief buffer for velocity command std::deque steer_input_queue_; //!< @brief buffer for angular velocity command - + /** * @brief set queue buffer for input command * @param [in] dt delta time @@ -146,7 +138,12 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ * @param [in] state current model state * @param [in] input input vector to model */ - Eigen::VectorXd calcModel([[maybe_unused]]const Eigen::VectorXd & state, [[maybe_unused]]const Eigen::VectorXd & input) override {return Eigen::VectorXd::Zero(dim_x_);} + Eigen::VectorXd calcModel( + [[maybe_unused]] const Eigen::VectorXd & state, + [[maybe_unused]] const Eigen::VectorXd & input) override + { + return Eigen::VectorXd::Zero(dim_x_); + } }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ \ No newline at end of file diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 7abb76c30b905..5be847b5767b2 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -14,37 +14,25 @@ #include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" -#include +#include "learned_model/pymodel_interconnected_model.hpp" -#include "pymodel_interconnected_model.hpp" +#include -SimModelPymodels::SimModelPymodels(double dt) -: SimModelInterface(7 /* dim x */, 2 /* dim u */) +SimModelPymodels::SimModelPymodels(double dt) : SimModelInterface(7 /* dim x */, 2 /* dim u */) { - // TODO this should be in config file not hardcoded here // Think of a way how to differentiate between "simple" model and "base + error" model - std::vector> model_descriptors = { - { - (char*)"control_analysis_pipeline.autoware_models.vehicle.kinematic", - (char*)nullptr, - (char*)"KinematicModel" - }, - { - (char*)"control_analysis_pipeline.autoware_models.steering.example_base_error", - (char*)"$HOME/autoware_model_params/base_model_save", - (char*)"BaseError" - }, - { - (char*)"control_analysis_pipeline.autoware_models.drive.drive_example", - (char*)nullptr, - (char*)"DriveExample" - } - }; - - vehicle.addSubmodel(model_desc[0]); - vehicle.addSubmodel(model_desc[1]); - vehicle.addSubmodel(model_desc[2]); + std::vector> model_descriptors = { + {(char *)"control_analysis_pipeline.autoware_models.vehicle.kinematic", (char *)nullptr, + (char *)"KinematicModel"}, + {(char *)"control_analysis_pipeline.autoware_models.steering.example_base_error", + (char *)"$HOME/autoware_model_params/base_model_save", (char *)"BaseError"}, + {(char *)"control_analysis_pipeline.autoware_models.drive.drive_example", (char *)nullptr, + (char *)"DriveExample"}}; + + vehicle.addSubmodel(model_descriptors[0]); + vehicle.addSubmodel(model_descriptors[1]); + vehicle.addSubmodel(model_descriptors[2]); vehicle.generateConnections(input_names, state_names); @@ -103,4 +91,4 @@ void SimModelPymodels::update(const double & dt) // Calculate current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; prev_vx_ = input_(IDX_U::VX_DES); -} +} \ No newline at end of file From 39b9de4a9e152fcd4b773d0c3e6dbe6f535ed97e Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Sat, 27 Jan 2024 13:46:36 +0100 Subject: [PATCH 28/97] Add model descriptor to ros2 parameter file --- .../vehicle_model/sim_model_pymodels.hpp | 5 ++++- .../simple_planning_simulator_core.cpp | 9 ++++++++- .../vehicle_model/sim_model_pymodels.cpp | 6 +++++- 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index d49f7f727c555..f75669f54e5ff 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -39,7 +39,10 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ * @brief constructor * @param [in] dt delta time information to set input buffer for delay */ - SimModelPymodels(double dt); + SimModelPymodels(double dt, + std::vector model_python_paths, + std::vector model_param_paths, + std::vector model_class_names); /** * @brief destructor diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 651158004f2c1..5763c30513e47 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -236,6 +236,10 @@ void SimplePlanningSimulator::initialize_vehicle_model() const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; + std::vector model_python_paths = declare_parameter>("model_python_paths", std::vector({""})); + std::vector model_param_paths = declare_parameter>("model_param_paths", std::vector({""})); + std::vector model_class_names = declare_parameter>("model_class_names", std::vector({""})); + if (vehicle_model_type_str == "IDEAL_STEER_VEL") { vehicle_model_type_ = VehicleModelType::IDEAL_STEER_VEL; vehicle_model_ptr_ = std::make_shared(wheelbase); @@ -280,7 +284,10 @@ void SimplePlanningSimulator::initialize_vehicle_model() acceleration_map_path); } else if (vehicle_model_type_str == "PYMODELS"){ vehicle_model_type_ = VehicleModelType::PYMODELS; - vehicle_model_ptr_ = std::make_shared(timer_sampling_time_ms_ / 1000.0); + + vehicle_model_ptr_ = std::make_shared(timer_sampling_time_ms_ / 1000.0, + model_python_paths, model_param_paths, model_class_names); + }else{ throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 5be847b5767b2..092b7a74723d5 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -18,7 +18,11 @@ #include -SimModelPymodels::SimModelPymodels(double dt) : SimModelInterface(7 /* dim x */, 2 /* dim u */) +SimModelPymodels::SimModelPymodels(double dt, + [[maybe_unused]] std::vector model_python_paths, + [[maybe_unused]] std::vector model_param_paths, + [[maybe_unused]] std::vector model_class_names + ) : SimModelInterface(7 /* dim x */, 2 /* dim u */) { // TODO this should be in config file not hardcoded here // Think of a way how to differentiate between "simple" model and "base + error" model From cc057e0c6d4ccca4c943844902cff5a0df6f50d0 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Sat, 27 Jan 2024 13:47:30 +0100 Subject: [PATCH 29/97] Change char* to std::string in model descriptor --- .../pymodel_interconnected_model.hpp | 2 +- .../learned_model/pymodel_simple_model.hpp | 14 +++++++------- .../vehicle_model/sim_model_pymodels.cpp | 18 ++++++------------ 3 files changed, 14 insertions(+), 20 deletions(-) diff --git a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp index 8c153c30c7605..1565480af68be 100644 --- a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp @@ -151,7 +151,7 @@ class InterconnectedModel * @brief add a sub-model consisting of base + error model * @param [in] submodel_desc descriptor of the sub-model */ - void addSubmodel(std::tuple submodel_desc) + void addSubmodel(std::tuple submodel_desc) { const auto [lib_path, param_path, class_name] = submodel_desc; auto new_model = new PymodelSimpleModel(lib_path, param_path, class_name); diff --git a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp index 3993cb44b066d..eb682ddd1d809 100644 --- a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp @@ -16,7 +16,7 @@ namespace py = pybind11; class PymodelSimpleModel : public PymodelInterface { private: - char * pymodel_import_name = nullptr; + std::string pymodel_import_name; int num_inputs_py; int num_outputs_py; @@ -40,23 +40,23 @@ class PymodelSimpleModel : public PymodelInterface * @param [in] param_file_path path to saved parameter file of the python sub-model * @param [in] py_class_name name of the python class */ - PymodelSimpleModel(char * pymodel_import_name_, char * param_file_path, char * py_class_name) + PymodelSimpleModel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name) { // Import model class pymodel_import_name = pymodel_import_name_; - if (pymodel_import_name != nullptr) { + if (!pymodel_import_name.empty()) { // Import python module - py::module_ imported_module = py::module_::import(pymodel_import_name); + py::module_ imported_module = py::module_::import(pymodel_import_name.c_str()); // Initialize model class from imported module - py_model_class = imported_module.attr(py_class_name)(); + py_model_class = imported_module.attr(py_class_name.c_str())(); } else { // TODO throw exception/error return; } // Load model parameters and reset the model - if (param_file_path != nullptr) { - py::object load_params_succ = py_model_class.attr("load_params")(param_file_path); + if (!param_file_path.empty()) { + py::object load_params_succ = py_model_class.attr("load_params")(param_file_path.c_str()); py_model_class.attr("reset")(); std::cout << "Params loaded" << std::endl; } else { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 092b7a74723d5..ff8e578da3bc7 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -24,19 +24,13 @@ SimModelPymodels::SimModelPymodels(double dt, [[maybe_unused]] std::vector model_class_names ) : SimModelInterface(7 /* dim x */, 2 /* dim u */) { - // TODO this should be in config file not hardcoded here - // Think of a way how to differentiate between "simple" model and "base + error" model - std::vector> model_descriptors = { - {(char *)"control_analysis_pipeline.autoware_models.vehicle.kinematic", (char *)nullptr, - (char *)"KinematicModel"}, - {(char *)"control_analysis_pipeline.autoware_models.steering.example_base_error", - (char *)"$HOME/autoware_model_params/base_model_save", (char *)"BaseError"}, - {(char *)"control_analysis_pipeline.autoware_models.drive.drive_example", (char *)nullptr, - (char *)"DriveExample"}}; - vehicle.addSubmodel(model_descriptors[0]); - vehicle.addSubmodel(model_descriptors[1]); - vehicle.addSubmodel(model_descriptors[2]); + for (size_t i = 0; i < model_python_paths.size(); i++){ + std::tuple descriptor = { + model_python_paths[i], model_param_paths[i], model_class_names[i] + }; + vehicle.addSubmodel(descriptor); + } vehicle.generateConnections(input_names, state_names); From 7eba8c1640e23a9fb2ba509a407e8892fb6df967 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Sat, 27 Jan 2024 13:47:57 +0100 Subject: [PATCH 30/97] Minor code cleanup --- .../vehicle_model/sim_model_pymodels.hpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index f75669f54e5ff..04a5329ff696f 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -76,7 +76,6 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ }; InterconnectedModel vehicle; - // py::scoped_interpreter guard{}; double prev_vx_ = 0.0; double current_ax_ = 0.0; @@ -84,12 +83,6 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ std::deque vx_input_queue_; //!< @brief buffer for velocity command std::deque steer_input_queue_; //!< @brief buffer for angular velocity command - /** - * @brief set queue buffer for input command - * @param [in] dt delta time - */ - void initializeInputQueue(const double & dt); - /** * @brief get vehicle position x */ From dfeb12af66ce80f2baf2e57b29397f67f3d87845 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 29 Jan 2024 13:40:20 +0100 Subject: [PATCH 31/97] Minor code cleanup --- .../vehicle_model/sim_model_pymodels.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index ff8e578da3bc7..51753722b85cf 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -19,12 +19,11 @@ #include SimModelPymodels::SimModelPymodels(double dt, - [[maybe_unused]] std::vector model_python_paths, - [[maybe_unused]] std::vector model_param_paths, - [[maybe_unused]] std::vector model_class_names + std::vector model_python_paths, + std::vector model_param_paths, + std::vector model_class_names ) : SimModelInterface(7 /* dim x */, 2 /* dim u */) { - for (size_t i = 0; i < model_python_paths.size(); i++){ std::tuple descriptor = { model_python_paths[i], model_param_paths[i], model_class_names[i] From 602eadf37cbe84d898e54de40f9df71cb4421906 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 29 Jan 2024 13:42:07 +0100 Subject: [PATCH 32/97] Add method to set time stamp from C++ --- simulator/learned_model/README.md | 13 +++++++++++++ .../learned_model/pymodel_interconnected_model.hpp | 11 +++++++++++ .../include/learned_model/pymodel_interface.hpp | 8 +++++++- .../include/learned_model/pymodel_simple_model.hpp | 6 ++++++ .../vehicle_model/sim_model_pymodels.cpp | 3 ++- 5 files changed, 39 insertions(+), 2 deletions(-) diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md index 1605e330384a1..3ffb4a2d8c254 100644 --- a/simulator/learned_model/README.md +++ b/simulator/learned_model/README.md @@ -60,6 +60,14 @@ class CustomPythonSubmodel: - path: Path to a parameter file to load by the model. """ pass + + def dtSet(self, dt): + """ + Set dt of the model. + Inputs: + - dt: time step + """ + pass ``` ## API @@ -83,6 +91,8 @@ Constructor takes no arguments. #### ```updatePymodel(vehicle_input)``` +#### ```dtSet(dt)``` + ### Example Firstly we need to set up the model. ```C++ @@ -111,6 +121,9 @@ std::vector input_names = {(char*)"INPUT_NAME_1", (char*)"INPUT_NAME_2"}; // Automatically connect sub-systems with model input vehicle.generateConnections(input_names, state_names); + +// Set the time step of the model +vehicle.dtSet(dt); ``` After the model is correctly set up, we can use it the following way. diff --git a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp index 1565480af68be..f98de77432f41 100644 --- a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp @@ -191,6 +191,17 @@ class InterconnectedModel } } + /** + * @brief set time step for all the models + * @param [in] dt time step + */ + void dtSet(double dt) + { + for (auto & submodel : submodels) { + submodel->dtSet(dt); + } + } + /** * @brief compute next step of the PSIM model using python sub-models * @param [in] psim_input vector of input values provided by PSIM diff --git a/simulator/learned_model/include/learned_model/pymodel_interface.hpp b/simulator/learned_model/include/learned_model/pymodel_interface.hpp index 0f0df0ea5be95..8f92bbd94d60f 100644 --- a/simulator/learned_model/include/learned_model/pymodel_interface.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_interface.hpp @@ -6,7 +6,13 @@ class PymodelInterface { public: - // virtual ~PymodelInterface() = 0; // <-- build fails after uncommenting this + // virtual ~PymodelInterface() = 0; + + /** + * @brief set time step of the model + * @param [in] dt time step + */ + virtual void dtSet(double dt) = 0; /** * @brief get names of inputs of python model diff --git a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp index eb682ddd1d809..280d0cc95325a 100644 --- a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp @@ -108,6 +108,12 @@ class PymodelSimpleModel : public PymodelInterface return next_state; } + /** + * @brief set time step of the model + * @param [in] dt time step + */ + void dtSet(double dt) override { py_model_class.attr("dtSet")(dt); } + /** * @brief get names of inputs of python model */ diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 51753722b85cf..e0e2f577cee83 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -33,7 +33,8 @@ SimModelPymodels::SimModelPymodels(double dt, vehicle.generateConnections(input_names, state_names); - std::cout << dt << std::endl; + vehicle.dtSet(dt); + std::cout << "Python model loaded successfully " << std::endl; } From ed65c60d0c0fa44d642f4c5fa33cd29b5b9582f9 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 29 Jan 2024 16:09:07 +0100 Subject: [PATCH 33/97] Update README --- simulator/learned_model/README.md | 73 ++++++++++++++++++++++++------- 1 file changed, 56 insertions(+), 17 deletions(-) diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md index 3ffb4a2d8c254..0e1b9e9299388 100644 --- a/simulator/learned_model/README.md +++ b/simulator/learned_model/README.md @@ -27,7 +27,7 @@ Using this Python model interface a PSIM model consisting of sub-models implemen To use this package `python3` and `pybind11` need to be installed. The only assumption on Python sub-models is their interface (see below). ```python -class CustomPythonSubmodel: +class PythonSubmodelInterface: def forward(self, action, state): # Required """ @@ -53,7 +53,7 @@ class CustomPythonSubmodel: """ pass - def load_params(self, path): # Optional + def load_params(self, path): # Required """ Load parameters of the model. Inputs: @@ -61,7 +61,7 @@ class CustomPythonSubmodel: """ pass - def dtSet(self, dt): + def dtSet(self, dt): # Required """ Set dt of the model. Inputs: @@ -83,15 +83,54 @@ To successfully create a vehicle model an InterconnectedModel class needs to be #### ```Constructor``` Constructor takes no arguments. -#### ```addSubmodel(model_desc)``` +#### ```void addSubmodel(std::tuple model_descriptor)``` +Add a new sub-model to the model. -#### ```generateConnections(input_names, state_names)``` +Inputs: +* model_descriptor: Describes what model should be used. Model descriptor contains three strings: + * First string is a path to a file where the model is implemented. + * Second string is a path to the file where model parameters are stored + * Third string is a name of the class that implement the model. -#### ```initState(current_state)``` +Outputs: +* None -#### ```updatePymodel(vehicle_input)``` +#### ```void generateConnections(std::vector in_names, std::vector out_names)``` +Generate connections between sub-models and inputs/outputs of the model. -#### ```dtSet(dt)``` +Inputs: +* in_names: String names for all of the model inputs in order. +* out_names: String names for all of the model outputs in order. + +Outputs: +* None + +#### ```void initState(std::vector new_state)``` +Set the initial state of the model. + +Inputs: +* new_state: New state of the model. + +Outputs: +* None + +#### ```std::vector updatePymodel(std::vector psim_input)``` +Calculate the next state of the model by calculating the next state of all of the sub-models. + +Inputs: +* psim_input: Input to the model. + +Outputs: +* next_state: Next state of the model. + +#### ```dtSet(double dt)``` +Set the time step of the model. + +Inputs: +* dt: time step + +Outputs: +* None ### Example Firstly we need to set up the model. @@ -99,21 +138,21 @@ Firstly we need to set up the model. InterconnectedModel vehicle; // Example of model descriptors -std::tuple model_desc_1 = { +std::tuple model_descriptor_1 = { (char*)"path_to_file_with_model_class_1", (char*)nullptr, // If no param file is needed you can pass 'nullptr' (char*)"ModelClass1" - }; + }; -std::tuple model_desc_2 = { - (char*)"path_to_file_with_model_class_2", - (char*)"/path_to/param_file", - (char*)"ModelClass2" // Name of the python class. Needs to use the interface from 'Assumptions' - } +std::tuple model_descriptor_2 = { + (char*)"path_to_file_with_model_class_2", + (char*)"/path_to/param_file", + (char*)"ModelClass2" // Name of the python class. Needs to use the interface from 'Assumptions' + }; // Create sub-models based on descriptors -vehicle.addSubmodel(model_desc_1); -vehicle.addSubmodel(model_desc_2); +vehicle.addSubmodel(model_descriptor_1); +vehicle.addSubmodel(model_descriptor_2); // Define STATE and INPUT names of the system std::vector state_names = {(char*)"STATE_NAME_1", (char*)"STATE_NAME_2"}; From 362a536aea0fe7857309897b4d66e49596285c6f Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 30 Jan 2024 14:22:45 +0100 Subject: [PATCH 34/97] Split implementation between .hpp and .cpp files --- .../model_connections_helpers.hpp | 35 +++++ .../pymodel_interconnected_model.hpp | 142 ++---------------- .../learned_model/pymodel_simple_model.hpp | 115 +------------- .../src/pymodel_interconnected_model.cpp | 125 ++++++++++++++- .../src/pymodel_simple_model.cpp | 86 ++++++++++- 5 files changed, 260 insertions(+), 243 deletions(-) diff --git a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp index d43a69266dc37..264311e0bc29f 100644 --- a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp +++ b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp @@ -1,4 +1,39 @@ #ifndef LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ #define LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ +#include +#include + +std::vector fillVectorUsingMap( + std::vector vector1, std::vector vector2, std::vector map, bool inverse) + { + // index in "map" is index in "vector1" and value in "map" is index in "vector2" + // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 + for (std::size_t idx = 0; idx < map.size(); idx++) { + if (map[idx] == -1) continue; + inverse ? vector1[idx] = vector2[map[idx]] : vector2[map[idx]] = vector1[idx]; + } + return inverse ? vector1 : vector2; + } + +std::vector createConnectionsMap( + std::vector connection_names_1, std::vector connection_names_2) + { + std::vector connection_map; + // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is + // index in "connection_names_1" + for (auto name_2 : connection_names_2) { + int mapped_idx = -1; // -1 means that we cannot create a connection between some signals + for (std::size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { + if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same + // and we can create connection + mapped_idx = NAME_1_IDX; + break; + } + } + connection_map.push_back(mapped_idx); + } + return connection_map; + } + #endif // LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp index f98de77432f41..e9f34dbe33aa7 100644 --- a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp @@ -51,76 +51,28 @@ class InterconnectedModel } private: - std::vector createConnectionsMap( - std::vector connection_names_1, std::vector connection_names_2) - { - std::vector connection_map; - // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is - // index in "connection_names_1" - for (auto name_2 : connection_names_2) { - int mapped_idx = -1; // -1 means that we cannot create a connection between some signals - for (size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { - if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same - // and we can create connection - mapped_idx = NAME_1_IDX; - break; - } - } - connection_map.push_back(mapped_idx); - } - return connection_map; - } - /** * @brief create a mapping between vector of signal input names from PSIM to vector of signals * @param [in] in_names vector of signal input names from PSIM */ - void mapInputs(std::vector in_names) - { - // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index - // in "signals_vec_names" - map_in_to_sig_vec = createConnectionsMap(signals_vec_names, in_names); - } + void mapInputs(std::vector in_names); /** * @brief create a mapping between vector of signal output names from PSIM to vector of signals * @param [in] out_names vector of signal output names from PSIM */ - void mapOutputs(std::vector out_names) - { - // index in "map_sig_vec_to_out" is index in "out_names" and value in "map_sig_vec_to_out" is - // index in "signals_vec_names" - map_sig_vec_to_out = createConnectionsMap(signals_vec_names, out_names); - } + void mapOutputs(std::vector out_names); /** * @brief add unique names to the vector of signal names * @param [in] names vector of signal names */ - void addNamesToSigVec(const std::vector & names) - { - // Check if the name is already in the vector. If not add it. - for (char * name : names) { - if ( - std::find(signals_vec_names.begin(), signals_vec_names.end(), name) == - signals_vec_names.end()) { - signals_vec_names.push_back(name); - } - } - } + void addNamesToSigVec(const std::vector & names); /** * @brief create of signal names from all sub-models and PSIM signal names */ - void getSignalNames(std::vector in_names, std::vector out_names) - { - addNamesToSigVec(in_names); - addNamesToSigVec(out_names); - for (auto & submodel : submodels) { - addNamesToSigVec(submodel->getInputNames()); - addNamesToSigVec(submodel->getStateNames()); - } - } + void getSignalNames(std::vector in_names, std::vector out_names); public: /** @@ -128,108 +80,32 @@ class InterconnectedModel * @param [in] in_names string names of inputs available from PSIM * @param [in] out_names string names of outputs required by PSIM */ - void generateConnections(std::vector in_names, std::vector out_names) - { - // Create vector of signal names - getSignalNames(in_names, out_names); - num_signals = signals_vec_names.size(); - // Init vector of signal values - for (int i = 0; i < num_signals; i++) model_signals_vec.push_back(0); - - // For every sub-model create mapping from vector of signals to inputs and outputs - for (auto & submodel : submodels) { - submodel->mapInputs(signals_vec_names); - submodel->mapOutputs(signals_vec_names); - } - - // Create mapping from vector of signals to inputs and outputs of PSIM - mapInputs(in_names); - mapOutputs(out_names); - } + void generateConnections(std::vector in_names, std::vector out_names); /** * @brief add a sub-model consisting of base + error model * @param [in] submodel_desc descriptor of the sub-model */ - void addSubmodel(std::tuple submodel_desc) - { - const auto [lib_path, param_path, class_name] = submodel_desc; - auto new_model = new PymodelSimpleModel(lib_path, param_path, class_name); - submodels.push_back(std::unique_ptr(new_model)); - } + void addSubmodel(std::tuple submodel_desc); /** * @brief set a new model state if it was changed using PSIM interface (mainly position and * orientation) * @param [in] new_state new state set by PSIM */ - void initState(std::vector new_state) - { - bool state_changed_externally = false; - - // Check if some state was changed externally - for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { - if ( - abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > - 1e-6) { - state_changed_externally = true; - break; - } - } - - if (state_changed_externally) { - // Reinitialize model - std::cout << "Reseting model" << std::endl; - - // Currently initializing model to zero -> TODO find a way how to initialize them to some - // other default values - std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); - - for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { - model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] = new_state[PSIM_STATE_IDX]; - } - } - } + void initState(std::vector new_state); /** * @brief set time step for all the models * @param [in] dt time step */ - void dtSet(double dt) - { - for (auto & submodel : submodels) { - submodel->dtSet(dt); - } - } + void dtSet(double dt); /** * @brief compute next step of the PSIM model using python sub-models * @param [in] psim_input vector of input values provided by PSIM */ - std::vector updatePymodel(std::vector psim_input) - { - // map input to vector of all variables - for (size_t PSIM_INPUT_IDX = 0; PSIM_INPUT_IDX < psim_input.size(); PSIM_INPUT_IDX++) { - model_signals_vec[map_in_to_sig_vec[PSIM_INPUT_IDX]] = psim_input[PSIM_INPUT_IDX]; - } - - // Compute forward pass through all models (order should not matter) - std::vector model_signals_vec_next = model_signals_vec; - for (auto & submodel : submodels) { - model_signals_vec_next = submodel->getNextState(model_signals_vec, model_signals_vec_next); - } - - // Map vector of all variables to - std::vector psim_next_state; - for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < map_sig_vec_to_out.size(); PSIM_STATE_IDX++) { - psim_next_state.push_back(model_signals_vec_next[map_sig_vec_to_out[PSIM_STATE_IDX]]); - } - - // Update vector of all variables - model_signals_vec = model_signals_vec_next; - - return psim_next_state; - } + std::vector updatePymodel(std::vector psim_input); }; #endif // LEARNED_MODEL__PYMODEL_INTERCONNECTED_MODEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp index 280d0cc95325a..fb16cc4121a6e 100644 --- a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp @@ -40,144 +40,43 @@ class PymodelSimpleModel : public PymodelInterface * @param [in] param_file_path path to saved parameter file of the python sub-model * @param [in] py_class_name name of the python class */ - PymodelSimpleModel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name) - { - // Import model class - pymodel_import_name = pymodel_import_name_; - if (!pymodel_import_name.empty()) { - // Import python module - py::module_ imported_module = py::module_::import(pymodel_import_name.c_str()); - // Initialize model class from imported module - py_model_class = imported_module.attr(py_class_name.c_str())(); - } else { - // TODO throw exception/error - return; - } - - // Load model parameters and reset the model - if (!param_file_path.empty()) { - py::object load_params_succ = py_model_class.attr("load_params")(param_file_path.c_str()); - py_model_class.attr("reset")(); - std::cout << "Params loaded" << std::endl; - } else { - // TODO warning that using default model params - } - - // Get string names of states of python model, convert them to C++ string and store them in - // pymodel_state_names - py::list pymodel_state_names_ = py_model_class.attr("get_state_names")(); - num_outputs_py = pymodel_state_names_.size(); - for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++) { - pymodel_state_names.push_back(PyBytes_AS_STRING( - PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); - } - - // Get string names of actions (inputs) of python model, convert them to C++ string and store - // them in pymodel_input_names - py::list pymodel_input_names_ = py_model_class.attr("get_action_names")(); - num_inputs_py = pymodel_input_names_.size(); - for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++) { - pymodel_input_names.push_back(PyBytes_AS_STRING( - PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); - } - - std::cout << "PymodelInterface import name: " << pymodel_import_name << std::endl; - } + PymodelSimpleModel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name); /** * @brief calculate the next state of a python model * @param [in] model_signals_vec all available inputs from PSIM */ std::vector getNextState( - std::vector model_signals_vec, std::vector model_signals_vec_next) override - { - // get inputs and states of the python model from the vector of signals - std::vector py_inputs(num_inputs_py); - std::vector py_state(num_outputs_py); - py_inputs = fillVectorUsingMap(py_inputs, model_signals_vec, map_sig_vec_to_pyin, true); - py_state = fillVectorUsingMap(py_state, model_signals_vec, map_pyout_to_sig_vec, true); - - // forward pass through the base model - py::tuple res = py_model_class.attr("forward")(py_inputs, py_state); - std::vector py_state_next = res.cast>(); - - // map outputs from python model to required outputs - std::vector next_state = - fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); - - return next_state; - } + std::vector model_signals_vec, std::vector model_signals_vec_next) override; /** * @brief set time step of the model * @param [in] dt time step */ - void dtSet(double dt) override { py_model_class.attr("dtSet")(dt); } + void dtSet(double dt) override; /** * @brief get names of inputs of python model */ - std::vector getInputNames() override { return pymodel_input_names; } + std::vector getInputNames() override; /** * @brief get names of states of python model */ - std::vector getStateNames() override { return pymodel_state_names; } + std::vector getStateNames() override; /** * @brief create a map from model signal vector to python model inputs * @param [in] signal_vec_names names of signals in model signal vector */ - void mapInputs(std::vector signals_vec_names) override - { - // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is - // index in "signals_vec_names" - map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); - } + void mapInputs(std::vector signals_vec_names) override; /** * @brief create a map from python outputs to model signal vector * @param [in] signal_vec_names names of signals in model signal vector */ - void mapOutputs(std::vector signals_vec_names) override - { - // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in - // "map_pyout_to_sig_vec" is index in "signals_vec_names" - map_pyout_to_sig_vec = createConnectionsMap(signals_vec_names, pymodel_state_names); - } + void mapOutputs(std::vector signals_vec_names) override; -private: - std::vector createConnectionsMap( - std::vector connection_names_1, std::vector connection_names_2) - { - std::vector connection_map; - // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is - // index in "connection_names_1" - for (auto name_2 : connection_names_2) { - int mapped_idx = -1; // -1 means that we cannot create a connection between some signals - for (size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { - if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same - // and we can create connection - mapped_idx = NAME_1_IDX; - break; - } - } - connection_map.push_back(mapped_idx); - } - return connection_map; - } - - std::vector fillVectorUsingMap( - std::vector vector1, std::vector vector2, std::vector map, bool inverse) - { - // index in "map" is index in "vector1" and value in "map" is index in "vector2" - // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 - for (size_t idx = 0; idx < map.size(); idx++) { - if (map[idx] == -1) continue; - inverse ? vector1[idx] = vector2[map[idx]] : vector2[map[idx]] = vector1[idx]; - } - return inverse ? vector1 : vector2; - } }; #endif // LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_interconnected_model.cpp b/simulator/learned_model/src/pymodel_interconnected_model.cpp index 00fdc4eae75e4..2a08e1411d800 100644 --- a/simulator/learned_model/src/pymodel_interconnected_model.cpp +++ b/simulator/learned_model/src/pymodel_interconnected_model.cpp @@ -1 +1,124 @@ -#include "learned_model/pymodel_interconnected_model.hpp" \ No newline at end of file +#include "learned_model/pymodel_interconnected_model.hpp" + + +void InterconnectedModel::mapInputs(std::vector in_names) +{ + // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index + // in "signals_vec_names" + map_in_to_sig_vec = createConnectionsMap(signals_vec_names, in_names); + } + +void InterconnectedModel::mapOutputs(std::vector out_names) +{ + // index in "map_sig_vec_to_out" is index in "out_names" and value in "map_sig_vec_to_out" is + // index in "signals_vec_names" + map_sig_vec_to_out = createConnectionsMap(signals_vec_names, out_names); +} + +void InterconnectedModel::addNamesToSigVec(const std::vector & names) +{ + // Check if the name is already in the vector. If not add it. + for (char * name : names) { + if ( + std::find(signals_vec_names.begin(), signals_vec_names.end(), name) == + signals_vec_names.end()) { + signals_vec_names.push_back(name); + } + } +} + +void InterconnectedModel::getSignalNames(std::vector in_names, std::vector out_names) +{ + addNamesToSigVec(in_names); + addNamesToSigVec(out_names); + for (auto & submodel : submodels) { + addNamesToSigVec(submodel->getInputNames()); + addNamesToSigVec(submodel->getStateNames()); + } +} + +void InterconnectedModel::generateConnections(std::vector in_names, std::vector out_names) +{ + // Create vector of signal names + getSignalNames(in_names, out_names); + num_signals = signals_vec_names.size(); + // Init vector of signal values + for (int i = 0; i < num_signals; i++) model_signals_vec.push_back(0); + + // For every sub-model create mapping from vector of signals to inputs and outputs + for (auto & submodel : submodels) { + submodel->mapInputs(signals_vec_names); + submodel->mapOutputs(signals_vec_names); + } + + // Create mapping from vector of signals to inputs and outputs of PSIM + mapInputs(in_names); + mapOutputs(out_names); +} + +void InterconnectedModel::addSubmodel(std::tuple submodel_desc) +{ + const auto [lib_path, param_path, class_name] = submodel_desc; + auto new_model = new PymodelSimpleModel(lib_path, param_path, class_name); + submodels.push_back(std::unique_ptr(new_model)); +} + +void InterconnectedModel::initState(std::vector new_state) +{ + bool state_changed_externally = false; + + // Check if some state was changed externally + for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { + if ( + abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > + 1e-6) { + state_changed_externally = true; + break; + } + } + + if (state_changed_externally) { + // Reinitialize model + std::cout << "Reseting model" << std::endl; + + // Currently initializing model to zero -> TODO find a way how to initialize them to some + // other default values + std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); + + for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { + model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] = new_state[PSIM_STATE_IDX]; + } + } +} + +void InterconnectedModel::dtSet(double dt) +{ + for (auto & submodel : submodels) { + submodel->dtSet(dt); + } +} + +std::vector InterconnectedModel::updatePymodel(std::vector psim_input) +{ + // map input to vector of all variables + for (size_t PSIM_INPUT_IDX = 0; PSIM_INPUT_IDX < psim_input.size(); PSIM_INPUT_IDX++) { + model_signals_vec[map_in_to_sig_vec[PSIM_INPUT_IDX]] = psim_input[PSIM_INPUT_IDX]; + } + + // Compute forward pass through all models (order should not matter) + std::vector model_signals_vec_next = model_signals_vec; + for (auto & submodel : submodels) { + model_signals_vec_next = submodel->getNextState(model_signals_vec, model_signals_vec_next); + } + + // Map vector of all variables to + std::vector psim_next_state; + for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < map_sig_vec_to_out.size(); PSIM_STATE_IDX++) { + psim_next_state.push_back(model_signals_vec_next[map_sig_vec_to_out[PSIM_STATE_IDX]]); + } + + // Update vector of all variables + model_signals_vec = model_signals_vec_next; + + return psim_next_state; +} \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_simple_model.cpp b/simulator/learned_model/src/pymodel_simple_model.cpp index 26f1b0963fa89..6bd86c1a53a1b 100644 --- a/simulator/learned_model/src/pymodel_simple_model.cpp +++ b/simulator/learned_model/src/pymodel_simple_model.cpp @@ -1 +1,85 @@ -#include "learned_model/pymodel_simple_model.hpp" \ No newline at end of file +#include "learned_model/pymodel_simple_model.hpp" + +#include + +#include +#include + +namespace py = pybind11; + +PymodelSimpleModel::PymodelSimpleModel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name) +{ +// Import model class +pymodel_import_name = pymodel_import_name_; +if (!pymodel_import_name.empty()) { + // Import python module + py::module_ imported_module = py::module_::import(pymodel_import_name.c_str()); + // Initialize model class from imported module + py_model_class = imported_module.attr(py_class_name.c_str())(); +} else { + // TODO throw exception/error + return; +} + +// Load model parameters and reset the model +if (!param_file_path.empty()) { + py::object load_params_succ = py_model_class.attr("load_params")(param_file_path.c_str()); + py_model_class.attr("reset")(); + std::cout << "Params loaded" << std::endl; +} else { + // TODO warning that using default model params +} + +// Get string names of states of python model, convert them to C++ string and store them in +// pymodel_state_names +py::list pymodel_state_names_ = py_model_class.attr("get_state_names")(); +num_outputs_py = pymodel_state_names_.size(); +for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++) { + pymodel_state_names.push_back(PyBytes_AS_STRING( + PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); +} + +// Get string names of actions (inputs) of python model, convert them to C++ string and store +// them in pymodel_input_names +py::list pymodel_input_names_ = py_model_class.attr("get_action_names")(); +num_inputs_py = pymodel_input_names_.size(); +for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++) { + pymodel_input_names.push_back(PyBytes_AS_STRING( + PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); +} + +std::cout << "PymodelInterface import name: " << pymodel_import_name << std::endl; +} + +std::vector PymodelSimpleModel::getNextState( + std::vector model_signals_vec, std::vector model_signals_vec_next) + { + // get inputs and states of the python model from the vector of signals + std::vector py_inputs(num_inputs_py); + std::vector py_state(num_outputs_py); + py_inputs = fillVectorUsingMap(py_inputs, model_signals_vec, map_sig_vec_to_pyin, true); + py_state = fillVectorUsingMap(py_state, model_signals_vec, map_pyout_to_sig_vec, true); + + // forward pass through the base model + py::tuple res = py_model_class.attr("forward")(py_inputs, py_state); + std::vector py_state_next = res.cast>(); + + // map outputs from python model to required outputs + std::vector next_state = + fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); + + return next_state; + } + +void PymodelSimpleModel::dtSet(double dt) { py_model_class.attr("dtSet")(dt); } + +std::vector PymodelSimpleModel::getInputNames() { return pymodel_input_names; } + +std::vector PymodelSimpleModel::getStateNames() { return pymodel_state_names; } + +void PymodelSimpleModel::mapInputs(std::vector signals_vec_names) +{ +// index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is +// index in "signals_vec_names" +map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); +} \ No newline at end of file From bf77450d27b068a4712102a44a6398b300d743e3 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 30 Jan 2024 16:11:21 +0100 Subject: [PATCH 35/97] Fix bug multiple definitions of functions --- .../model_connections_helpers.hpp | 30 ++-------------- .../src/model_connections_helpers.cpp | 34 ++++++++++++++++++- 2 files changed, 35 insertions(+), 29 deletions(-) diff --git a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp index 264311e0bc29f..de76f84979c75 100644 --- a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp +++ b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp @@ -5,35 +5,9 @@ #include std::vector fillVectorUsingMap( - std::vector vector1, std::vector vector2, std::vector map, bool inverse) - { - // index in "map" is index in "vector1" and value in "map" is index in "vector2" - // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 - for (std::size_t idx = 0; idx < map.size(); idx++) { - if (map[idx] == -1) continue; - inverse ? vector1[idx] = vector2[map[idx]] : vector2[map[idx]] = vector1[idx]; - } - return inverse ? vector1 : vector2; - } + std::vector vector1, std::vector vector2, std::vector map, bool inverse); std::vector createConnectionsMap( - std::vector connection_names_1, std::vector connection_names_2) - { - std::vector connection_map; - // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is - // index in "connection_names_1" - for (auto name_2 : connection_names_2) { - int mapped_idx = -1; // -1 means that we cannot create a connection between some signals - for (std::size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { - if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same - // and we can create connection - mapped_idx = NAME_1_IDX; - break; - } - } - connection_map.push_back(mapped_idx); - } - return connection_map; - } + std::vector connection_names_1, std::vector connection_names_2); #endif // LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/src/model_connections_helpers.cpp b/simulator/learned_model/src/model_connections_helpers.cpp index 05b14720b8395..6b3541b4e960f 100644 --- a/simulator/learned_model/src/model_connections_helpers.cpp +++ b/simulator/learned_model/src/model_connections_helpers.cpp @@ -1 +1,33 @@ -#include "learned_model/model_connections_helpers.hpp" \ No newline at end of file +#include "learned_model/model_connections_helpers.hpp" + +std::vector fillVectorUsingMap( + std::vector vector1, std::vector vector2, std::vector map, bool inverse) + { + // index in "map" is index in "vector1" and value in "map" is index in "vector2" + // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 + for (std::size_t idx = 0; idx < map.size(); idx++) { + if (map[idx] == -1) continue; + inverse ? vector1[idx] = vector2[map[idx]] : vector2[map[idx]] = vector1[idx]; + } + return inverse ? vector1 : vector2; + } + + std::vector createConnectionsMap( + std::vector connection_names_1, std::vector connection_names_2) + { + std::vector connection_map; + // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is + // index in "connection_names_1" + for (auto name_2 : connection_names_2) { + int mapped_idx = -1; // -1 means that we cannot create a connection between some signals + for (std::size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { + if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same + // and we can create connection + mapped_idx = NAME_1_IDX; + break; + } + } + connection_map.push_back(mapped_idx); + } + return connection_map; + } \ No newline at end of file From 7a67372d5474be8cb9fdb80530e095285ceed0be Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 30 Jan 2024 18:28:31 +0100 Subject: [PATCH 36/97] Add missing definition --- simulator/learned_model/src/pymodel_simple_model.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/simulator/learned_model/src/pymodel_simple_model.cpp b/simulator/learned_model/src/pymodel_simple_model.cpp index 6bd86c1a53a1b..6471aba30f51a 100644 --- a/simulator/learned_model/src/pymodel_simple_model.cpp +++ b/simulator/learned_model/src/pymodel_simple_model.cpp @@ -82,4 +82,11 @@ void PymodelSimpleModel::mapInputs(std::vector signals_vec_names) // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is // index in "signals_vec_names" map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); +} + +void PymodelSimpleModel::mapOutputs(std::vector signals_vec_names) +{ + // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in + // "map_pyout_to_sig_vec" is index in "signals_vec_names" + map_pyout_to_sig_vec = createConnectionsMap(signals_vec_names, pymodel_state_names); } \ No newline at end of file From 95a271b790550e6f5d0c6282030fc61c40da3e32 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 5 Feb 2024 11:43:19 +0100 Subject: [PATCH 37/97] Fix symbol visibility problem --- .../include/learned_model/pymodel_interconnected_model.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp index e9f34dbe33aa7..ce6c96e812c4c 100644 --- a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp @@ -13,7 +13,7 @@ namespace py = pybind11; -class InterconnectedModel +class __attribute__ ((visibility ("default"))) InterconnectedModel { // Vector of unique names of inputs and outputs of sub-models std::vector signals_vec_names; From 47023de8a991d1392b2846080050eca565761845 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 5 Feb 2024 18:53:53 +0100 Subject: [PATCH 38/97] Change naming to better represent functionality of classes and files --- ...ted_model.hpp => interconnected_model.hpp} | 12 +++++----- ...el_simple_model.hpp => simple_pymodel.hpp} | 12 +++++----- ...l_interface.hpp => submodel_interface.hpp} | 8 +++---- ...ted_model.cpp => interconnected_model.cpp} | 6 ++--- .../learned_model/src/pymodel_interface.cpp | 1 - ...el_simple_model.cpp => simple_pymodel.cpp} | 16 ++++++------- .../learned_model/src/submodel_interface.cpp | 1 + .../simple_planning_simulator/CMakeLists.txt | 2 +- .../simple_planning_simulator_core.hpp | 2 +- .../vehicle_model/sim_model.hpp | 2 +- ...ls.hpp => sim_model_learned_steer_vel.hpp} | 16 ++++++------- .../simple_planning_simulator_core.cpp | 10 ++++---- ...ls.cpp => sim_model_learned_steer_vel.cpp} | 24 +++++++++---------- 13 files changed, 56 insertions(+), 56 deletions(-) rename simulator/learned_model/include/learned_model/{pymodel_interconnected_model.hpp => interconnected_model.hpp} (91%) rename simulator/learned_model/include/learned_model/{pymodel_simple_model.hpp => simple_pymodel.hpp} (85%) rename simulator/learned_model/include/learned_model/{pymodel_interface.hpp => submodel_interface.hpp} (88%) rename simulator/learned_model/src/{pymodel_interconnected_model.cpp => interconnected_model.cpp} (95%) delete mode 100644 simulator/learned_model/src/pymodel_interface.cpp rename simulator/learned_model/src/{pymodel_simple_model.cpp => simple_pymodel.cpp} (83%) create mode 100644 simulator/learned_model/src/submodel_interface.cpp rename simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/{sim_model_pymodels.hpp => sim_model_learned_steer_vel.hpp} (86%) rename simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/{sim_model_pymodels.cpp => sim_model_learned_steer_vel.cpp} (79%) diff --git a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp b/simulator/learned_model/include/learned_model/interconnected_model.hpp similarity index 91% rename from simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp rename to simulator/learned_model/include/learned_model/interconnected_model.hpp index ce6c96e812c4c..0cfc1a5bbcbf1 100644 --- a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/interconnected_model.hpp @@ -1,9 +1,9 @@ -#ifndef LEARNED_MODEL__PYMODEL_INTERCONNECTED_MODEL_HPP_ -#define LEARNED_MODEL__PYMODEL_INTERCONNECTED_MODEL_HPP_ +#ifndef LEARNED_MODEL__INTERCONNECTED_MODEL_HPP_ +#define LEARNED_MODEL__INTERCONNECTED_MODEL_HPP_ #include "learned_model/model_connections_helpers.hpp" -#include "learned_model/pymodel_interface.hpp" -#include "learned_model/pymodel_simple_model.hpp" +#include "learned_model/submodel_interface.hpp" +#include "learned_model/simple_pymodel.hpp" #include #include @@ -20,7 +20,7 @@ class __attribute__ ((visibility ("default"))) InterconnectedModel std::vector model_signals_vec; int num_signals; - std::vector> submodels; + std::vector> submodels; // index in "map_in_to_sig_vec" is index in "py_inputs" and value in "map_in_to_sig_vec" is index // in "all_variables_names" @@ -108,4 +108,4 @@ class __attribute__ ((visibility ("default"))) InterconnectedModel std::vector updatePymodel(std::vector psim_input); }; -#endif // LEARNED_MODEL__PYMODEL_INTERCONNECTED_MODEL_HPP_ \ No newline at end of file +#endif // LEARNED_MODEL__INTERCONNECTED_MODEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp b/simulator/learned_model/include/learned_model/simple_pymodel.hpp similarity index 85% rename from simulator/learned_model/include/learned_model/pymodel_simple_model.hpp rename to simulator/learned_model/include/learned_model/simple_pymodel.hpp index fb16cc4121a6e..4af43209100a4 100644 --- a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/learned_model/simple_pymodel.hpp @@ -1,8 +1,8 @@ -#ifndef LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ -#define LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ +#ifndef LEARNED_MODEL__SIMPLE_PYMODEL_HPP_ +#define LEARNED_MODEL__SIMPLE_PYMODEL_HPP_ #include "learned_model/model_connections_helpers.hpp" -#include "learned_model/pymodel_interface.hpp" +#include "learned_model/submodel_interface.hpp" #include #include @@ -13,7 +13,7 @@ namespace py = pybind11; * @class SimModelPymodels * @brief This class is an interface between C++ and python models. */ -class PymodelSimpleModel : public PymodelInterface +class SimplePymodel : public SubModelInterface { private: std::string pymodel_import_name; @@ -40,7 +40,7 @@ class PymodelSimpleModel : public PymodelInterface * @param [in] param_file_path path to saved parameter file of the python sub-model * @param [in] py_class_name name of the python class */ - PymodelSimpleModel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name); + SimplePymodel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name); /** * @brief calculate the next state of a python model @@ -79,4 +79,4 @@ class PymodelSimpleModel : public PymodelInterface }; -#endif // LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ \ No newline at end of file +#endif // LEARNED_MODEL__SIMPLE_PYMODEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/learned_model/pymodel_interface.hpp b/simulator/learned_model/include/learned_model/submodel_interface.hpp similarity index 88% rename from simulator/learned_model/include/learned_model/pymodel_interface.hpp rename to simulator/learned_model/include/learned_model/submodel_interface.hpp index 8f92bbd94d60f..b1dd24a6b3068 100644 --- a/simulator/learned_model/include/learned_model/pymodel_interface.hpp +++ b/simulator/learned_model/include/learned_model/submodel_interface.hpp @@ -1,9 +1,9 @@ -#ifndef LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ -#define LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ +#ifndef LEARNED_MODEL__SUBMODEL_INTERFACE_HPP_ +#define LEARNED_MODEL__SUBMODEL_INTERFACE_HPP_ #include -class PymodelInterface +class SubModelInterface { public: // virtual ~PymodelInterface() = 0; @@ -45,4 +45,4 @@ class PymodelInterface std::vector model_signals_vec, std::vector model_signals_vec_next) = 0; }; -#endif // LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ \ No newline at end of file +#endif // LEARNED_MODEL__SUBMODEL_INTERFACE_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_interconnected_model.cpp b/simulator/learned_model/src/interconnected_model.cpp similarity index 95% rename from simulator/learned_model/src/pymodel_interconnected_model.cpp rename to simulator/learned_model/src/interconnected_model.cpp index 2a08e1411d800..bcecba0aa09db 100644 --- a/simulator/learned_model/src/pymodel_interconnected_model.cpp +++ b/simulator/learned_model/src/interconnected_model.cpp @@ -1,4 +1,4 @@ -#include "learned_model/pymodel_interconnected_model.hpp" +#include "learned_model/interconnected_model.hpp" void InterconnectedModel::mapInputs(std::vector in_names) @@ -59,8 +59,8 @@ void InterconnectedModel::generateConnections(std::vector in_names, std: void InterconnectedModel::addSubmodel(std::tuple submodel_desc) { const auto [lib_path, param_path, class_name] = submodel_desc; - auto new_model = new PymodelSimpleModel(lib_path, param_path, class_name); - submodels.push_back(std::unique_ptr(new_model)); + auto new_model = new SimplePymodel(lib_path, param_path, class_name); + submodels.push_back(std::unique_ptr(new_model)); } void InterconnectedModel::initState(std::vector new_state) diff --git a/simulator/learned_model/src/pymodel_interface.cpp b/simulator/learned_model/src/pymodel_interface.cpp deleted file mode 100644 index edd0c0d2483d6..0000000000000 --- a/simulator/learned_model/src/pymodel_interface.cpp +++ /dev/null @@ -1 +0,0 @@ -#include "learned_model/pymodel_interface.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_simple_model.cpp b/simulator/learned_model/src/simple_pymodel.cpp similarity index 83% rename from simulator/learned_model/src/pymodel_simple_model.cpp rename to simulator/learned_model/src/simple_pymodel.cpp index 6471aba30f51a..6ebfff3f27a6c 100644 --- a/simulator/learned_model/src/pymodel_simple_model.cpp +++ b/simulator/learned_model/src/simple_pymodel.cpp @@ -1,4 +1,4 @@ -#include "learned_model/pymodel_simple_model.hpp" +#include "learned_model/simple_pymodel.hpp" #include @@ -7,7 +7,7 @@ namespace py = pybind11; -PymodelSimpleModel::PymodelSimpleModel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name) +SimplePymodel::SimplePymodel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name) { // Import model class pymodel_import_name = pymodel_import_name_; @@ -51,7 +51,7 @@ for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++) { std::cout << "PymodelInterface import name: " << pymodel_import_name << std::endl; } -std::vector PymodelSimpleModel::getNextState( +std::vector SimplePymodel::getNextState( std::vector model_signals_vec, std::vector model_signals_vec_next) { // get inputs and states of the python model from the vector of signals @@ -71,20 +71,20 @@ std::vector PymodelSimpleModel::getNextState( return next_state; } -void PymodelSimpleModel::dtSet(double dt) { py_model_class.attr("dtSet")(dt); } +void SimplePymodel::dtSet(double dt) { py_model_class.attr("dtSet")(dt); } -std::vector PymodelSimpleModel::getInputNames() { return pymodel_input_names; } +std::vector SimplePymodel::getInputNames() { return pymodel_input_names; } -std::vector PymodelSimpleModel::getStateNames() { return pymodel_state_names; } +std::vector SimplePymodel::getStateNames() { return pymodel_state_names; } -void PymodelSimpleModel::mapInputs(std::vector signals_vec_names) +void SimplePymodel::mapInputs(std::vector signals_vec_names) { // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is // index in "signals_vec_names" map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); } -void PymodelSimpleModel::mapOutputs(std::vector signals_vec_names) +void SimplePymodel::mapOutputs(std::vector signals_vec_names) { // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in // "map_pyout_to_sig_vec" is index in "signals_vec_names" diff --git a/simulator/learned_model/src/submodel_interface.cpp b/simulator/learned_model/src/submodel_interface.cpp new file mode 100644 index 0000000000000..b7edded274e6e --- /dev/null +++ b/simulator/learned_model/src/submodel_interface.cpp @@ -0,0 +1 @@ +#include "learned_model/submodel_interface.hpp" \ No newline at end of file diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 85ce6986ee111..8835d568b213f 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -16,7 +16,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp - src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp + src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 1b0e3c7dd3350..eeb587cb45f82 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -211,7 +211,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node IDEAL_STEER_VEL = 4, DELAY_STEER_VEL = 5, DELAY_STEER_MAP_ACC_GEARED = 6, - PYMODELS = 7 + LEARNED_STEER_VEL = 7 } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp index fcc4baaffcb93..fe044beb95cf7 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp @@ -19,7 +19,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp similarity index 86% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp rename to simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp index 04a5329ff696f..2ca4cfbeb8359 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ -#include "learned_model/pymodel_interconnected_model.hpp" +#include "learned_model/interconnected_model.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include @@ -29,17 +29,17 @@ // namespace py = pybind11; /** - * @class SimModelPymodels + * @class SimModelLearnedSteerVel * @brief calculate delay steering dynamics */ -class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ +class SimModelLearnedSteerVel : public SimModelInterface /*delay steer velocity*/ { public: /** * @brief constructor * @param [in] dt delta time information to set input buffer for delay */ - SimModelPymodels(double dt, + SimModelLearnedSteerVel(double dt, std::vector model_python_paths, std::vector model_param_paths, std::vector model_class_names); @@ -47,7 +47,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ /** * @brief destructor */ - ~SimModelPymodels() = default; + ~SimModelLearnedSteerVel() = default; private: /* @@ -142,4 +142,4 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ } }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ \ No newline at end of file +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ \ No newline at end of file diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 5763c30513e47..d6e7a2de26f3b 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -282,10 +282,10 @@ void SimplePlanningSimulator::initialize_vehicle_model() vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, acceleration_map_path); - } else if (vehicle_model_type_str == "PYMODELS"){ - vehicle_model_type_ = VehicleModelType::PYMODELS; + } else if (vehicle_model_type_str == "LEARNED_STEER_VEL"){ + vehicle_model_type_ = VehicleModelType::LEARNED_STEER_VEL; - vehicle_model_ptr_ = std::make_shared(timer_sampling_time_ms_ / 1000.0, + vehicle_model_ptr_ = std::make_shared(timer_sampling_time_ms_ / 1000.0, model_python_paths, model_param_paths, model_class_names); }else{ @@ -484,7 +484,7 @@ void SimplePlanningSimulator::set_input( if ( vehicle_model_type_ == VehicleModelType::IDEAL_STEER_VEL || vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL || - vehicle_model_type_ == VehicleModelType::PYMODELS) { + vehicle_model_type_ == VehicleModelType::LEARNED_STEER_VEL) { input << vel, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || @@ -590,7 +590,7 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL) { state << x, y, yaw, vx, steer; - } else if (vehicle_model_type_ == VehicleModelType::PYMODELS){ + } else if (vehicle_model_type_ == VehicleModelType::LEARNED_STEER_VEL){ state << x, y, yaw, yaw_rate, vx, vy, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC || diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp similarity index 79% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp rename to simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp index e0e2f577cee83..46280370cdda0 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp" -#include "learned_model/pymodel_interconnected_model.hpp" +#include "learned_model/interconnected_model.hpp" #include -SimModelPymodels::SimModelPymodels(double dt, +SimModelLearnedSteerVel::SimModelLearnedSteerVel(double dt, std::vector model_python_paths, std::vector model_param_paths, std::vector model_class_names @@ -38,39 +38,39 @@ SimModelPymodels::SimModelPymodels(double dt, std::cout << "Python model loaded successfully " << std::endl; } -double SimModelPymodels::getX() +double SimModelLearnedSteerVel::getX() { return state_(IDX::X); } -double SimModelPymodels::getY() +double SimModelLearnedSteerVel::getY() { return state_(IDX::Y); } -double SimModelPymodels::getYaw() +double SimModelLearnedSteerVel::getYaw() { return state_(IDX::YAW); } -double SimModelPymodels::getVx() +double SimModelLearnedSteerVel::getVx() { return state_(IDX::VX); } -double SimModelPymodels::getVy() +double SimModelLearnedSteerVel::getVy() { return state_(IDX::VY); } -double SimModelPymodels::getAx() +double SimModelLearnedSteerVel::getAx() { return current_ax_; } -double SimModelPymodels::getWz() +double SimModelLearnedSteerVel::getWz() { return state_(IDX::YAW_RATE); } -double SimModelPymodels::getSteer() +double SimModelLearnedSteerVel::getSteer() { return state_(IDX::STEER); } -void SimModelPymodels::update(const double & dt) +void SimModelLearnedSteerVel::update(const double & dt) { // Eigen::VectorXd to std::vector for model input std::vector vehicle_input_(input_.data(), input_.data() + input_.size()); From 90be6535e45c2097d0aac3eb843ca7a222f6eda0 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 5 Feb 2024 19:30:33 +0100 Subject: [PATCH 39/97] Improve documentation --- simulator/learned_model/README.md | 14 +++++++------- .../image/python_model_interface.png | Bin 88467 -> 94435 bytes 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md index 0e1b9e9299388..eac18750399a5 100644 --- a/simulator/learned_model/README.md +++ b/simulator/learned_model/README.md @@ -16,7 +16,7 @@ This library creates an interface between models in Python and PSIM (C++). It is -Using this Python model interface a PSIM model consisting of sub-models implemented in Python can be created. Each sub-model has string names for all of its inputs/outputs which are used to automatically create model interconnections (see image below). +The idea behind this package is that the model we want to use for simulation consists of multiple sub-models (e.g., steering model, drive model, vehicle kinematics, etc.). These sub-models are implemented in Python and can be trainable. Each sub-model has string names for all of its inputs/outputs, which are used to create model interconnections automatically (see image below). This allows us to easily switch sub-models for better customization of the simulator. ![pymodel_interface](./image/python_model_interface.png "PyModel interface") @@ -24,7 +24,7 @@ Using this Python model interface a PSIM model consisting of sub-models implemen -To use this package `python3` and `pybind11` need to be installed. The only assumption on Python sub-models is their interface (see below). +To use this package `python3` and `pybind11` need to be installed. The only assumption on Python sub-models is their interface. ```python class PythonSubmodelInterface: @@ -81,16 +81,16 @@ To successfully create a vehicle model an InterconnectedModel class needs to be ### InterconnectedModel class #### ```Constructor``` -Constructor takes no arguments. +The constructor takes no arguments. #### ```void addSubmodel(std::tuple model_descriptor)``` Add a new sub-model to the model. Inputs: -* model_descriptor: Describes what model should be used. Model descriptor contains three strings: - * First string is a path to a file where the model is implemented. - * Second string is a path to the file where model parameters are stored - * Third string is a name of the class that implement the model. +* model_descriptor: Describes what model should be used. The model descriptor contains three strings: + * The first string is a path to a file where the model is implemented. + * The second string is a path to the file where model parameters are stored. + * The third string is the name of the class that implements the model. Outputs: * None diff --git a/simulator/learned_model/image/python_model_interface.png b/simulator/learned_model/image/python_model_interface.png index 28b217801d57b9c3a020d7bf5444b4e42e641c22..d81caf7a94000c60b8d3594613bb481cea839c65 100644 GIT binary patch literal 94435 zcmeEu2|SeR+kZ(J5`#op$6m4wvV>ynQ51=6&DgVM9ZJYxicl1ltzA(lp;ET7bW~_T zlqHIit%yYZuV*D3> zCn~QkDkq2c_Lg>V#XI}pJ$$7-9sS@EeDC4q;OgY+=rGepPF7AqN>)xvc8wWUSyWyV zi-rHlDoV>@mA22c$2&WE&fajNSFo$QJ6=>yca5|x+)Ba%X^w-xwi5h3{rqSCHgfg! zC&D$NvZ9@$!pt?F0Oa0sa?-LZ;Tt_idp9q*i<~T07XHP;C0$n^M<*}%nJ%@}%%dIs z@Xj;c46rcUu4m}06^L^RHgr%3QV!5onQ0T?=;Q0^=9g(Z@I-;_g@E!86yw>dCzz0vf$81{@cwed6JGjmclAB3@tAz)V=&5b#Xun0u zewT|9+~n)K(eCL%)bqi6yKMAwaCCF4F;?&s?L^+kIxPftht*(sa7Bi_fyEAZ=PPG0V_ zuQ%Jq8{Yh@yCWA&?eQ>jRCAisY@4ftpUccuLQK&jD-t&wu;nHGRI= z{eN>i6#^C~>*lv%gMpEjshOSU2CT9y&AtNg?*6lbJ2O(genGU+_VM!fbfB(MuvA>2 zi>sfbsW;vpX&wj$2G?BtJlx?EvI4;gxNCX2d-=d;PcKhs1d9?eHdq~gjx-aTx#W%i z`NgR3`s<_n^BC(ndU-hd`2@jjXYZ&qJ20~rErpd;n7tDCvw>i-$Ssi0xXiJRH8kVE z&swGPm%AfPf6vT8o7su~^*2C#<2UwUvPRxqNp^$wwjfP&r9giV#Q-xCDLJLtG5gbJ z<+~~P!9@M~JN??Md|7utPYZ1Z=1z+&ZT99)i-O|pT>XWnW!6jn`HSVxofdi7*%zTr zi;D91i$zi82Wv&)FSu5Id#Qkvr^a6=cSoWo5`MrbIeI$G{;6&6j`#I-wg1|WeeLvq z!C%SAIy!t6ss0B`WzIsN#jm-`fOgHt(H-yS8t`=h{L#|*Jze;D&6;?*0;KxlM&;Ig zV;Rb_YowJFXI`K7G+!@&AA85yU;aK9IrG3@eC&4y`J>1B;eDJP{r(4!mDi&B=PyCy z*GFi!HPuc3R?FY#%@zK&P`SA=_18!BH%vuEf$0|>@BxCV&IDLNy6($Wi8Pj7!eUvMQ7CZ+}(A-9@o2}^HIW7^fPFYWS2kTuTK zOh;#nowklCw3@v{YyThI#oXe@w;=ey&8R*8rJEwIY!2)Nw2Jg<4z~66^YLmjw-i1qs{*Vhm42ifd@VYw#8rgj$pmDM75 zu>7y?V7$dZXOo$xnSt@9@9*=k@&AlHeqj9Hd-T7x^~?P(^EdS;00Bp(h71rV<%Fd4 zum}JxLCTK%MGyYpyUAHz;P2)m*U&PeFKH7kZ~ZL?N#i1D2zo9WLJae_xgjco{R1(n zobq>|)YsL++uhO5+a2%e>FVih2cP{R=Kv<>XU{m-I{yE)0~&uyvjgCbzt5lLXp=rS zo1^{muVV=EFpbDe&Iy&Kem*e&a1EiNk+|qYwE+ay!G|w65AW#!AHDqjW^o^odGHw^ zkgF$SuyWsEEhWSU zAd@iHkI+nIuK6g*e!*maAq)CZn(&Qt_|MoDz|1~D7J!~*r4=C8LdY7pU`f58qNG5( zpp9_9h+$B_)<#z2mro{+KCVC!BhZ_AaV|=qYdc><`nlFNGb2AB;%4X%xG%K$A$v<( zQZsqquQZ$;xPKUjB&AFXi!}eJBnx)?znP*-DbrTemoBXN)h_&PQ*`<7q24dJZ|@*K z7bs3rtFBbg_q!drchJlasrbMH78?+e)I0s_&0z$TX>tCGsnKlj@9E9I2Zq$_z!Q&D zHED5twy+CkfZ&6f$dC8;^Md*+LUOnxl{p8*{d&xT0RrBa3JR!pGm9&}i4niWx6F0U zzvRyUZcspx1{`U|qo^n^t*9vb<)7IS@LW@(dDpqX;Qy3?QSiq3Y<95pS9aT}ztPZA zcgsesgVgUJy`6#P^8UpzJ8NQi0<8=^i;@07(o6o^rKHL~*#d{z%dryKd@;_+u8=qxO{Tu;JQ8R+s_5a;ZsGR5TA!&c(S>N7RR$?Y> z{a!NSYvS}bqzd1g-mhBnxB7+uIoV2s_H$OX9Iejx*ETVl(&<-p3I7??|Ig;&pMsQU zy}i7GwDOvt3s)+$st=k+`7`RQGvodHp8vN6#WZ(4htrsO?>WT7zh(*$?FauN<)Lpy z0W@{U9J85YwJNhI$e(DovA7n}ML~YX1E{Lh`?Dl*U`l{%EWG*+_rMuK%^jxtdtHZ`|hJ(IkJkcxh_& zul(dUbN|;%E|q9ubdC%7VP>iLt?QWa9DgB~@FT|mJC~sCgUA)Xa7N#zc3+~^uScf8 zKmz#1-2Tk~O{B2j6VCrT*qSyA|G*qjZVpmVo(;FO)&9Sd1pZnF{=R7XcWk2gQ?kH+ znx)Qev5}+2uQ^%RuNlstV7cD_8iYUjwI2L^P51Xwxzs%FOS<=mCW61Dqy3iU{RVga zRu$(jl?BpP>K|BB_@<=rgIeIgQB@}jMoLNf zo7MRjia69`z<%M#PuhWEKcB8TcP|L_TiSW9GhfX90HO$-_=B7vsy6=++q`*#^9-~# z&4{*d(iRqrRE-t&8#iQ@tJ7OqN8GHVi%|{lysZ(|Ie1%{e&5ZT9D@4ZOD;1?$5^4> z)K~>g?!8)h_vx|gYkbe2?|7#cRNMaQ^5x*{8#gAKds8P%(G__apU)d;(6J|k(TfPq zXVm}s&w_-qJbY!vXAEUHRAKHHU)qE>p(jc|e_T(`9huV9dClxcjfOunN>G$nd~EJG7@Zh{0DLbOVk*x@8zH*8b-=p#22it6+Z^&gg{3 z?&Uw~V9LsMwDKp(Y)=V;DO zJ`ClIyNR6KGLi2mPos;`p~7GlnhASBe|y}w<_5!Uqo;v6*MH;Dq8}I}JYgwe?KS6| zNg$<5u?G@%BnKr*f;y>B#u?pOjWpNWz@9Ha+yJI=leSC9u`0+Nost%Q* zSo!i(|4) z#q@ZGz=TKm!eyu${@v~=yQ-I;aN70a$hP;}|OpVkSS7fH+KD{q0jnv_?QJ8wYt@L8=@bQ{Q!W6aP zvIY2zkoSz%MYbP5eH^NkI2|kZvJrt)pNDkE_Umt6-s(k8O&!_z9V)kVw9*=^} z#P?5}Pq$oYz%7nkEK?%8`r4x{U3D_0%vSFCIuiq4p*(TxKTX)7u%#cL4OPIXa<>Qf zU*eapj9RkTbmN+>#Lv^?mC&B|qwA})$t*?B0<5@$dJah{F3fi=3!9IBymi~b=G35< z$12UOJUDgI+4Oj^sW1m!Z?4v}$dnA@G|+C>ESURLpW?W>c%x%xJ9 z#_qxE_}3TCvpn3WWMi>=;9BJ|=k(>eD_qZ547Tqc;;WP5Bn;jfZs#l^~Hw`bSHSZ*j^HLveo z3F(Gqq{YRZ1M-#3@}vbU%7Sl-I`($ieV*Jl`l!5;9^)9V{=V-zf4b4l!5eNRd}|fnFR_&vold}L zRqP(R#RY5QdP^^P*;!pHv11O0^In}h@qYL|DdkOZtdu>=(6KFE*}WN|>r#o&_s^Gp z!H+FSyD|03IP^ffBKLq(>bkeJ!t={xKdHV|?ek`*+C38!} zf>h-~%j~W3PIaSdvj#7P#CX=(4{@mB=Ld(qn{5KCxOPxjY|YtF8zNIeKY#4lyrZmp zUOZjNVj}*Tcjc1Z+H@ptZFYJd+nylTmysc7kC?w^BYgVU@j1%%e3~M^mIO+u!cgYc zQmw5Qm*bfES;^woTtf%fG+WD;Ouo+E%v*~~M5$`d!zq;QCS1P$^1w-%M~s+-r;#Adf(<1kJG)5Hhnb}*b$<%y0~-S{9XIqOr!}K zQkX?u3uIU+m+y$3SiQP*>TUiKVq_8%_EGT}!2qfb2x z?6{>z4g&K!RaRjkUDSX1rgT&MryBH0bO1U#SiZzTvCaMJf#|K1m(10JZyqf*oWV}V zF#vm**VNDAvQdut6i#~Pw>auMT@)9A!os{6|I^Czu^%+hm{rSbI!DT|_OV!9 zHHRZx%^90GG&Y5D!r zcU{jN6B8sh6dluJ4ypY>CclW%&?@F4urCT#7IYbNf+x7~vL4BK;C1bFW81YUo0(YN zz&t-P#VIRj78PY?pc337xOUKU9?pK=g>zXblMd5Q%oH?1oA2`uOJ0Du93A6hMliBF z&K+|)P&)EpFAOaudBBLQsjpE`W_b1y+6m=x!GVlINuw4V8vr*q;wa#C0)CPhoxrE2 zL(~*wlx&wdhSoZ9rQb)Y!W~U$vamnc$G5L5P0LD>5qso*%~kr%(XyB~FHd-}L<{UC z=AJazW*6eG{G2TO+<%x@-fe$ib*bgacmi?ONEXi~3{Q_qI36{O*Wn8;77m44D}&=9$k^GNty(aGs*9xqY^ z>pJB^2G(;MaLmt{=mbu2@asL|#FKX&VZ>TH=j2x(z<9k-u|=&v9Y>DiwpAuROxjK9 zd_oFjDRpkjLYrpu-^w2job@U402z;;e5o>%+=TBH3R9w!=|wFiv-MvpvK_rGn4u?+ zW8O!1QjxoBHTY5sSl5e#N!&OslrbirIjV@>R+2PscJ;1s6xXP`62>-@4ZjyW(l`o> zVnzvpG3*)@DtDv9i*OLowzcv5jxb@*?$eXPoO|%0U_mZt>p`}Q`xs25;@71V?hyFI2OVy}XfcLnJr5bS0gPctud3?}JjeDlSKrvkGXO^6`400v_Es+nNyqBOt6ZdkW#VS2JLE^v4iX1xK7oXnt7JJEjh zGNx;wQBQ!c(n~I$yJ9QacsLa!Lgup4rayeg{OW2wW|?}T|5I?B*z2p?g;2|3Xay>ZM!cZFhBvvZ7H2~obj8C`?kzJqmJ#U9(B7f1Tm;NmzZ zGg4J}wYE5EU)zaeo{mytbrek4f!sU)o>GTeu<0SMMcB9o@e)t{bnAPxk~fZ zWdqb2;;zm^VG@;fSd|?+8L_;2Pl%5?Rl>`(9&|piNknr=BqzI&ShSf+yoQTikD#>% zIVx_T_z2O$6am`_Hp`vMZS{!hf;ho5CW|U?r*X_G*28bZ7Sk;X*^f5iKTeU}N(>cD z63puii7?U4;%LYA*lCvFYEMa_I7JE^n?+j2#3*?S6>K+H?#*n+C3@a?buLAg8+*_u zr?_de9v`QA;6+C4u(kvoM9i;o7VJ!mggNPZ{g8cBPZ`OTnieuq8Yn!GG4a281-SmeA45JX|T&CQ42?zDZu2_3%3(p*$3B zSkOqk?!^PwF^{bw!$&o(akZ~xwCF3cv(!GDmU&x=$MuwFg6%CYc{98nf%#`-IUHzO)q1ve^*komjQZD#tnImDxR!4=Yt=*gVhpOV9TyPAD!iPqED28L!8Zt}ilq$q^;m zBewI%W2<;`n+_l9?=w@tKOJ~7RJi@SgibJgFR}nx8*ZKXLAqNdl8H9GySM1^4_KSg z1rSO0h7F3&c5!YK2{HJN@c!ndqu&!QnF7eGE%1|B`GdqA3mneUs~sNKe$bg|4H$oc z%-sevPWP)je>*pGJJZJi@NatFGs6v;i6~h{m$~273Wwt>qh92YHQ$xN$r^lSHrjT?!NMfuO+ zm$H(>`dfkxf~?Q8>8l5D64qXdz7$U%j7bP)M;Fh1yH?FIe|J@h-3QIl)P~y|4Ueu2 zU&gMl&RCFlahcSdQ3~g$yKK+L93+|${ALxU$6v%^r~+xXblI6D$v=14MRI;~zDLzS z6+3(hk7f^u(=o{8L*8p3>uU2?1XRC8jfO4b8UCA0*dRW}rki<4tH_0}m2+mbbp!O! zYS-?368)BVo5&PWn*C;W7l!ahi5jY}->jKSO?9qq!X|8kQM!yXvenx+CcPSIh1YEL z{&~{E3)#K-+RUHYuEvBvvfqXg!}nSw(YI;}j)^qHt=&DFrOv!gahyMM&k>E+`Pt~i z=0iJfEjEN!CKd^b)<0Wv<|TUTnnKPZE<=66v6ii5gBF!#qTvBBnv4af@7rqq+)@J> zz6F2N;S*dQ9{TBB1|AZZB?!C0?Us{YfyAgy>pt|zwiCM_msCS4#P%{H^i$pwrxT*t zhS&D3`#kkd79d#^{37e+wY~37bdZ!f_Q=Pozi+AJD|&ZdwI);mlHE1+9lrJIm6QFs z0#0fEO;N3*2t?Yt&1Owb$=jd060}%c`>a`!)ttZugZS=&Y17qI>a=uoCu-bBok^$n zY1)%5IiJywJ3M3Wk1qo@`e zC&}0otMS@E(D`1`4fr~mjRaGfe)=_NP5#ima!&T+O zCUGCKA&J4&S89d24jW4E7pjLL<0@?3nLP|CSM>Y^izsgQReDoLwv%}fE{1y-g{`v5 z%8PH|sk_3|e5qaP?gbbDCn~P}oLeL8bm>|X6G9^?_on(*0C3?_x0Dz{NFqJ{Acqw1 z3dYKXT-NB9kTi#h-@Ddq%OeKaSRPOiq=SmB5^8`;@aNM4m=;SFm=q)2%dQ$euAu4R zN>==EXZZ+sD=C`Izr*w9Km`n|KpW-y)cdz}GSNZ3X-ZQsWJ32(eFkzsFC8c^VoJ#S zCzUWJ{74qh!Xss_N5?L)795F=!2Ay$q0|-Fk-y{K;tj|OG>KJ&ps24OCd*6DtW7!# zV_Te;&cIL1Jcst39&3( zOwJi00T#rr$V!C!3V=ij;RAFVol2+QFU~D_yP@+20-%c_pR7nS-LxFZ8-^(y#@d*d zkSVi5PPwr}8lx|HsbCAYf{V%04X-%~>A|lr3^!=8>NGNLj&c(gITB2H10%Ot5T}64 z+wRw>bNYNk*6FwvI^2+J9y_GFVkMLAQ0eEr%8)Xjc4iCZk#o=|!cz&mM~*HJe7dU= zC_@3uoV*GqJg`qOr5!*G;L8_s#d>slP3;C&E?#JB5P55bXORTr>c0>5?dHs_}J{C^wD~S8E@yZeE0zf9ChFN|E4}dn+J^fnaWuRH)(g z)s_+5E8tPk_}mL+p0vOA3__&$Ed|%0dR9C~@AOQd(A{0uE!t3zy!-T1UKtti4$J4` zl28~SUdH;RYML9g5R_=j ze{>D%Ic1_dUZ5)K=Fsh%(L(7$C%kS4ErmQ^5j@~T8(ihl3P2Okn)-1kBDaiubV%H| ztdjJ4jI8Nr8TicK{t2-i-EIbmdm3lKw$Io#otGjQCwfxGAWmMAlYla)I30Q8xrZOH zj?oMPdt{k(w%Pb^Ee1L#nr991eZwGR-A=`&0>OPJH@7@MCex1z)=1g|j;7O_XD0l# zdglmxpEKPB@$kK#amf&ueKvKLyt}_R*=}!8F_?Fh8)J|)LJz0Za(OLN+mZ)vL@7AO z0mp1BNnXwU!5F;?Z2(NuFjB?H)tj3JIwVW0sB{Rp72@sDCai4NW(%1uK}9k z%L*8&VO^8q@V)yoNLb%%VByQCpUHs|+<|5xjW4|84_S+ekT$)V>g}nwb;W6G40f|^ zJ8I>5J!P9H2cgz^Ukw9cAsr(Dwcn|qjZnw4f9{WFRFP2uZs+BR#)iaX9M5QVga2RI*w=X7!t`-(Ilrxfh zz8%M$zV6YkJEF<4{Ib?DXi@s*6i&4`)V+#_WRV(?1=Vc~GAl~Lhu@ssL*l`uQVUFJ zH~=^2M@eHYNz{1u(1n#)LO*#7!b)bwiS8Gog5lDFVd9GnobT9x$q<;XTk1bmb#KX& zh)al$%sXoo22@f7nB1_L<*q8u$_wYyS5&1!ZcpK-XA029^=Y2z+!G*Kp}({4zCH=H znZc*A(?cr_+NXX1A7+2p>VBow*<2j+oq+3uoZGC~5+{V>o(8meU^^D4Yr4NX*lP@L z%a^QtRQo3J#vb5q8?Q#GVaWz4HDWWmDN^sQSLnyKmc2d=Wf8KH?9)O$RDcVRw7+H3 z>m~gUP*jk0j%|`(GMq&a;AdZ$nB~GbaEuP^&1V(1Y4CyAIRpmUtTk~rA4gy%Ky)jy zpW$bN0b&SSyeyJ+XTtBnhH(oD28L~hQrHwksFEldoqyrF=jVmDXP?V1gl3lKc9Hq*dd^JnaqIS-z`EHm+D!G5Y4 zCbKL(@tEpXpBesi6x@%FcwVC}Pb83pDue4rfsu?18Goi^4U2x4BgaG%jo3YJjWr=qD-zs}D!?1wjU26D%+H%zG#);Dw_} zkR7jibEE(AM$=%xFck<3B7O7Cm2-8&)Dn*l2F6iB{aMI>5rX({c*VAyk`_lM+s9fF z(isWC@RM56Q2>S3fGbkX9|JFIJONms5_tLGv%3baHAf-ko>*|Rr4R3}(4uJplnWnt z0sUdh4Ir@+;qP^qJ0{uIjn6|SQAZ4(T6-&FsPH^uIUo30n{J08-hQFBOCqvx#KRcw z0@uxpR6<%FEKzI`WRizUoL;V!o!g~rbm?+E2`h_smTrE3y*Hf)K{>zw&KjQFK4qza z?Lqlx0lhg-0Cl*xi%l!*0TQ$joWL)14~5Onxw4@Jsp>FE<}|!;@JL7I`!>vXo6_F& zXQ4A;>QldA?&|P!$c!JqND`-pvK@2?g3J76da&*QKTmt~^Px_^d ztZye>Pc-I@3t&$v4{`iPkyoO|u`@q}0!;XzNWvta5$2p~&HhWT2}ppH)^&UcBSOcq z!|Ii0#G)$n88%(+lcFNGf#b~sxJ_}J$<`ZGA;{j6e@nhELQQ@BoYn(zYzBESI~t)1 z!aYSbeA*4f(-dAtxkiRezB=F7|1z-jqmAT&A|u37ZS=s)H~YKdS0o{+^HQh2mIhBG z7w=mpT{tI0%)AOOIPm6Ee!%0Iq&QCTs}$o)lC2`zxmNK!pYt^>1Q~Z{9{wr`qPfBZ zQ$R$%t-H*vFx6PW@KYSa^UX)uc6|JjTx+aDG8*Sn%YKlC2qtJWGEU5soJ8eKe<3LZ zGr;e?yxNg-jjA|_6z^P+GpwaC@8<@|NP{bFIimoy;%$5>$~htl^2q3>uIYFU?u*v4 zCkdQ#YC!5^bMh``{mef4Jz<<7^eub8xZ?z4B(;0u)wDgHyC#gD$+(QcqANOV-j@Vl zLmKI=W0B>n3}keo#8m!$)V~9m@UK~XyRIU=A-h@@!VQaS>Ao@u+g9X&R()#7oExae z$ggf=2{AW-Nih(NFW!N;yq^5mO_p@*S&3z!?pOv|C-dG${uv({BnUo-^_Bylຖ?|Aj|*f=MyS0%?tfInp1!1>r~5@Cvg+SzlwDUxe!xlKUBI4DCWD%oXG$I^ zOweFMDK~STO!hcUFJhuVcYi>qe`L_r3tX9=(Rl0#iKT1r zJvj2#1$hJL#nbd?+beOwGqN;w!m8N$0?*sI+57fFvu7POg(d(v_~-Y&e?AseRG>W8 z@>uwsm2T941T)fE{_;D5@wc4PL>sv05521<1{ELz z&rG1V&f&V!<^hcwUeKqu-7JZ?SsVlikRNNKX5qd)5CN(^hYmBpZE=HfJIFS?&MP)q z52-KLE`TnmjUM)YBn-}XK4Tx-X}#FMaC#<#vZ`Rr&1bsM$YxX7^;>nS#W5$6gWtSn zdsr1wb9P$RTEeg?d#40?{?*dY234-Z% z-(L^#yb?&Y74IRB$$PZLu&VSk&`|i6JiPh~cc_|m0VROafv5$YB7m7L9CEUc|YAe2{EAY|0R@!V<` zU@@7wY>D_3&{btGy9o?uB%+x^lyDae?SMk-0=3wZdi8N@URBU$?NOfSjM$w5QWm*g zY6wH2{-P8iQ=^tUp%5hruN!xFA3T6rI8K=`H4ea&!xj=**>ZXglJWX!g*@)+zGn6D zr}FL~h3W$8?qK&Q=sT8Q9yB$=>hNYGAiH?HeRsDPLRWSx-}@;Dba9oy9WD!iSs3m+ z9Uqlny+ZOX@EKh_iN-A52bF9qS6k;O#opzhax`k-3F3x?BrPJ59A7V1Ya^YCFrdF$ z%XjwvZRMHPWX;hOU}h^8u&SYTP&w8!N;cpt(V^W{#2BeU6vrlI{GRYjqLin=v8q4w z2Hai6HT|Fz(~|1U>9VXHg!Gz8W;n6=KOt+P-A-OBC zL}!>^v!O2r9WjBuuiQ;K9l!2I^u&u~JN|Vb&&->C-&T~vHGCHcevVUtT2A%rKkrcn z0o>_c3g~z^`onq|YJ^TP^Q$TE0p;9gBTXZ2goZ`faZ4Le+Tow?7iWRo*Kp5B4N}mc zC!GUnjUqyZZ&pV1f(zO?iPM6UPl4thStxMSaCN@+=#8@O9vC22&>(tGKv|)ZfA^cl z?yX6W{egl%z;8&lN3oD~5mn`=_i===Ml#a6DpiLQ9)*m+A#BTGCDbD2kr+*fiifY@ zm6#AF-(9%3o6Ypnt$V?gPJdFvdsr7$!ADjQYF4=Ausu1ulyJq(VGkSHEr{}nod712 z7S?+gdr#p`CUX`RYMojNWRv+&g;xNekh>8u!o3uK5Y+-I@+*)HbH%*m%A$b z93zsTUe;fq-omoz1&-P76YJW(%n5cQ{rlP+8_i6~8cZZ}eT=7)!cbFiX@Zt-fgx}2Qk7#X~n@hS4YzVcltVd5aV zwE(nz3+Z{))QOgQXUYxt<#CeYx0+juM&*_r`0NO!d2Lih<#Q+uF+yUzdjeE({FX>n zsSCLE19WJAt(-d*H$iZswqX#4(i^y4ogqv`7Fr%PfJtI#TsXAk4R|zGM54Jmx)8Gh zV}fSa<$D#1BMoorD%v^VVNNdZW(*$y>hFV2(c3nJ8uPsU;pG_|bGsbMAvRk)ohbCw z@8QOci?L?GP~Tmz1iN1cCJk8-%LC?=}3t9+EE ze9Kq6Y!Jfck5;aA#C=GL0NGT=Mj&OdBFq?u%Iqj>an|)y-cxqh-m_OX8O3qG%@xEQ z;>dd17x+3da4S=Mip=c^p(rLccHI-TzNCg`tGJZy{MbhyAhADi7u{K#s{n%3>XCiD zTTVB{eJp8Q8&pE&5JnG%`R?wR2nzT-gEuF0Wo})eKQ080)8!)j^qwuM+-4jEVxx$q zw9H$gc#>GZ)IB!SxU5{9k!uj^x``bvh~&-0B;D-V5u1pKa6ME7JzfE19YIFJJIhVa zJQ~*4d{p*Pr0!DX5b?apvQxE)(3hi@08LIRqgc7y*N_&m#ZmH6lE+4_gaR47Z7D&S zVSepb5bDjT)~H-pZ)35%GO~I+G)m0NE@tN`t#Cg$;+grQuK;u<@cOC82<8^FD{% zV{F^Clu_xN$F^>0GTdT@Gq1gKc=-pTmKRZ}{Fmf#0ZzwV&$Tq&9cC)mT8Mdx=xmLz z*j5C)EWW9YNAZZbR4rb#E2={*&F{ekPw#UT5(5)RZ49p1_``zIUW$v%Ga^H1b|$b} zkPg4$Rr5j{t_q@zic!4zo3)4W(L&7i2?k=vM4*5Y=x~QTGN?%uPF~Wk@VK?j-G!a7 zkkM~sa^*9dn6-y^oIxrchu?}+hL}HBs7lIPbYCLrZFCwF>6a2vebYoycARpK3gjYi zo?2=oCu!VN*W#FZbg{u3{%VVRZ|NSZ+P3Y=Xtv^tpIK`P0MQ?M7bLw0eiL}e){bb~ z&2%A%#q^mvPQL1aK@&v=EtruDqn5>wi6Y%B;ZAfWB@X~TP)jg6`Yal~SS}k>j+-?I z7KdBk3K>0b87OR%9;~Ir_+~T53*a)dID%}@+w>n9-sXMV)5%t8Ud5_DYQk(a34v0; zu9Iqu%i4~B?# z5_gFv@m0S><4S$*&l=z`Xs(8zxI z{=Vn6YyFF6CRiz!sm(+JNwhbSY6jIvH}kYMzkFe`sK5apnoG1xo6+h%UmHe*2mtrENU*hc%{WAj9O7-Dl|6 z?n6p0G6#i%`e3S?lZ7s(8_I2CuW*VItc>B^Z+horW1pdo;VRdYaF05aY%?EdCZtwo z1d(`^k2&sRzUgv^Y&Em6o6vn{{N3HQyIgn%0sy3@({zY%^Ttu?12VmbdP6VC2E?u8 zBcJR`JH zO7mv!fdWyWY-Gw*+(I{76Y987YRzkrQp%3=Cym0M06KfVN!mhcQM0x0Mx-FI;O2__ z$MRM>!hL!75yT8q)XOF`9qKRyd<6?z;w)O|tv??w|(06SH-+>it0xl3fzD6o8J`P2h zX8z|>LALRRYxlthQQY2a=60*z?o3oDH?y zz4>mPn`r+)Ac>o_!gfMVczu<%?>){Ut8Px!qXs$`UChh- zszb3JE0suRw-1QXcRw(cIVdw^tUhKYxHO0FJ*Zb|+|oe%ve72)*6qEg2Tc!9Qn6f=6C0Y^hOCEvlFW_8iB*T> zKR+N@?eX-2+lO1t+v*RS-}C2j&(kc1@^mGrUb5yrWL)rUEqIkkp@xgsZ<(My+)HO< zc#~&&Ze`IQM`~y;x5X~0B+?(>BYos)kc_ zb?*^8Ux^-0FK-p4cwa(_L3h`Go%LYKC?T>=x$j`#QM)4>Ni`cf{3aT*)Amw z$Wr2E=W<`D715Vubq2$Z1D*I~B_U|KmZspcu=)A9Y`N%q*fu1XWT<|{CK;)%w8#)Q zb%j6bi0U+N*F$zNxhtuDc8IdL-gDUSJ#oXxcmN7(-P>Xxe^50Qv*T#TZt|XxeDk~h z#1xFkrQ4IQJ$T-Q;5)A)9OI3H8j>&Hd)hnfUYj_Bua8Z;UlK6^(H?M`S#DS&l&mX@ zyW@D;!3S~g4V$Q_TYIXyvpnK7gZ|ls6nGNpu-d1IZanO-VM4;v)9YuCZe=H+Q-;Hc zA6_K8F=3OevY!!KOsWwH5OrsZ#kJNb7QPkzBz+Qw()mJ5c?n_pbM7$>Kg^k!;QZ4| zy0(F8Y}eAuKdBSa0VtIPH07o3oqToLTeEN3i#IR=SjwAIG13Rpw-SXLol26>^23KK zuJ$0iZz}i@X@<^xG&ZmF(iQ;^P$F$63I#G%a6DaQsY>DwP!eYLgmP(Ih}o<5{>56o zH*Pn)vql0_ngMW$@*k_Vk8PUqkZITT^fOOWYuOif`J12Hyr`t%`N~}&kE(ep0aaM9 z(S3q%B(Cnn8J*>oCD{uKn$@u-PN%;hFZX}6x!VyH35ZwVd0nViyDYDr0XYab1_*Vdp;+{7mVw(406?u@UuqJ;zdmL?=F6=bgsK?rqR=(;5dF3=;V{awv z`q-{LkGg@iiJuNnc+BKBy3R|7Z?!445cPoF+tax2;e*!M+NWJdkSL=F5@GeXTu|hC zz_o}ZamYF;d7GNZm68<$Z^>ORmgg>h`>>O5OWi6)Y)@=?-DyA|53XciyP0OA=gD1R zvEv|yf|f1-lvo`qj6w%!zcnLWfyJ%NBuS;@x+1xrVfGkS#mOC7I27ZqcVhdUV0p}y zE)W`Gu3&DyEK2F@Dp$tby}*HNgC(nYqDGFpoZ5dXQ`?5Idllz2gy z$AezGSs^L_*r^`{>8)Gw_MQDRrQSXU@KNoFeSrq*1D>YIW7dl{$4ccGoQg$9I5T{%UJTQFAIPu1L;Kj? z?i**$kyiyZ;sTk^$W{Sv5G0Q7(Nrtn5V>4!XCds5tvyC5CA!}Q$`qRm>YzpS$H1q{ zyzG6YEv$t85IP72r^%5>a5{*LVd znqj%ylgkbliq`~ZoWF7T1(NtCX!L_27sXu?Edq20#qEc!pRV{ZiXf3t-?m(FsYHaL zhG(NFx}+;# zq68#3QO`n#ci^K1159>CvmGWS-vc3FM_gUdL0gnxWr(LZbx&d0Fl3;7Y~N|Td)Uq( zC8E!z_euGJa)&xd4!2AJadJ$mD6GHl(^xQY$~J4&{2^b4XrweX zh$I{#A={IdokdhGI?GiGueu>#{Vhs&QP7(;{j0;PQN#?T>bld&d-jBbS@8$oS!)hy zFAxXHpjntL$FDcv_G>?Yws6ivxE*?U{yB8F2PRwy?R{AiC)1`2Dmxc=ub8IpXAj#! zGasKaxSXf(j38pGK@_oHEB%cj#eNOCV2T;{f_0QzSdoTls1V*PjrTYSwLdjjFe zJY9EDYkx(t%x#;wskOg-zpwT;N*<9;(7-SjTgW&RwLyU}vyM@@C@_d2aMwFfBd8N@ z5c66JK=}b=yRhi;p8V#VtMn^`J44#JW#XxNat7!^P@iajmLBy)jRs-ecuj)$${QnN;1@9Vhlsf#{NyzZf-gJyGoS*MK^Qbo(5X~!EMgYC$Ki8 zQ8};&`g)eLxk2xHYyadUQZw|Ba>*~wa`u$V#3k5{86ru!3ZkeuF>i?7k+AK=-;Yo7 z-o~J8-^=r?o*ZS}t8qGLL?{77{g%Z!at~TV8N9_pBj^EQuN8m$-VGA@h}j zYH4kENP5lMUi@l{nx+}9WqXISttOoTLJ}V~zq~Q>PUgnUTe0+$hr-5#Iy&oey{6Qx zpm>{5^HD9{|9+)KUfeHu^F&SR@g<|eP@cVAo@@sUU?G%}1>?tq`1@0%^LOxB8P5op zLNbCDGKwH79R2Or6*6?IDHb!b z>C^ZLZ2sS#0+qB1pm@=_2R9D_uxT;9PphGMBU*bXTFI8cc4Yk9JL0S$++v2^-4>9k zu^p#Nr_Q$LMPIcG{WGFX8zpUblOjZZ*mEfPtE&074~=j8dDhnt3eZ8lMbEs8- z8ad1@9ghqndP;z-GM_%4jOJ&0XuLjTDFKl`f*4+jvO$OhBS|14#g53(uhwk4!`TC=%IMe?a~i zhO!z8XCueSn${7)!rXk2>TjH1PqlcZ$13TM{AW0h%REAgS#5~HzpY)oXmugzc{6u2 zBI|P``$`k<{0z=}yWZt|5-yuwx1%h~8}#ucsEURbA9w?D#I7gK83*}A0T(i)L-!i5 z(&r1GMQc5Q-Q}gHEZ)Ky4w10MBFh!?5)uU_@l4+Nj2LPD`I{MUOG z!=tV$T~709-dVMsZnp@?=Hx-?MKK|Oe4$C9g1J*FgNAJXH$Zk+dyn9-Ym zKjgKD@!*OXvu|cz!n0*5;wevgUrj@R-2v?~XPraAH&t6Fi%O@Z++hy8pwc|46H!~V z?|dC1puc?M^~IDrQ4VbF7_OG}q%Km0&4}ctWFjKermoV@(_TxB(V@uB09#**1845i z;}ox{X(V0jGWq;zywrYzGhNN{(+S<(a#W}B)TNZT2T^Ju(p2BnmW$G2;4}INW{NO6 z2^x6Di5v;{=H9|Ck-RjI!Y72mBp=w{8-dmzHd#4NUg_oiy5Y=&x4R9S+D@QNw=c6@ z2hx1+;@npO!qiwX?dlO`FXvtgDM`j zr~&Foo5vbpIj#-ft`0@7Tb#}BRPUVff};BInnb1j4eQyR&dcFOi<_Yedxut|F6d2< zeufh9)?l|6^jsE*lJ`>| zkYZYIa+GRQVop)Xtw%8lr$AP4z4ttLC6VAl^o=A-(fAakl#7Rb2~6-itNLAVYDfrD zpE`w(P%=J!bjAamu|pH~j^FI4>S>IPaNj2FBiQwUq8tSIwJMIRS+IJegw2ICR<&V` z7AULJ@w=^LE_b;0*u3B1@IkcJ+W{!ysd}&P9MP2)^TFkxu((9u`~qjB{*=w_sBQ2o zWj%!zD^d`!bB25KQB1fZk{J}_zqZ{#&G}jplY^LY>gC;{&@=c^1kOOD0>~cN@2WnjHZGi2}?1P7uQ%L1|?tSiz zV6A{`X4%4{iJR75fGtOm$<CTTY>Uq5M%jq5#`kh-&-_-SO-9K9!0cz6qsWF(bJDdi=3A1MoZ{qm9Bbx~mZ;_J^8zb`FWWGm>i7D(w^GAaD!A4r?KNtY-D_;LRcaNN$p{ zHjA2kit{n!k!6~1c1Tw*hzvTW;tlaArtP9I1vx@x^V0IGOc+PxTqs1e!W?*U>2Xp^ zMAk0k5SWH_mvx9NR^`c1!rxdC60O9PZXatJfPtDgUNx|!saQH4k7(BKzS5pR?_=@k z-a7%ilX$5-l-yCC^qoodEUqw55rOxS4SL9S!@lzBcjwIa^>4r#c|Bz-;|Ic}@5O?| z-)*lXhKQ_xwi2t{OF?4tQ+4^P8Zp8Y3@;nGb1{Hmy%tmJ&7>J^I?exT~ zyu>Buh`?moo!` z2&R)!_Ha50kylPe_*BADE-yWtZ*BaRRTRNJ!U6A;C}|LdhPiNe!x{Lw`_)0f!WvDF zkLq1$5HJM%iYzP_U1J%wpbbm#4E6k!BX?Itif!$-N$?S=`v zfFT|?Aa@yJgOnzO4#cdL9xBl5_oUMqvbRm)xdvmPTjy0Z0vKAD7b?dkA5r=Cwju_r zuWy&bL>NHtIUO~;C2nPXh;|>RZ6X{kGXDSQdh4htyYFpWdXSnShaN&gK)SmTX{41d>5%Ra zhE4&M7LiWr5O7ElP$?xO1*JPgeb3=;oRrm_dfgVeeG*sCkQB) zv|!I5IbU_KFgte?Tk z+vY9oHgk`l!3yz)4BgEu(=}kA<1fnuh$Hy(hsGN!tzg0t z$rVky>=PkMsBn)B1uc#8g9NBUUp&(m&!>CnpPx#e9n2ZMlNz>l_V#bcRT3Sq{{SC5 zDMNfLK_C8d5!fI=qr_PJY~I^O8jaZd{{h}jxA6#-W@H|4%REY80L0oH+j9WFB2TRD zs6d#j;s=<+ryGb=Vh%I!C6 zHY&s%w3q&<7JmHIN!GevKbGVHMkvKdma#~n*aX}_sRkR;##%4Lq{!+vkb!N5sZE3; zMR`c6sTqcXN#B0ku1>N8cO`7nmq*=0V;WPwnth7VXU1OrXt5`&B;jdfhffl~JUwY2 zDHK92b8LVT5kj6^0b*etcgwn)veRbBOmQ~{#1;PU5xr)(x-oR3Z=STLm1O(f?{sd( zUD0ZD1?k2?d$h4t*AuoWb&rRd$RA0PJJF1LlS#(tJWU@jL?Qv>EdcyaTz>aIYW3s4 zK`pmKZtbC|sH|L~=8A%^*sGkL1y&pU>UB^c6qOj^JcxOgmi8hp{4S<46;95Jx1U5F zoiwxX4!#0a$S1e0f1(yA=+jqM6S*9}#%7t;4OmI4?%6IgC1B_Bl5FGl%`A-$QHPsJ z4l@W=zVRmN@Y|2Yrt5+*yi87AzgQ8+|6k z=r8c3+slRF+ zAS3NhaGoH0ZxQdo8SG7R#fo*?nw(_MHINb1m@{A!jgePV67bLP75x>EMLSa4`}ts$ zPxt5M)D=TIK<9Yzo$p-tF{U6wmsOjm($FS%6-Nxnt&^T}@j&mO$Ly_JDOXkJP z_)Bo1qB%zE83qLtns}FwA^dA!Kx*h2b>91RKOBn~*%5MJkt_+I*cHzzo8h++I(^UK z7SzsVW*(oU)Awr9S9T0WR-9=-U8x!$t8s{JAPNmViT$!jqai9nE4=E#}w6&5pf zqBC?h0c)@IZyKnwB?tn3$4E+Oh)1z=0Z^=inunNc@i&;J@6VAioXvp|-_8e6E;qd@#FgqhP!wL`213j1y9%3k@~=x-&PxWN<%qx&MXm z98iU1%%I7zCG*5Rh@N(<#DpY}4_f}r{WOt@4k45d;iJ|(Dt zDGO`Wn@{TR*3)|Rac5LBkHAhjS}{+7zKg>4GfK4WYqdSxU)wT#bVX0X!p4*?437R* z!%taWOfh%@n3y~E&V2JC=ziWZvdK&K99YU_e)3t4g7u(DVXBVU5wb{++pWwZ>#97t z#`Qf@aDe=Ua^_3=sqU;Pvu7Z-bRNk~J?bmt+=D(jPh`9FF?gw<@eA>ho z@t+}Zp9!#LhQQ#5J+x7=Y$nA4h^#;q0%*>pTx-mM`d}#*SqmUr)?9L>fvs?V`T3eSXUIoYWG@yLEc}l$!g?om18+dw z-$}>4Q_T3YwaJ{n)vfuwCfn~6eeJM4Qm?P~kG|VWf4R+6s#m~*S_)B;f%i?~Yj3!3 zm&6Sc7F|~LzfTgvSSz=D_}<^TIt5QsV~8axHEPgD`V4o*GEcihFnNcj0Q6>eD)ilF ze$+~2*9-=nt%TkR)&m~+4+@nS0_F+!TuQ(@7?Wz%=>{EPpFBLuxOO!f6#+grJM;E0 ze+a1b6_Dqb5jPZfDMBlo8KoG=LtxgBv=uH~5?_YrQmp~u#h4i`NwH9p0EW^@7LdRh zA>VzM>MrHGdqD97KLJNEpwp%DG>R7pWr#AflO#|vuWH;ANTa1AXbDB0KQ6=jjPE!HK1zebpr{qOp=`wpjVHJlHe1Zj;k_*XeuCj z)x8IR2uA*U2oGxk4=7>f)vV}^y65ovG=9VijBF^y7 z6j;TgVNZ#3{1!`rBo)m`;1$lOz^}8Lynt2 zZhQotab^?n*P(us`Q4`^0*q*jXk)0IxaZH!>1KL8nFDXD?y=PInC?fDpa+;_{%!Ri z#&`s{Cz}cawEMMRBdQb6s-nw6ETrmyN1pL+-6Du(_pIxpb4)BUp%X` zmpO#d5E4)m&_>0a(jKs?f1;R30AhbGZk4|~==`@dWOzi6CjCK*1` z^k`j@>BcIm$^h@ri&`-2S>a5QfM6wV?VBTbdJ6iGy}_ecUxyCvkA~An{$vNbZ?c%4 zGie+sXj{}z54_}@Q!a0MIk8hoyKMGII$0<7jBL-F^H5AguV8Y}P@VYUvF&s@_3O|Y zfGqA75p$9pY=SmH(r~nyYhE3r6#hs5*#^E%p-Q0xebqjc8us9tc5!PU4-hEY^O4rN z1|JU<3-cY6U+siDyRP3WBPbeyW8RQdvM-|b-#=XW0}9qZ%(+oMn7n;t8hr8}z8%!v za=2^gXP$^2&w}qpw5Yz~?fyN*0_azP&|a5E9%OT(VKzA>j;zx z)j_3sIq}UO>GAMuU(-ePb5YTg9dagI2LBtnVV$W58n3k>6znsER%%FI;vBMQprgTY5~Eo)+Us-?eo%k?XJ7ohCPI*b9DbfrES-5 zfa}EK@;1VY$Ktta;4L3YYj$RdThq+Hw|y8Sm*N#rtm{#XTx;>a)#y#KqH=LM1xCL5m&s; z$hTfvsiRW1lGo2g{kZ%zD<(ysnhU*8c^8CVk6lXwBeddt(xK7FhFi&XN-8u5Mk`%v@tiwnWL5bXG>qW(by#3 zWixy}mG=#~ck3fLU<~JBJF!N*AA24A^PT1hvF4Yi3MP4Y&%-o%#d$&grEStWpsYU# zXu|86=lvC(blvu_IW$YSNUyJF^5Hv$R2+JOX+^$f$?M5&j43NxJKN6?5y9(b$t%-&`2vtLzP=^Fmi&X zr=>q;6@I{L$0t+IErzHjt8^W{r|7Cszxu-bTq4B}-W*md%5EbVK=-1;m_Yj{Ppn=G z@a80u6UiI}jh_s+xl(E+fqvT~%hwTLI`H;xvLNU;sOmncF8Tsn0x)x&Bu3qDghd1& zX+j!y3eE$22sNwjsEf_a=;p@D)bUR}u)KR>w5F9y}0<<7Y3T zQy{#1>3FY=iF?XPba+!;y#Ae>6U&k816b2n*JHDn1Jm(n&M{odf8XPEX+Abhb@t#j zn+LGj8Nd)=&tFq3(Cu@(8cO>DI$JQ=%>6S!Dih{LrCQW1RtL{9p(gG$doxHpc6{&} zD24zMkTF^MS{sF-bLfxvcV10CJBwXRXKE?d6ddJKeAmxx?_u5){D6pRc7QC9%(_fn z6m1ZR|1pR=x_N;44n0P&FmJ4Z7z_S$v0()SeiBt#^y;=q%;+1stbFy~C?c0lZBrb! z6z+dC1=+0aV7*))ZLA1T2S<+15a!vwg(W}oQ^y|=%LE#@d%4_5@lbbpFRN8Oz z0|4O!;4T2ip+E1}(ZZ5Bf3dXjr0mf6sKsP4XsoqzgAwU#?r(hCUE_%<%V{j@-s? z&&{eJaDPP8vQz0mwYgF|081`t;5 zpI?URBkwLK`eY8i%cgR^Z!q~AE#Zis=H~w&0x?%jt2H56MHUO&FKNIdURPU|zjq~i z+a0inCo9vTM}TFoj1l|9tci|4xcdLHhp{X^$F3P5i+U5u!~`!`)=q?X-#$X-b6i6~ zzqP`7&!choi?aiIGL3GZMAmm;R71kh@Jbbs{^_~Ykm76^0dnBM? zEXu$LI1H++J*H{7;@h)|#1aY+{T=6iIruPu${M)IqeaD_ii)34FSjl(L( zMYA{H&r^m3KWhjo!(z6YZ8}a2=QmD9c|)kl0w}B}Nv|ASZ%7wl{ z3-F`GKruvn%1fW$o-TXHz%Rv()~efd8zczj0V8OIheth?gDjxCQild>cE@9-vI7BruV(z?O3H)JU2g&{X)`JU*L9FY`8Y)Xck*R*6K zbK5lK;w{(j3}3Ek$reUB#~079nb)nB%zelIXvqzdhj&o>b{l{bM1%s6-W#tH<7PBd z@`_BY%W_mUZR`<-SyIMKY%nvckqXiZPeTfJE*azZ{%2(^1Df6G0ebw(x=$wPe3zJC zn+G_O_7Mj_#Q%cI10Ow|f%wbN;k{BHF%eb&i(KIyns)I7K1~kBXI!pauQ^6U)_rKQ zy6m5&gZbeYs5ju_4+QfC*C{9gJxXn)@wJ{OV1a~w#O{l=j4;;Q%i5U|n_%3g8Ig{C zeGWS8pG9oZBr)(F7|(yj(`lR5LrS_qljW;as6S$<8Tp=rMwYx)1JD&HaF7 z2ua?^iI1Of(t_F$EL8<4qu$QgK_rByK&MOVkM1F|w&q+zgW1>L0=_2;0nKF`V8 zDSWz~Xq5pmhl6_n{th%@#SDHxgBM*Q9E|n2DYS{HY+kPsD9hZ0Gok>b#FW#{&37tz zK&^6$S0?lmM4TM?NyBc}Ov^N?CdqtW`qryOCekl_meneKxq3UjRXouH|*jspOr! zYyNXh4#zo!tBEiL-msYwo$@#BM?uwoYm@aFvbV8Rs-E`C>aeSxb_dx~wW$(l+hcQE z4}9Ta{$>3X4A|WAevjsY0&O&=rN-j0)){YuX&e87C77bD9Ux!0I!Iom4rPBJKUa%! zAiVekwS#E0t(6)!EPh(+qDz>;>%g({cSE#7e@3IBrHLp|p$B`|EDVMT0>Ojkre8Ur z@lwRnDaInQ?0Me_EeJrU$gHi=W!607L@OSOzS+cF)~WP+2AV9RKzN3~R%si7&adg6Z1jRp%>71Za6Po8t#(U8`s%o0W#ApaijIWTwf@{P>O5hZEJdL zv;$Xwh&s{14fmVonEvv&Om2%)@JVso=Xwr!fW3a&d7h#6l!T;{oT?u}!c0QIq-c^1 zkIn)z8O*zH;$90_DL^>?6dn4t-PdV97oUB zpIg;rL8JU5Hbktn@?5~B13!sG6|x})*4~5)ofqfBgYt*0q@V7CZE%zSk%|JCCG9(F zDDprwf-$D;`h(8_? zJe_G5F-s#o^Z~8lKPz?MAeMyt|5NRD263Kzn}X~NY}ui?HVTB2EPXMkwzU9pK!9J(8iX@bv{_pY z;L#*5q(Uk8zawJ790!se@GTS<;mw{Z!PJ18v8p8#Eouec+FZs&6gDzBJpBTyxNsn# ziexe(UL^+;s5vg9(;Z+Z5C@_rWz}Sx~VL?G8Ses2x1V?5ApQN+GMx(sC@O4@ew;HYjG}l%ZL_2V5yaeGayf;C0kwr^?zFHQR#$R80A!=?w%F za-c1i&)3}_{TVsQi<3iL-g?id|ILJ9$_=x+b=+(L_h({@QAZN_&3W0An!$TVg3TQ@ zGuPn%hGyYtHynSFPdv>DregF6{m zQ*Kour9!D!4Fvy{^}gu?~3*@qH?bt-g; zTJwKPR3Ro*YHI+R6tFwT>Rw}4&W<6eagEWmr^t2WQ*0j#0=dP3JpbAHsFE_!Nw*ty z&~rlHV}rP#Ep;m|_o^w6C4#585!Hp}r_MLD&ktcmK@>mz@hYvZ;98W3_RQE&-EzU) zX&=|u+pLwCVL_!9q;;xS4K}h)bIvX9HpJ(TN(k>=i_ZIyoD@l|glGee>dlpVTCiqS z8`32Lh-c5U<#zRMm~}(k4BOaYRpZ#t3;Ht)>-xKEhFs<3^}{F+he1L(A7TLJ{V)-3 zv%+!L*3gN&!Cx&5jPmj8(%rq>KbMo10g5=Q3Pqhf!3UfbCn5)AMJ?4^`-qcxI{ptY%(9N%HDFwm# zt}oZ5oVmgaiFp9p@OZlmK z3?!N!Xpr*b-8^AZxk6uEHGHYwqVtkRxyh7Fw(5dLrLXUQVQzWjH`9w&2DH7*^Z&kL z;sIP@!z&%d!vL9WZZKpRF_Dd?`pWWEmt#w0%1Q1W-G^LOjx|lMcC9;TOr82>e+D}^ zq+R^~`(~}%E1F(t&X&d{YgWGjg=f?gjqf8J;0iP)ZMYImBCjSy7<^D$W2f?0)^xto z3>>c%Gz?vvix!z_ zD!~dEY@OBl1z=U8hx1X_uquKI`|Cv&`qsRa8kubc#qJzH$Q2amGs_~8@t+~47GV98 z^e}V(#;tO9c6Xc?30lqRPuM5Meo0Ym14QZ7iGSAxwSLor*CCLP0FB@ubq5}R0dal7 zp6y?|NW}3bNR5*7@1VJIiTx@Z7h&5Eas2NOo^_$`TjeC~FCypC^7I3LvMITC_2w6v z$o+V3V$f-U0302&gYpD&M4*7j4ygu?Y0hfqI>3jmB7%j$JN?3k? zIM&(0wK$XCRE{U-^w`mOE)<7=C#ktzEzIs)!C-^nA7_tOp7$U(Y@;IhqFHZLL`1-& zaj>z6d6(|NU zVGdCdv(YK%dVeJgJe5;v z-T|7y`AoELVv+bRyGWUc{GrQ}Ejy?5dU-gBCQz6?8-I9$h|Oq%gzJE3_KEo+2hrLBVW0@|Ddfcp6UE;^4_t&iSCmYINjH`+ zj^4$RM2JN!z0Z?unVeL--x`4ugz_Ud7h64F-hCjCTDqWKV?@eFQ6u4*mZHYbg&sM6 zX|S`s5)BIryPIx1oUW`ALJNVv6aVl+G#dpw79q6XE)cgF;P+1+Xk31EFMF*~Y%~Yi zi>K|V#yqlpb#bH@*#oQf4!Ur;0$AjYPXzq}vv3S=m`<1PGbBG%fFy(8)w$uiBa5?_wOn=TK~R0!7t}IkF?~QC)eqROeM%ohBp-mI%wsv2r8orzmhUL z^?gD{*b@ygW||AtR0#R+TLxe?DFusYsM25EaJi~9s?D-B-LK`CCe#EDkVFzp{2vGC zMms?F#-KqbL4IlS$P4G`sv=$r$-fe$b|u&Nu+0usp52 zU(W1pSVVuczpU66!D3LZ@l2p7c`OGhAIjx*OTJI;SATBBUFceW5@|pKM`Os&S7A;W zf`%fkJPG2+s+|QTRx-7QX4kpfKQvC*3Q?SH(brK$Ma~Ga-=$#mxpxeLy0<@ItESgZ$97~Q9Hic+u`zjcRj6&x*#=I_W~!iB6gc$oiL zK!zbI`dttiEsG@)X2r}MGXCk&Ncyo}`$Qde#|A_g<>~H%z)N(4k0|1 zmm}Jk!(hwa)H`8YA}u^XP-B3o>cNkFf00Y3yHlgqee^XePB#J2I>4M}4tG#?q>psL zcSHVBQ6hY-7paPpOMt?dUu)&v3_k6@`0j)XUz791SDHpN;f=RClCY{e((oe}d%atV zQ~Z0qoAZx|Yac0hrB>)914QZhP169ANZ%&t>p075jI@u&#eivbF z`LM(HY557lrjOZr|05|XiK&A6sWk#4e1%}5GUACcDn}4Z=+iP=Pq6Xz5IEGDcRBw3 z^Si+*ol`#`&$(%g?xTa5)aVLo%J+AKSsinoTv{R;&;aN+07@iGpgqwWosB{4`@yV3kyZh?%9q%v&Y zI!(3Qhx{GO@f(Lhd8Bj`9_9p0NgX=E%W{*p|CXG<(p#n_0(ol@U&Q!?M5)~PL zg#&WV2HPbu9GuGoyIx2CiIl!_a{XO>8@BF&K0Xp5zYcYi5$0LRnI%h)B@<&h9;BL^$DCe1FGm?cru@BLZ>@T0GE>?XqMZ zcvchsH|a@#6?+yk678d8%K2e7>Z7J76}p}l!l!ZQPlVdx#}rYK z1A)-SuPxv|d4?ncx)}82TU6d!*W02YwLpr^F0;hU%NF z5XE>>KN1U7o80a?H5i5r6jX zZv9E3=dZ+YYwY{(nDGm%-{I&$V~PDZT+V-&;*YzVe&+0tH4X+wC;5-*|2x0s&ZQ=fB z-66lq!v6lzKpA()U0A=N6qgw{fGs%>7&zn)ez;C+F0QxNLTW}jg_*)GzSjscN7To*BtTG&0W&y17~XP8O*f#GLVTEkDIQLWz{uu#hbifw(jig z;(He1sUxb9IOOR+ynuDbnD1 zJ3DmGEelphgsGkETQVa`2_g05@7FRL6v^8xG@zJin||k^RM0el{Pmd~9sggm{qWBJ z!li;xLK`p{B5bpSY#fjg#LQyqnQ620$B{L~31tk{`uKS@kHcrGa6RbV26e#)$Sh^V z_#VT@JYj0(FDGZ@x5maZo10&|O1`n#x?SU4z2Nu%K7@?_GS}fW9%jkRmML5q$g;LR z0E~617+!m{{~_+?mHERwynq4uLc9*Hj~#e(b}2BHL8g=jGtR(I z%rt?84~@Xj?{xlN%wV~!5>}ZnkolLHecX_0G)lZd#;<|eH|T-13Sm|-`adAjpg5P7 znbd=vP?ogHwmcR-_wOW*zfs)Eu+6P(A^VnyPX4&0pTq7I7O_O}#Dbw`o-tk~xgB;Q;x-WpUIKB-Jg z+`CNA4hp)2bNaM?UCyA!sD=)m*N7_rxQ#9G4{VFD~+^~j*?kf zlzThRc}RBE`2U^E@$72u9|FewSXwrRIYNFHir(tF82*!!f{uq*`6u42HiZRkmfCO( zGw`hf&QtpdO#QhPqTPko&YFaRYmaW`#o#f@7==M>)sIu1>?GwaI5E2}p&87|SL&V% zzlSuheyyx#zF8h|zw|bZP+h+DfTGn>m#nkJ44aCviUs>Rm^>D0t9Td?5I`y|DVZ2; z`i$dSORDv&^6GX1w{c=f*&_BfU$lqcFdW=C;}#B^kj4zooAZm8k&#u@PJUcCIb~cn z(=FKB^&;Vh-jD8(3$?hzG5h5QI$LSYR5eAUElsKu^V^1vIW*v30B%XnSP9l_Ra?3SrsAroE8`qazXM>KwWm@ zVRdBa$K#*>-J3r9N_TmFC`EAc)sLptj|;ZnB~u`CnOj(*{3nYCyEBZN1@&b+<;LF5 zX#v)0kHo*1t$KmCP_Vuwo*k@pdXtO(yKueTKRl~(*4-FY!~#CMhf*gsmTIo> z=`&azAx8HY&%1jkAGL{F*v?Qm#$`srb@Tabkj{32q^oc9q~qQCLO)U@`57@5=ROR^ zr%Y1)`?QzOE$kNOA7rI=2M5DSq%-&5&rTYdFwUojK*4!E>p;G|^^_mN7#b|XvIK$b z`?!5GY46|{y~W!eUd%RRXQA5ar_ys1+x!7s(c<^FA)#9$Axu3%9o^rfy1=up6N009 zzZ{lt^QKxU{n=GWW)hJ|Gr~UBFd~26BYU#%3@QiS$G$oBbuVSnzU-lt_`hE&|A<+y z)X>5}^gTGXo+I|RM-lOxlzF6UG=+D==Ho6P`-^TehQTG>V9n?gVCL%f!S}@hi(3cj zRYMOrwPJWN>pxGi6zKonY%HL9RqbPPS9Tux|M!M(O)k%`s-Qnz*`ous=f?4OpTQt_ zm@@2vsb-^@$K0v<&|If4}8 zj#4uI{$2lkFP>budw{{y?>0IODm&J!Y=&c1r+yEGWBfwLx8HTKWAlR>8yt_8znpqU z2H9ixMf7B_xDHEOF~q+p_YpH))_08nPSPy*bZ!#!pJ93IKO~2Awo5&^`n{{0j+wE& ziQ`vIigrcyZ*QCu_$O{Y+Kq+PF``|OBq;Njk9Mv$3=-Eo8S%%4R}my~mxP{;jWZ`#;P^TbEXpmdd*+Q-h!7ib|*b1hQFlf|$gv zv$ZSMY3z)0{Z;@X$SFhRUIf2R3TMj@WRnOu@{TAA_oTDzcAJARL~7Q=ko@(gGQVD? z;b%lkF(EMfQ~6<{M<5w%dasl?Kvw4CS6j5)#f->3VMfNhs!Qfa})y)H(4 zpQ}~3hlF@@cAfpn21jU>LH2w~68huK&OUr0G!CD|HgwB`O;DhX>mVVPJ}uMgUF!r} zI+keiuW(5^^L^^*Wo8OodPe~Ea((+tQLSi;QF_6Qh`PCT8-3u$9oGo-Dk){YwrdXb zOhddu?(iFiUZwZ{U1grIY=&x^EBP&7+$JJ#=Vg`fvr`YnA);CQf2NewEHPZ@?&?Th zoHtSARImA1ac_5U038fnwrez>I}o~Ha*2*WHgBS>DoB4O@<;t{!WhdL_S@M(jNPX& zo$#J&3Wm(~ubf^9`48ls=cCoJ{oRe?*jirQ`j1mjR(LuKwWbBBR9wno1DqR=5DaM#U8P8BB_yX z!hGUw^shf*ZFwpDpI;v@9Qb`9J*(Ai^jNtjW-b{@V zf&g3O^l1O!LOjr`BninX_A+E+o(0@Oj4_U*%c(CYTZ~MjXdcllmkU4hrQqpq1%(@^fmkOqWH% z`#a(K=rh5I-@6MP`hC&(kCu8Oglq-~f6liF0_5!O9|j&%q1$>TkPms1*?J{v@@Xup zr2zTRt+-!~IbI+eCR1(N6pu^7pmFP7R%Y1W%M-4UU#C2Ed6Yb+6S?Bvj-xsJLhg%F zfft9j?8b7ReQkEVr};w`C*?2Hyl8`6TcIG zgJg~qdVcy;ib@G&j##9BFI73MQApDKC8g1thaHYv(|fx1{upix?ZUUTa*eyoVpLGu zUyj~xc^s_trxWu1p@qD7^tp66dkNU+qLB%1 zWwn!os>G2#0p&KKO?TK|nOE8c=IMe1xXifD=Rh6<{IATgxXXMi|HJmjaUyQ>@Y^u` zGM%EH(oZFKS!U{%-4RruOEtM>RDg?e1|DywS1GHW#awj}m;#w1(DWCX5bFS`Z*UIAV4hC__b zh&tZ?vLl34$qS+I3Lpif`4dR^Rk!UhDUGmGW}*m$1cs%+?l#{l1pe!JWYTC)ba8&J z1J<)GV$d507mGyRRQjDO6`-fGi%OtMkz@7QoQMDwEdu|Bl3IyY-t#-=oofD%Vwu`s zswB|}jNr9`_#Btcrl}4CU6Q#ja0OP#Pi{q0|pM-`-lN@Jk1sCIWcn)?Mb@6tG;snVN?`etdqNE{2IgDjOQy zpsF;QC+WwE;rHh%K%C76>x*uqy=Gj7I`hB;-sL+Q+*DMtv0!8(hUjNS}&J_z@)H2pYv4`y78bJavRlZ9{X zgEob`>8$uqk@A!Bm4?-82^-)lnm%Pzm03$N*x;OZu{t~L-)Hgg?YzrBvW-VZ$f`9) zzC}pMh2AgMQ!XvKL2z}nHca+xQNPNV|1N7+$giyQTlZ9YTtL`*eDc$P`ga7Ce<}i* zD|b7gJ_0F!e+aZ>9M5(a1OyznXY12bo4cb*>;>1rYZ%CIf%@y;kGH(aYTbbukPXPF^u&) zIfviot80YK!^(Iz2hS;*5tldbaz|%_0 z-pE6L$K@ruk_G^S%2gmFhQEEYaQf8gf1q&MV%du+5qw^!u^!2QkEGws>w!Ltx){pg zf2nbXnTDl!pXgW}Ivf?Y^bV-Pv4mkUVZSZ(jzrQ|3tJby#XP)eD&I@Ag<+#G`7|Ds z=sDb$grQdEO zdvNdpqqO*Byq=NlR^-bKCP^~P0&{cbJ**@S%_M>`A@0}I1baJLf=};=LjU8N3Rjlv z!DLnFbUXUK^)<|-UNXOA$EXV#u0E5)=IEKuM5q@^+G;gA)GvAHp+D0@<*U4i>ZN7Hxos6??+|1JN z%Iu8gTZGyGCPogGqYwb5YhALVSAszBl#JjNY)@2Ba&M?Og`m;Gvw%r=>$40r#EMVf zx@$sLeo}3*GXN_k>N`G!(9v)9J!nz+533vx#gVaNg0IYDU6yQvu+|YSclpvF(14ew zgmB)c9U`$zmkRI_=iYpY7#V_o1lNm*gQJAaXW#7KFARjUKJjGN69;>(yh0J^p^UNx`Enoh5qYgjaLejS-nf5 zdbM)?)T4Cui%ZTor=Hh^dOm~`>RKS$sgHqqtvP)mJ;DQQk$`h|QA5R`2j7!M<3KT+!8FzMRo$?Wr8i_zGGGghDd3cd> zWcE)G97u9g9PX~h32*=Y>Ch;OGStrEHevl?%jb-Qk23LL1VD7{j$@k$6`;(0F-VqU zSS}I+ADaDLT{Tm3Mw*jW~#9W2fS|ivBCZwnd(PeSCHCf#>k6LByx2wj#~g`GgPG zrO3P$?c+kys5&H^mlr5#Ke_`uLU&DyNg^?)vJvD79JoKNmH|LPldyNBtFnL)cUAAk5?@rvTQJOkHk<+ZcoFZ9_6B_}qrj*o4B8 zqI~f6-p@vo@t+e#>$bQpuwvkAGFlRTgBjSDmv@fw?95T5oaIub2{c{)j91*?m!!sY zcdKQVt>ww=MBQYkedbLV3wC*qsuA6j|LpSO@zAqjXERTAqrIAplu4hM*MVdcv|bbT z5yqV6+(eJqdP5%j5*~}NZp>yi(4W#w(Sd)b6Yg!} zu~-UY*4AjSgu!6>Yb~6HExqhti80J}cAAfJ%`y4WCIsG%Hl?K~oMstEaj}X3~}P!pQ_G`1qzfCRm7jLll0JzBpoDa@_j(=~><5 z-xcj+QNO6C|5*QM&0RicSDniUx_Uq^>TTu#Cr;_@hxvP25r&+EDFW!s%aiq3JQ%Fv zl8z^6UE@E!%g;ag__KHm%s&A*mQ!(cD^RJIyBx9iTN5cYw)x=~r7nc`23#QBR)GxYy z57XMm_CeLUEKmG2*u~%jo?Q&1bRaoOI+Y)4n~uwj7)|0NMN@Zn#?H z?MIyaZwXPn$h!Hzr|Vb?#_6e{Zh!7nDPH(pN5sMztCtGnpqzUZR5pZ_2hR*wIia}6 z?|<@>LbE_7&-9HWIu%Tx3e>s4d*!3cJZnk9AeCy6tRw##v>+O^?|>*U zvPgcVO&w1MkRnD5UI87ubUB5$b)a(8X?FQy`^eJfUABKFNet5=UI>dcm-qM4$!1qG zr%xquS08k41`Vg*YFNO=C*5?T(XEkCO{R-J9Lb{D@0}e;rst}U^bj1;tmMlaSNYX$_0labyI0WES661m? zFyBCO38pSNRTAOy5O`E=mbJ&PQ`ZaYz_lS5=MBPQoDxC^!)om}{nuKrKmq(deFS0G z;4qFZiHleI6W&vOio_xK1nLKTl)57*)1q|;lj(0gi}v1M`PSrY1gif=R;rs+cQg=4 zjW2*9VE1SR)WUi-585^X;pCHhs^B>QTcUIn82#gj-!oQkz@I(}?*-Xq)lV8FsuA{= zi49K7-$4*16&>X_AIKY)W1y)$@>(C!9Y~~J4;0Pp9yrfn)4Uzm2ZX+o+Wq#l07oDd zo24rB_hp0a30NFtYr;PdO%2d*eh= z``vNxxZfDx9pmwz$1%=!uf5hU=A6H|zKy713j5qZJx;fZ+Pp610-ptA%D_XB7d!k6 zrG9qGB{5J@zZ0G8+#1bh4$pXexCJJHS!C<2XYzqJ4Mj@_$VNV*2{cNRI6GGWP*Dj_ zoBgb(4zTxDz@Gr0&pNeId&iXJPo35Y5;6Ncy;4>j`>1CPfT0XpqOZb^A$t&u(JaJx zjksr>b$}j8SL*iAq=q__VX&sWtTWGquXZ4o{~tM`EDB{>)Y#z7REBX+X()}jWuS7p#L1rYQMxyIO(sN{(| zz4$Ww4i@)Kp!LI_k8dww&&4-z^4lDis7}xeO@N+-2HfCzZryYtHtSC|Gx@ai=dAx- zdV)RIl=KZpqW>B9?9JT)J%wliGNzUd)Bgm=Id~2pveA3DN?ENo;F3&o5I&${LYv^R zdCBLp1Hxz3O~QPXGJU2ND~d=txe2h*Nt|YKvSh18FObEIT8Bx>dDoD5>yl`Kx{6_G zK(Ct6($eA$ha3RXggKP3v^C(bxGh* z$?$w15<+DyG+OI<(T$0U3u2dv6|)|mtF;J%b{pNnK=ARL-d7IkP6(WzfzR)>15{_l z00<)c{n-RJ3>^;W_~_zzfkTnlL8XRLIDGu8&+L}cA!|TTNXTp>vayjD&@sgTpG=a= zd^i^nIt_6PY6B^p!D*vkOr{;MOzr1~<40lvjsSZsk^!&XV%!g;;3lyXORL$p+gn$J zKT*8I*0t=o@1I(4Q=>Sr*19+;EW`okSF0V4yMtJLq@211}6HZo+$d;AG7P4klmmV=;-A} zh<378i%6i@6%t|3Ic+ic0$@7r-u%go>QWCWe2&Jlv!cq|K&4u9GP}@de-N6eMIN*Y zAAbVCAY~LmO`RE1!D-c0SlICK=R2#2 znnaYG@bS;agTzQMQ3*}JZzSfW0^pHobo^8Opv1vuYtb;YWnqYb=j9>mxo|FNfkn^@ zv?K9BLZF(2;r-&=h7CSbo&5BMJ^gtQ=cZ!jt7Ir4#2+Z-WE2CnFo$!AgOXT49B^=O z0o`yAJ5fGLPvirWc90_i7wQ;EaI_;^zWrIstXX6ekTxo4+D|qHsDFHQ)Nwxx!xP-0 z4&aj@!G%9UJ}w?^f}KB%#VQ;>wi4+*t?!=HFysU-Wwa zi+aO-%kN@at7%>oRegwsNQM9UVq(e;aucYRM8+srLPcdF@va#kr}y5 z1rYL)+0TJ|iH6Im4#kA%iE^hEfPv^t5=a;2I1N8(IbUZb0eUf8OO=f@K@Vs)1c@QW z1|7bv>V`(0B|w|Od5?|=2MG`z^amkK;Ad!8RlBptND4ex8#bZ@iGHP=L4#t}DT$r| zYLhf<3gP}Ut~)*U35tfSDPS86F=>FdCFPa@B`HD+!aL;qj{qhIPikWPUoF4@lfn2h z|A#{|07hI+VN0KB06Mpd=lVyKKs@>;`yqg8Mdy^uBWXcO)!45Jxv!Z0X)FBj{=&QU zuV6G0zIg7nF;W!HU`wtXfv4$5Ye!C?@min~;FEu;R2=HkA_Hh$3@V~LmX(+TW;3x& z-nXGvs(OTG9+~}@g)PC{&rBf5?CtK=NN5Oo`p)Ha0D!6-Q4Y~Z@uVN$_7_G+jY;RS z(2dU2qK62fPDsfRa{>*krPvJp&Vv86cm@a+uP|7QfuInV>AF+2d)2)T+-jY0{74;v ztV&jEK9a!5Game{mFj>Q=u6@`W$eW>5}AJjEx#!V(V5AYUT_{Fmn9`!6@X*+cZdyb z1Ax8LeR@Hdsr(%P+BKX_#V`0e)gd(XjfpvOH9Bv+`|>AB6ohE^`3``Q_{Hov+7r*- z--rW*fzJqvmx=Jd6S8IU=Zp06+mBI`vwX_&`96=F!&^U}2o*9gRx**lWq>g9859%$ z185|mW36bKPZ5EUz0@?A(BoHs`YKT%EPwzLf_YSfv8JK11L)Ea(*08nEqf4oVb_5U z*ZWl+g~~G2-!Qo6p)?X_J2%Q4(2eJJAv?@4qCx;!K@BE!G4Q7vbm;mh=mC+12XO+Z zexhGcnq#~7H4Q&9R=Vf;p6oEzPvQrPW{!jT`os;4EdXU&}1461QBa}k7 z!|fdK@BT72U^HK8XkZ`KzRJVW^CxZA5yY<`#9m7r8=F(GzO=KV;PK1^>25}M*%djoC_E#*Los% z9q)L+Tc~zbO7e`%zYF@S)xtbjD02OtD4iRMU7qC_{dCSK*1aB2uRT0VTwwI}dP_3u9?puQ{LY?NmJP#Z}@!#Ktt zEApUqVqZT14^^|L|0;%pOoJ&Ggu-6({u3CYi&AD-n$cZs4`?(R5AIsqtK*hM_{9ez zMjLIV7X_$uso()INRb}1AcM)&50Z;PeVsrcmZjwO^zHqJ@)H`VjP`D-Wn=b8(v?{a}Q)07iR<>ZOTYk*N-M!s`kbvf6!hwQ?Tdh+S{z{*rXMHH(mT!brqYh>dA@nuTq}m2Ps^M^^%Go?96O z-PKsXe&1wv3px6>jd-24BUT2jS9#?F0Tx$yJERSr^e6Yy<6y;A2a{tp7`!R=twmjs)CTzJ$-uX$uLY8zzB=jE| zx=%;zcjAu=oTQhRFC#uyq^+kk@q1A|DTw-rzAw<5fD90?y#d7wVlM74Kmtfh*< zK62v{*S3xw(TYq)05>y63&@&(tG!$~MtMD?a!2OFAQhpumD4S~2mEEGza0umgh^BY z^(e{hZ%kBoTXxw$6n}8R9OHz?CG^ts6Mck_sx`*qAmPI?xv>5pO=^5W+-~+m_DOY} zrcTOPEC1v5oh^8-mlmJ{euh^6$Jd_<(#L1;ZD7OeD#-djyuJWpyZ1>^<`8aVxvV3j z1U1E6KT1yG*_9D)M0$C%LXf7JclIaolRh`WZD}xc{`OD2A|MULr8>H~f%p959F>5d zz6~vKs>}ld#oN@ilni`C#+3PT%_i})C|L0WcfR6aNRJ5#|D~K9Aeb*k zuuBGip@wH?i%QO42e6<9Ko`Oz(sfMYAcd$QlKK+-E9w)?&$Jf@pXIBkX7jbJtXn!Q zv;AOyLu3?(L${7ejy_tzlOEzJ&en}9faB-p%kxfM?eO2f`*?PyZ9z4iDT{jUhIy(j z3}_6w5kF=VvoJL`sRzDAARlo}Jx;xJOHH>fAuauj_Dr$HQ=kB%)p!~Ca?$E39*2Ml zfyaMM0MS1gVG9I47Q2zoZ{<_I!ijMN4t<*>2ndkix8@Yye)3{+Z;c?5@^3qTJ70-P zb*chiVj^hKxEOC9wnWxrmU@~9JTB9zg!s!|{XPyfYP%IxsrH#W<(dQ~Aa+=@5k;~1 zw2bKTk|t8$2$Ll=N*PdUhj!l5OYa`%6+<{{*=Ps3Cf|!(3Gk9KMLs`!3rgnXIZaHi z0`%Po@OKr%HsAdHKj$QS*l*Ww{rzTa65Xcd?!BtpS@&AO{@WLVru&(ZkT(*xQUIVA zGFE&|6YfWeF zw&NQf^|3u3y(Cg<`s-wbnRT6XK%>IdYikw$53fq8?em$#hhw#4)5TQzUmi10d33!b zp}N=1*d(dUhL(;BhJ^m)Fx=kT4i530G1+5b6~F+W9x1R#{8EiKre$4YQ7_o#t8>@d zYSW)$+#JRGPXKao`pW1-o!c`a`By@sdsWel-7CtIzxaYWmleLso-H|8VG?r6a5%=Q z;5PcB>4yJZp22yVfPCbrc()+mT=;sCtqce5{nr2g-#4N_eA3F1>Gb@*vCaRB-xon#77?Z?Ffq2U6c6_$f3bu5zpRh* zrGtcAipk5SzJZPZi{JM{{FpX`@gDi`ZA+kNlluxyeZ-|d)KuoQ2z z`&ccmR=Ndk&Mu=Tm9L6NkgUMN97QG-u&;W|M@e}AUUU4r#f;BO z*ppc{Du>em{;AN<*Z`=@x>o;Rbz`ncb(V#JE%Wx^&m!+##>d3 z3R46X#UZ$1DwXd=rp^~G%XCjaa+@1jlxtP;MQ3s5z{9tXkAJic{Cc{ZC?e!>5qNhm zq8?huXuE4&YP<4NKXCHCJslz$W2;U<_t+MAxabE}fBbE$B?9!lcpS#iYh`y9r@_0k zxqTV{bB2o_GefKZXoFi6Tn;TI#GEa6R3L!5I};@ImGg>ki-k&mN9DtY|2o=+&7C*q ziw$)d#&L>VTN4bb?UFgPXM~1hM3q>EH)TYTum12!MTQi+R*5j$Woe7$>(0KPlMo&= zRJ-98QJt#n+R(oFU`IEFLLuU#lv=e?JS{?7!o5xr#{mOwrjUvmCbnGL`+RmhMK8Oq zfLjRoGRN;s*6U4IEWGqQ+enY9)1is+@b*o;8~KT2#w>}@%v}=h{6=M(A->FOA9=dk zgZ$>|7}u3p@MFWBvrL12xs0%`fGeV81o9zZh_Tp4gJ^WLKYXo#&~@ zWS?YWnA*I?i<;=eF1pELeG~?(_2B6OMFi`&pCCThCm{mQiRAYi|0Gt{+3QA1o!Jp~ zS4q&1sxGNlOG~=u}|5D%kGSpA@c5}>yAhY+g(dw z0*xH#C&8B(Ht*#x&LVA(NAZKH3^KKS7pW#g@%%!rxW7WMNM0ujqXKt8`iScFq}7_q zNn3niEeOp?Vhkd94Rm%!6yPg4)b`t@~1IfH@B8R^CRB43u>4^tzvg5i5gi}35Kb@pGM%_ z??)wq5w&(U{^k!9Xe^a?{gf2Xe&2if_7^OT@lP!Nb?yJ|7_ zmmcAuMll)f{?y5?pGvs62ZQ>AF1NWTy%-r@a$(=f5U*NiP$FG;+B%Y7Q8JhGWHlFI znQ8XL#)s3FpK%HYO(!ltYH~3oz+N(HHX@wct)%T~GRq%$y5yho-tBZY^Vi+3q7jd9 zX+clVEEnQPTwjQ%PnHzFgJf8A?hw12_ma=OLe0zP(T#MBN)7HUEyJ+7JKaWFi61n81_5>}-uG_qnhb-`6h3{rFh|DDB)-5#4{jqcW-hJD*K^WK>(S9=!IHYF}-%R#BAd z?2ZBp$ynpS((c#O6fx-gx1tBopxvf2i3w8Ye;E8eO*Q0Dy2+_C#hOA$%=z?}J9HoJ zx5(KCwZBTipqTwZoIU9MCb$;tia(xL7BN5sw)TGjN#9L@cxK|uFG}!;TEP6PN=9l& zIf7B?gN*4UE44!_^vOtOFGhpKpLL>z`s4&AdVE$4MZ@axT42Ms<_X!$e( znoOF(86PN=ZB$Yg1b@70&HX;3J6YiTx`_Q-cv_T|`oIn;(WtF< z0ojt*QCj6vtNbaH<~ins0#;Jtef{2@ciu2L*t=Ri7x7uRFfk zQlpx_tr}m{`Hk~yQ%pBTR7Nu~Cc9mi!HXClI8C7K04X*PSusmO zukgKv)A*z&oBk>&)h`b3W#LA*%o{!^1R1#3GR$beDY5&lJl`zdO<*Mjs9G|ZU~l;y?pRnsH@tQto2e5VVCo08k*_8IhZc6( zRp<}9mIQH=XUwE@Tt__MY%?f6Tq>)idy;Ra#l^VnFC%2oLX3@oRToH>S3ejm+OS7H zUdGLT)DGk(oh)!Q%9)rX$Pcq<7_!Wl|JVpcpMG||kX5w5TeEFTXUx)JdWA^{q^#FO zxEyR>VQ*dg6}rFf4WE7mrW8BpKu`R*knp6ImR3aeVM`3WUO%d!(6T1#c`>1QTQ)JB zR3{BQGK5a?fvL`X)N(MB*=E(z3p*$Om5Ks%z7 zGJ0SjHIPV`V7t`x7?%%?VWIYiNV_F&#BP7~(&*?CB^ht>pqzX6jMg2BSx!jIB>Q=# z{SAnW)YKXbOVg9)RfEqP!&wBL4E^~iaNg`YCe<4;Rz?ie0JScZK_Z7_e*dWDLl22FuX!{TCC=3 zJXQ6b7I#kx-%H?s@EvVb@Bf`tLzG@!AOw?7CBX{|-m`6y3K5&&!NXrR5)^k|Kj1Lx zBN0?AZtp9N5>bwM%prX^QO5@g%erxQG3x|ClIQZy zmq3>el+60*CB~=_iJBe=nhT;5yE`|&*4JP%D#3%(T-4Zp>A^>8CKs=F? zi*t2?&hnhFUaQv%Bo~d_YeR=;g~E1^dOH{;Yq~X*p;$!(WqPIEh=F^JgCn#Y!FaS# z9AIXCXT#qg-JpH`=n*8#Mmf6F;#oX>1=q_SZ24zzuAoQ?6PKeE?Irvy6)?Mr{}s@o zy#+@(RDspIi@idO`yynokusliup-##S3AC$Z+D3pcmZC&ygTW@qxR%x7Iex|r~DGS z_-3)<0%MKK25ayV^_bhT!jncGavHtBT~3v`2XCJ zwe8O8+UeY1qI+`<@subK9fj^`JS&M3KzGFIc_(InzpOgu7F`-E<8$MF`4iiBZEgY& z{Of!typ)JLZMp4{67r!&hBftAT#)d4r%;$OXC%ynI@tX+)?K6<1s@f)(T$6xxW4Xe zqA(AREp^C%khMm+&hZCg4--mpOeJ6j-(Bm)-H-RWt!|ljO=9Ox+;Te&t66}y$OxuE z2SZH7o5|UVu+VEw?Bv8y{dJdL4wj(`(zKt59DUm=)fj7=pqU5?M^Jdz+l}dBjU1b) zSmnK<@{Z~x(!#t@ngiJU&!~-rfW59Akj)4|&NQEg*W4^#Xtgu*a3&VHS zA?)UKAq9ab{a(A@W%VQ7Z=!^JU|yU$XWW+bXra>&o9t=(G<<=(3IA=Hl+>=cCD`+I z(%kXw4f$D$7+|g6?{ley?{{DmrR1s%W@3lmpPOyxr+v(FMohoPeEool+80h zr7i%$oBt*i)b!^^S-NR!G#2Ko@2G?k4Z^-RP!NVhKb=gdkF=6H_P0Z7D2HtRl{jX_ zYfXGoP9obc=b)dSOw;4XjVqF}ovP}DgD<7X-c)P6o5T7}OFyDdRiFy041cSkAb2Nk zxuwr;Pwa~x;+{0Z^%Dh*l-On}q%&Jp*RQx#V+qNtSnI6j+ATtL3}zrEx(HsEia46? zgVf=!#gzHUu~A0?+FMLN6mTdzGDtu3?ov*m%~^Jpu|HDMm!-QKn7PmWO(pZvJNmlK z!NB7=-h;G4hI9$kW$IdpISuA`{W$^dDV@K8c0P_Mda_I==l70@$zqJTNuv4^oo4C{ z_5WED;q-}&2BV_EeFn6qu@)ABr|n>hr3k&R4Z?f z{aX6<3uUa#6uK=#xG#UffRfInry|6~dEmiIKZ20{m8-30tQ`IT`_L+JW;G>&IL48+*)LcRDw+mJLfu&Qx7_g{DfXlZL(pi(AP+167YjQXdGaHU@$X_Q3 zdhyO5&TK+LvyxJM-AZ~t!llfwRX)BT#5Xi9!542^k>3D#lw#0$0&I==D&|=sm~s}N zhNXdPC!?aUi%M66C)ad=@S~eQ;=@n!6d*WEsFvP}LMU}jBVpcRP*H|Fyyz{%wFWrGKV6LpIk;;6Ru znMU&`xUJ@Khj_luA#i|L+#09d8}Q6f@C$Tzv@-fZpr(x#Y6+?9*dCWnr^lGYaHnkU z1%kM%C+Q}GreQIC-iS+wW-^ojaWxHAy$)GZ|JzbHZ~{Zt=Rjp>cqiEdQf=T@tLs*J zC4L&}>}LZ8^rTY2STW}xMBzbNV6wTFw&Xrcj0@bg?+->LhNCmC&?etne0x+Q0zfoi zGtOwc2u^3pXTVa9zX%w|2i6gAfs-#D@5D4)efYf)fMdok2~>-So>^7m>LjfFNa|39 z5%doAHfyuZ>QEe@Jj?ACHt#i#V97V|RZ5MB$!-G4bQ690NFD1E1yWJJh37dD_^AcMwjrMnh@q05G z=yYAYFf?kX@bI)RpskgE1|Q^z%R-Ba%P5&@fxm?S3CVIL*U=-zE|82vRQ||QP46E% znQX&l#`^P*4Q@5|N((SORbhL>)hoJ`PbCIJW^)sZL2dNx`=wI}O&So1Er}*!nJE6> z>NK!dMRgzz#-WM)yl6YWgqcMay^EhQgbqt?3y?5l7F__3mu1S zohkHt{;{w)UmSF%0+)!v<6<#YOuc#(_T6k3ob`4T?GVBRZoY&W$aTY4Avr3dF^5%3G=@&rg@s z{poEp>SuyJ0I!jknh1t=FXDmOk(hW(+Qhe!A-(#@u_c7!bsx@uPpDD%>O#!>mJ6oK ze^Au94>~YqJQAW`ok$i7uVQ09V{DJ4d!ol= z?teCBDO@PP-|pXLMKV~=XjMD@pb-9Den{=^wYC1Pi~@*m5kwS!&~u1=U&!YN@no&=hSpM*^_I;$nBh*?cf zBSFlp5qv=L?4Z36Y}I^dalPJSGNf$`DKDO_9!2rKfDj)Hm63sBK0G(JIQlwwHh395 zJt;OrQ)m+s1`}&pcoY)%Q%Yy&vCNkWhb%~ywQ4g~{*TN6^%~KO2r$r0ED>?=o zG5+mlz7oN=|1h7J!fi99%>#%afrFi#k2^}L#>K_%bIk$=L>@-gAO|-HvOs9**YJqK z@)7MpQP%~k`aWosmW;DVROcfI2x$>BMpc&`u(@l`wQt)3(M8G(pH7j1Ling~=37a*K zLfEOKH6H?Bz({HjVM;1g1}!;;H&YQt+Bq!46NzX(RPRj~%|Rxg&gezD-_9N1{( zat*I7zN83<(bw$o!ZN5j;(RZyfZ z-H&R&`YKo6IfJ2>K+~K1V7WO!p$!`YTN6Vw(!$I)Mw3TxF=qA&CJDVzvC&3U_MY+- zbegJj?I^cr+X{5K8qLN-J@D7G@yyXlt$9@UCX{DsZAzKBb0|ogqzs z%5+l8dkFxUg~5DBWz!>PQf6N3O@D&OWa@K6q51`uy{ceIgO09e^DX8CMGEmr)(v|S z+iV_kkgc2``cj{c2^WX^x9NczWwIZLkC%YLrxup>o_23?11wY~Kje8kFH3%}#Z-Op6Q$Dg3=ksfUJd*Fbv(-bfC#ubKs2=F=q8gFaS;`#W?KFf-$UVH8dYT_oqQ5ri4&yxdn%AGAxr*OZ!;1sB}Dbs-}9M(h^E<+&tKwn0|cA&=^ zo_6JEGNm0)f{k#jE&**_d|f}o0@+%+#ZD5(tM*3}jXt66*Y^(QF@vRC-<)UC#H;Mq zz3UUpmytoTUiex}W1KQ}l{kpUOsTAx)yA#=`fEyPIKyq+kxG;G6Wh0~{r}AY{)fkx~Z&j4jHkzmnz0vP|!fPn9&CHc%R~{V`OO#i7k@gCJP~>{6XH zo|k`~F5RRX4tl=X_bjtIWxTg9wLQR+ljUNXIve?FWsqqNXi{fuj$lDn(Vje{82Y)v z<;Rd-p`awTAiLybcb!cScP)$v`osN&0sB}RTRCvGg>E!5WGnT(hQ*%I@+Po{q$G9SLH$kR}iMcZV zlN622j3hw#O9T8jeo<2$?G3tkx|!#KPW2Ty)HR)HejxamYi-Bg$*%gtzVT}g;uRs!3=K@FNS+SV5+UA%RY{F&-oP%&Vc5pqpzF@quQz=&h#fouE&PIpfo0#Wspvfn9UD>^pxr>4JLT+GOMMD4rlS9 zC?*BC7%h>Nfz?}VMFcz!Ij`T&Q!~5A{3|`=4g>q2aj47S^%4P~=-2SH&%nuVJ+_yN zXQ_VATMs(VX3E#aF=b^$%hyeolcRXdXre&{Bajn9ziC*_2D!23(1nC~Vdl_C`VlIT zkeI63)E`pIKH+V0kW!VIlmv%8mhu7!&j21cNUn@(Ezhave_t-54YQkDzm(y6Yf?rz z4irRhG}9B!MhZ1Arf{N-U(auB2o4B&#cW2j_`Gj6*gGKF#=47dS&sYWO@P=ld-H9Z zwb_9$R1Mz91mKELTZ_c2jdxW5pfK_u62^;9ojdfVY+ z=qnT4K3K)iIW!@<`XQk-E4WVAdtHk~{@Xepnz?R*uau**Nytk5~P5Od5VC0&R*?-5P}RJQi#Q z1q?<;GhV3d3hBXs`;Svl@phKR9N2r`ymr@W~2B(L!Sv<|8d!D1S_rCaz z=FSgb@HZD57w5%V)(vHe&HuK^h)}(#lE7dNs^mzCxwh#1sHHZP*j)g&7>wr+TGS9* z_BXv^Kn)PNsY|ta#*5nV;m>C1eBJ)b>&A!dOw}bOv+hUiqQpmi0*H~L<@a$rje_rg zL@F}NPTvfud9xd|6|)dejjs=M?O0Gry)s!NX?8y#xc_k7jCaly_c{~P`3w|sXb}5R z1p_<@TRDM|5<+{?w};Lppu@~7ywmNEMCm1@%47(~T){4Z?}28&StBxRZ41HmRuQ9D zeHmR5&7gua@=at=*L{LlVjL*0@PlW=s?E?&J)rW30 zM??k&V*YaKCfV;NzK%Qmbz6#p5vgyk&)TlYMQA5mcgAR zsf4D@YG)g?46*^TBEf#&{R3Ury7CN3XwZubC!&xJSuv9MjP)aDKH#``@^@9Q*rj-V^ z%%G!YG0^(X*FZPQqf`L#zVQGCmL>tPfae3@wvnb|hQU`_lUZh)k5%)ndZl@O`SJGx{VN%+q;v%ez!!gG zznG^X?kWUOAOh}pWSTY;iGtXlOH5^PkYkW%LVCw^m-20mon!z00e5__o&O*hR^OFM^vS>dtwB6oNdATA}&*W3FmQFJ^TLL0dGSSyY!x`oP^YG^tA1|JD#Mk@M9_9;)4h>l*g!JU_%V*u*25)mq+@IDza_ zkAf3L!;^2$CXA>6mIe%2TNb=0*nOESM;ISZ!%+3XcMLs)O)WI#^NQH*>}bj^v?Ixi zfm$CE8nx1b&Fv{l53Zk#t_kOK4QgpjVEXMb#7hNT5p|d~nnIeZb^HORbMn0C(Bb)H z((Kr+d@&sWm59nvGSL`TR&ba5Nee)NKRm&1b(WqFqTQk_d^MSwG|rVP?ez6#&nt4M zT`XZIZ{ExOz^*ERqOGgWA!nP-*-Aha*umro_#c9#qJ$h#mAZqqo&tg1dyz%DzollB zs0_e9&ZY}rkpcLb>Myf;Qk91)d(^yTCh^iAQ+C1*eKBGFvrvVeUVW?ENk%OMS4r<& zC~ZM?P_y<@=pL?N$8o_K(x-{qp~uj9?qzc1=xj-5il27o6`Kwkihy0I&-2V8bovZXnwO>e@iJalqQj~`C7vz#*N_X@_)0%k6A)sq` z$kzYM`GAvI4g3LYhf4mm2RGC9adgB?H@^DaY2{2afPOMc6HngJpk;sTq91VbURM4X zBGWCpK`7qJ@R-VFBL#p+xJ3lQh_SGMDh6!Sj&;~sX`Ya-dO-$$A*F*M7#sv6yBaSw z_h_bJ;s$&`ii*4g3&n~`N$}-h4CJ@#{^+W`V2-?GcGD*WB;+3g0sx4F@Dw>Yi4zRq}0=QG_?UnhIYW|8v1M-IOZ&dSVw0JDH&GIX;adTu5-L;orzB4J5DSO z1-xVMK4oyH!WiGy7ey7b(+wK^Y9jbUXYi6w=jdMVv~!65kyS8X%~=e(hh(a)o9@7PL2;bu2;TTT4BVm01#rEnf*Ky zM6BjNa-Ry?J(cXiBCEq1tc0YUpB1kFZIPNaen$E4CaZ7mgL-+4*MfesnnMHywCZ8% zeQ+jL_ZL1j6yC#R6D>>FxK4ipcNc`B}Q84Pf?`hwvGH|@nMHjO2_ zkoy;^8^BcKDiZCi?SgzkFp|wyPB8N1wzJz#vEL9h8aE0jSd&|>u2B&(UN1%wwIS1pcT6yRHo7XI&P@`6p zLg2%IIt~Yd)EK2D6mq{44|~t9Tfk$RCGzoPdsM1J(@rTd>@w+b$*|5=p&O6=>XYDh z8tgl}PDvYO<@3aqmP@J|gt7pjVwo!SCAV6cn9`_rWZ)J zpDPA#FDCW*_VLuVhUok5%J-xGK~I9OSs37li;>fW{daZW;s-Q-_%W~tIP59peogng zldB?0-H$W|_(pnQ=sk3=xJig!s#6zI>@;<#(I{t?e)v4dmm)&=H$e}*H~=+CcKZu5 zJbF>h^+Lh#oQ~%et(S5$bfB21;5Zgol$eHmHw>&eI(?ejj>qN6P3#(!^sp{&aXfv| zQv+`uK7X*&M5*F%2)`zBM)|12BhhfWQ;l6TtQYPrbg5pE_SjOXas`j{Dm1*5gb3me zs5F6cCdq?ovRtR2H1`N4R3=NCqhRU@U7`IS=)jolKXgD#Y9tL<*ki?{GHqK=4}e*S z++rJJ)Ne%4WPXzsPDJ0XLV zYgS9*6Innce&prdU8|H!Y8Z9RtQ(6KV27W@;b0KucxTC*-j47ZXyWSk&chhws&7yr zy+Vp&o*(udjOG&@p_mb3rUPkO4FN7M@XDWkdq)O#UjI$I#~L96L`@D$WH#;k5Q8>f z=BWZvcPMyQsMqc04u6lPKhc8So3TYEQo!j3o*Y+GLadv6hYwasrcDiu?Y*mH>5u1j z45ZwCtaXx#izumII4hDXicpynNZxQ9cY$)pj1>uOT&Vk&(Gvp`qMffq<3y!70aqWq zQ&A>d7M;o(7oAm9LxpRugE8R*|7I^Ic_Rv2&)C8&9Zo<9+M3D~I~?WCE~#5AOOyq&An@tKg(`6XyiW@E^(U2o;y)|Di()5Q^ubpuw5P)% zHq(`kSbNmya%A|XXz%04^Tg^D9Uxl)GF;*mlrIJCmvDE*5v=?fqEuU)$|w^roZ~a5 zJJh*kvsOcxmh3gH!b%qrA;qfO-x34^M+|BViit+Zc~u_#L`9!v-~5p?a*52FU<>M1 zh0h~J$R_^TAQVQ+5eQ2wTn1r4*zP_YT11`)Rp!P^m0j#<4-~Nc0W>$aj z-#PwoDYaBE=zEf6qcF0sWe(|WVt?}@JNpSZMqeFiYNGs&y}2>#x8>JkpLg`oDU_-w7~Fy^Az3$dDO`MB^}wm}POA$|~#J`#f$e#|Mz8`Ju(@0+X0G z^rb4n+rt~OI1>%DiKb9_w0KT#JBK%_lH9bLgfp*?@6s8>1eq3cxx~nRigusjOmOry zvL(ScJ(3AhVu1r1 z`FRNeA0Mxztk>*GUe9t#p)Y zwk9MEcCY5-t$vLnxLEfYdr)I3b&JxY@V5L;pF(jnth#?T@GwR+0~nDy(@9Qn0HSwE zJ4dd+UE@o50?MP!AZ!nc_u}#S-HE$$cpR1_AxMfyFZSx&5NU^nqVnl)k4=Jk4vzUN zgPyp<;_+wU99LCyb?yhRuAPJSHiI4Bqg+qe56mAVczc|6hecq(uP0qyZM%AAM!~*? z)p8M5O&pkH0)*Mk%-|%@@$LZV>vV&oB4ZZDJ~PBv%Whtg0c+{0Qgb;-p)oeNZqEZs zwE+OPpfn0qD^LpB^{13tERZV7!1qfHtOv|GQYsged=RPdq`mzsAi_Vzk}emE_Rkt& z-W+s53;NOC@wZ9Nb`8lyD_W3ZN?PbgtnLU^o-+HsVs5uyV?Ybg5RY7c+lfZ}d8$ML z_j(bl7P!VT z54-1Fscl6KYyEXDBh_8X#vjIxln7-M03*rPGo=)AAiN%mlW~?GrTSf){gn|YUFs&? zdWXgV2KA17^B^CV`EAwi^A0@~Hg~V>2H(Z_$S~rDa!(P}5@c zbMTFu%f;WGNE%K)FVZx4G4sHhX-!Qp9yO5Soay#kqN~B@i`4^PD>;{d+^l6~>_}bd zACwPzqiqb`h(G3))Lu*A7+;h>?os}#)3h*=j8{(VsIkT%!d;pD92nSCW4#b)z>dx_ zNl-P^S>~I%0j2ogSKCZrV^d=-V3`z@b&S82Q$F zZF1fruN9rev)treBW%7|{@EznTptiWV0KX7p`yh-LyPO&r$0KHS8kqh9fsnAU#nN8 za>7D94bcO~BI^FD1t7Hd4d~p*Gft^mDz`2%6+LTQE;KaYfhOT4t{AcL8_ZS#brU%|>4my5s-50RHs2Uz<9xeMJP+Ec#lBtNitfYgDTA(%cg< z;H%VOdFp~;&q9dCj*(c81)sLXYT~K^TMi*G!)&a(ySM37|63SQ?MRN(SfL@yrUKn{ zU?QWlnV#2vsdr|d_c+hGdfRTO>Hlf(yMvnQx_1Q;38D}>NDHAz6GCr-^d=x(Is}j= zN|#=w1%Xf$=^)ZOh=mSH=)G6zMLI|?!ac$F_v-t7-`sm=?%cV5+@F(iLgr+jUDjUf zd7ib-4p~tA3~PRUI000$H95<+)ZpX#4Ua`u4IQ!{MMmk;rIMW@FtfKGgy9VIF*pKW z6_J=hS_p77qip}AV4XY7oG-atQorU-OyWIylzekHw#x^F;)(p1KJ1LC)Q?22>bq_} zAJdMKd-Zku#@9&nvR(nXQTa3b8VrxYqNlrhcVI>udfjQZit0`k!3{Jn+*3 zX>)FB#7<`17rwdJnoOFa#peb%fdo=`aX#*L+G0_lksali5(BkweAfHfDJK$8s9$^^K^qj$b@pN#%5WbOc+n`$Y_6>2SND$`apmOm#hp!?b$>Ag zhF5m?!&fd~coi&@5t#%;n+PHQCagQCz%TB=cBaXvFYnh@sX9T0?x)(s>fB0-<$_*% zxUJe#eh>U4M41NJOuFze)J02n-#YA$8vPm`)pI@c_^@W%c$Rkz$$Z!Exl#F^?~|!B zDrJT(lXj+P5#8Ffiot=sCuK?Y{Mk$S&0$3=C|l!r`!BNZIwys2ev}(x{6N{ z!Ym4w1xc$kvrIJ%x8-aGD{SV0pN+<=r~7L^`n3>MywAFOUgHFqqck#lTgVeQ(RBXX z@(y+)F&vpJm~qiKpT1w-Xz{I&aIqnx(RODzsn5v$EPdW`ZnBAue6pyCEs|syV|Wf&DJ1WQ1-y(>8X61qU0gdS6!Ib3&A!#Q+8H@Yx6W{ROQvwJ>sfJkxg)5Ulw_~&XMSHWZxv#HN~ENd?)w#$_1MZt zzE$p9OLQ0mWozXd1=M>KIX?(6RcZ{9ALXxv_!03t-TLLfov@ysOJI>$# z?j>)()pQ{#c0^KomA9;{6W>#_*!`V3#{Fw0G>@{lb?;_)40K1WJFQ${-M_|IUA^lE ztFQ+SmLRtt`9PY@d88(=5Y-u{QE%{&pScMokrJ)x@*R&AWuTdsHCizUG*kdD5l1^Gn-qd z?0BzRYeL43H9}kh%+r+kE1dbf_l{t=OYQH@Yelg0kp&W4YXxEhZ#~>oz))p&;X4O0 z7I&SH8`DNNc#D_t)-@>&{6xxhmc)j0XgG{MJ{l-ff;1O=4G05;`Vn!GF^igOyV9_B$d)5c!_9t7UYmP-KLW!K!N z30r6j#$+vDc(+>PZqkKkP~|~ArZ|jpaS2qAj<-=3Hl4!%`C9O`+z2&sx>QvTHVOQF zTz*mH*UvVO4CC%b5n_i7?c^JmoMn#?jf<)u(1j8f1rCr=4CFq|glhh5+P7ZGJzY<>d&+p!2#qVyB8=_Jz@MM-%AeADixL zNJEN-hW*;i6)(NV0_S8q5!(rY86NlidjsQ5@9DjJbWWAn__l07%e94t;{&SaA(hTS z32MkzAiDekwZZry6q#}&GeTM8;#~W5+kX2y%Mss7u5B~I?_s58y5;_rWp>>F5D%Hz zP$yUq0meu#CDdAY`6`X3KL!~7p&46%Iph@sRg2E zBEBJkpgC&^h7azUpJof23erFtam5BwFFF~u4#u$`BvexkkQJj27>V{rGnjgYl7rbK z*m4^jx6ZG|^6hDkD8KHcsBfm$5*V1ve`9fM5)?n5)3f5xeEPh9-l*C^spoimo&R1) zfso-f-XQXl3$0~LV}HMuEfZq6(HDxdTtlp6w@N_?)SgrWC`sr#IyhnxR($wGzrDxe zALVCB^w4;Ho-BE7jGywuU0%rJ9R^>oY)X;Vk5-GaDRyO#5AF~b_dlY=5Z>>QNfvvq z_WaB13{gkkmDXmVlk>NHX53^2>9;vs!IYJ*ntZR4gXUa+BFty_1=xYN|`J4U}K^X-2i$(WnT@NCQO_Kx~i}PNN8aP-j z`$3~!eI-wixl5;2;2fbq_i0Z=na7em`=c_N5M)|&ywApdPqRu& zWMfOunH&NPS-dSo1Xhy$in>8{AsF12|*A6{sCLydP0$2#pIPTI2aiVE^@Tw*)%o`eQBJFs{M##tFm{ zO>9{7-iUfvyoD2-4uZw}ROdBz-QW@GE?;hdJ)oX4v`nPB=dK;fVd=80v{H8rFPnPn z8y6nWnsuX5btpL6_WXxSRe3JeGeXGHcJSEIed>Uij!Xie#N)~OBYxe;)5B?B=+klf zldX>7r+t#)&Z-<*<=0f68g8@S^(BQd%(hfbg^NgbMy`D}OFQYTz@XkEk8v+wTZXo# z3KfwS-(vGu3}me|0KxY44~NC$@EC+*wB&|n+hWAD5EQT&lHJAz8%%Jt=y^DpJZ5Ahq*Vtuw85=RP{|HMs3?}bIZsf(>cLQ&~xff9F zB}oLI^Cr06k&V~9^?Vaz9CSk7yQ2S^EtS@?`{zr}y27lO$<$S-Vl=%Bt{NhN*A^sA z98Gx#ta6PIgYNC9C0req& zfNGFV=nt6+ggT#|KYV^ghDp%ElZ8Fk-YWHK^DR_ z$$9~;zNs;@lR!b{AoAzFJZ`k0GjWW8=`VLsTvcAdexIM@KM=?AV!BfC5D3$eboU|6 z(8>EyNyrR`C-!q{>O_LwSXNRIsUbo4?G~w&6v!Cod)jC=i%s0t{HiO;H2qo^_cFuD zR}u&FF37Pu4YRk3^giA3C@5B-p9!>-?HqJxb8g8=(*)0$hl=+ZzRK*i>xI|SdksDt zkQ=EF1riX!uFvvB03tLAhKn74mtxnD@zFn|3T7L5O81JjneC(g!n9O7f`ZHDgg4pwp6b8YHAWI~u7V&XE#0rYMSAql(IGC{|Fn%z3p+ynnh;-I~Cd zvYAMH`tvyps^a!pO(fPNgUN{qc@Vk4mtFo-kXuJ~JK#`!dK|9-JA)qpQ)aI|dyQy6 z8{?^1`vSP?*nyCX93RD*(aQG0p%ib$Jfu{ro5HD0n|*YKW{o7%jP#@vZO5Gtv3@I~ zSp;Ub485CiOf#sC#jFV2#_Pb`el@`qmqc$nQ>Ob6rq1t264!ydrhYLrHwD8pvD?$l za-xktCe*zXCac|%)qI}gv?qoO+njf$->i&unWn87C`Z$JTv+O&u^tf;?F0iej(dXn z|FNTiQfZ3b|QUtAyB!0AP^M36|tj=@|PzBmNk)pNQmxrf1_byRQ{s)(7gM z9%tkhFPECOQ;QycQek5^tfk1)U}&C-9oU}OPs$z1Cp(}K{S&_4q7n+-syc$oEq!=Y zIm?WNxAF3%$aDTxq!?WCt&)b;|r(4I)cBe&mg`~#m>JmSReWafKYxqRvo zQTXNn>B>N6uNnTaDfU^5%7_zxt{Tl%In^NwSR~Co+1=s*x)iIq=FlM0X-%1ZTt>+c za>M%d%FmKTJu!3#PkhOHF=$&N*BsBG%-t0WFf`28EJFB-d13ZjFzE*ml8;s26nlAy zao4DdlKmO%>F_(P@4O~>o8K;^j@j^oolj|EDL;628*k;X`W0N)UI~wP?-)*(s#mMn zKc!~uHg6a`-vF=GJ}2luGt2IjD={S+qQZ|l3=5A>7p$k)h*F3k$&6M(`G7{GhV-r#wk%%MR-!`nxS9~iN zIe5%!CV#lqSExd=N5zd%Ah6N!MrK~Ez+FHZNeq%2>gi@nhXe@IySTQ%IbH-ZpfuTx zsRF9_m35gI^;MLUF}La+{9(rS@O%n`^z0w?6%BhHhB4^?sPYGU>a09XKW; z4-j;gp6OL{6F%k$sx8Qd_)L!8SslUSV~dr(SO31J$)q9Kf}GojX}FUz6rqU6_NX!% z8Odt#(#j_bcexp1RP@nr$>yF!!OTHba#$(Dif5zskApv@ z%a(WFiQ) z7Qc62h^;NyKroO%2c%?qY6@#3XOig}JWQE8ln;`NC- z^LI4Q1{W#?U02C?brOV%-IQee4P&$Brn4%~!?$a_ms5?T9?r5qL_~LzQaIJXAtN7e ze~zMaQVjKd3;<$lW1}iw5WWK?3lqBI+6AMrEAky7_Lrb}QDUxfDG89y z3E-QlDI1ND#;og%1UzaMYyR;*IFIlHZvcx_`Vvi%kLQkENCOOv`JP1LGTsnnMj@`K zJ+P2rIN!@y9{yIK4CRp2)G}^}e3G2u%On|YoNSD=IjcM>JG+Vj%$$Yf6j6&$~ytpgnZ~^DJ zf8414XB+EIy&6i!8%tkDSTm%GJ6Lxa?Y@-QZ}d`1p9mI^y3tD?O_%qy^|80fgH#aF z^J=?sB`uh-7({z|b*=>+#}-~XqS6SVOr&PS%7_uZfa|K43u+~NY3t=@b7+=Ag0$uB zTDNP=DCd3J5OPI=wLS);N17OM)%+f$wvn`Vte z-c+JgXtxK<``12NszfX*X0&8+geeFCMfs@`;V2`bGdO^QpN7K$c^SzD%hK#3jp7{k zAL;qAC^Fl^Ix$O_&$p2oL|Q40$a`3F6KR*4b3vnth-Oc8RkpEFlTIxNt-ljNlFz1L zMFB$-9I%EANZyf{Wn1x8J$Ey!zZ5F?aN7G+mm|?av6TQrjvGFg&GEvUwVE+i*asSC z{!}`YN_WHxm`dGhcV^)O+6d|nrnS>&tg{j}Yw|yD>+C9hLa@=$qo}lNWDo6fT5b@- zDcN9+BwIevtJ@20(@}*al?+l#aW~>Vy3MmfWzP-)#?tcyne*O|0s7dvp~hsH46=IC zH66{XhRIRPyTy$YGX~waePI+M!4NmN5l!FaqUS94W-l)l1j~sPXSmIhc@%A?5vM%oKGR}gYhR|DGikReqvRwohHPXexI5IF{P_GARhNCr5?%qC1s zkwKRsX5-;!8w$hMI;glX)`GdtO5<;&aCjc(d#C0Dne*fXt;cP`91p1s1e7e@0yUvb zypU!<3eIdb!~o6>k?m?Xw%N&w5dO;WwG-(bq8Sl?YgF)AVKG=kjk_NrqmgZI!cnvc3;qLBMIFX;LymDo{#W_%jdTC;iXd(l}{js{jAQ#p! zc`P=TVowd~yu^Fik;P!5WC8}s1RMzT(U4&Eea?dSxC&73{a}@ZrL|JfiO(5zdJ5dmTIOZ9`3jU0G0pX(fW(puig# z1Ni>-u$X`t0A2dsz%(U+x94*ndulc}QD}KC0_*Fn^Ddx8Wd*ZXs`~Tbi9NJ~fnwEMvLXSA^0#4|fi!M!^R>&nNrP_u57xuVowMH;a|SiRW6T z^UhVaF;o<|);=ueXN`M7Za)sETNB0jm~O3}u{+)2>SSPRFHv9?J(^EneY>BqnapS3 z$!mC|8yI&VBD;xieUBJ^^qt;+o3X+B$|jm^R0jeKU>44Boe^z+ z0zu?2=rL3o8Zs18zmtqF+{CJg+{flTS)zQ{y z*xq#tThht4GQ!&$Q#`eMHAg#H11V}7EV<4D ze-OPO$gnL6*e3<}(=RXoBm!%Eqa1B*UZf0UDuDIt;lvZeZSv-YqoFcidpD=&YY0iW zi4PjbsKda3h6&I{uP^wW2W~$WKque(aA5`m$p#u{RPh$`>Z^5rU!8_a$Pl4dk1;0LQLj zN$|iin?iuRAw;*^k$muDC8>(P#T#tA;DY@#*Pp|_NT!d^w3Q~DfA;E0hZ0gK zi|pyF`Os3=740S)SD!>S2HSa-J_7N7emX6Y`eQ$uL&}B&WzMOkf8zms3(aYyBOKg* zJ^_jsPIzPmh8*%4gxYOR>ssSwd4wF6MBhB1$qb5r8sO9WK>95OpL?->rx7vyfX#F@ zC+pRAkNCyRE~c^R34%sGClI2u8~lMuLqZwSOss&E0&>FSSEnQz_MQV-$0;zGK@dLL zy0;RK3OLMFAMf@A6ES z_L`HW%!>z%r8Y}AnGX`Tg*bECjCC#IKgdq4Q|#9tQVL_xd+n|D60gMCySQ9nxUEZF zxk3R}ke1M7l@2=yKG*$WusAz?EP{r|2?RV2OK>!}EQsx87|CKALF4X8u{sPQOO8Hz zRTFKbIk>WMUFI*Q^4{TZrUFPP)yX=;)RT#l1)jUyv#O9K3#%O?Li;qpXKa?wrKHMS z`o|sii+)N%96%}yxt~k)1N)Nu$TwsXlzU!)Q<@xpF4<+O$whqH6!Y5%W5!}b)kJfN z&?Ha2i>$$VjAA#gCOJ3DG5t9S>(PGHSwgX@%@I*B@^lp{ka7BHK)gdM`|io7XD_I9 z`>nq)mb9~jAxnwEpY|fCw$#2V$Eb?;!xe$tsR1r5W(8(!F~I&%mx0r$cz9kBv2yRp zyVRZ%1DK|5Kd6Pj=d>ptNFbw9V>Nc#(H;us}myVTb(y$o>Wa|LDfl)r}z>A(FoCH4w zh%C3@IA30930v%-9V3yi8YP>iKxRXDymfyy;3id4I=yAk0Bs)F7BGkx5QLMY4;t%K1rKm`7b7wdaktZf--4}Zm;TbF@L`6 zb`YH=^FZ+k1_u}@U`LPJy;7)oRffumG*5uHs8F+ zudTK8o#AO^tsFo8q7(}4M~Vcna-YpHQg8r8R*mmMuR0t~T?K$I zenNjXY8~bs^=U>&m?!(`5I0&{)3hB2ksY$j0VOztlHpu2;b{+=Y`#IKv`;STc~v^^Q~t<*5@onbJ`nV+!&&yh*f>X- zoJC`@+DB6)+fJs$gjm&;A}S8bEMZKGGtsPxG3pqZxg6xRduSXexJTib`vANW&(LIP z$!2QVTd`8bwb@N)_-dgCP??9mg=L=hWqvW->x1a5MHed;2gV1tAJC3GYpFC;3UE1S z42j&^;b3WIqfTDc;SA|gj0Y!43&tSTu1BUwo>i1-_07N08;oJQlQER!YSpb%{Ovwd z)8m@_bm&2F4uOV|MH{;Y|8;?9^0wS0JB47gmkccs|7Cyxbs$QAOYl$A7|}$soUYQI zkj{TZ5^yoQdZaU^uUyqzlBKaI2R{ad=sx}|L=0a)pe;HyK$~3gGb`QJzf6wmUrp{I z0ycj?544NUC_MU5_4JUPPv0#83S>*Dl#tSx66g-N{ZBgJCmBw$UT;_*3TSU@l!E># zDV^b+YJmWZY{mcu;sbHLF@#O-rthVs{ov>{oF~dp4GrRCBj!!e(3SuffVOzx|Etip zR05GV@}Bv<#e)E0<5<^0>o6U=uOE)toaLZKC2?1R;zv5t$~`%O&eo26o~q-%)a@sr z!E3w1=&l-FN8F&tAl$YBLa9M@h-CP3Ab{*IE3WRMCpb>r`)~Cce`J;9(r__x$$*@s z$xRIX!1(9Wp`j5IhjX3B#%gqykHgodF}7-`udaRGlnr|t;IK4VazuCFxxLj-1>nFh z)4mrG%^Rnt=Rfwx$!^vc`)w9ceiwgsV$E5%v5wDDnk1r+jTV>D6+txjJi;HRV8dr4 zZ*R|DFMG&t?3!F_zamZnLyM?Ye-(}F4QHj!l<2W(>B(8w^wvzdIS9;^$ri(WObli2`XXy_Iy-N08SC?k6yfrHql%(2D057(aZX!9242 zUz7T#wvXJh2iXA9DOd<_$ zl%s8ySj67gUTN9kdjI@3L#G~YO40}*mN&s{cWlraZ_DS-mKuw-*Y{!IwWaXp6wkwG zz#om)!f~f`UMR&-S(NdbaMe9*Nf|$&@dv87%-R+tzORbk0x}d|<`m8=TuyKOcwG|t zghKs74grcU#|!X{N6V?UJ6Z9@JYE__?5oGILy`{w+yT=7QB53DN&73_bhsEKm5!iX zti12wGOfjUE34H|VvN!+6GSvqefM0?oMND)mK2Vf%DBBur=BSHi(%GAo;u1>fXh(R95XCeSFrn> zV@wPe{U-IHM;kD=@l~O5J7L!ZaAQ0@Mh6aB$NS`!s07%W5ZcxbHDw>vBfEGvyhD%~ zfU0!lvW06<<#==FbuQf)`>sx*Tmg-ggT)1F$~_H07=J)9d{c-!M+Tjqv1INxD7=<5 z1pHQAeaAA9`Q6Zo76CfXym9iW@{gqI3#)w2m|GdPmaktPkaY>ZHPV~7opPE z1bkaM`)WTDlv|AWbgKKlQChbHr1o~=uCVeL7R@}c&sZfEU!sdFZmh&Wovxktbf-+g zjQHb3()V+gcY@VeujU$*dAZF;aTXj=_PF zCk-`6JIPa87XaCaYxUE*ztozgEo7) z?nex3GAL>a=!t&cHgFUfW>9=1bGM1}w5!LufyL^(XQxc9(TvpH7arH#wXh>`V8^Vv z7rwLH04UiVqnvMv2#943u=FP^I6>kxq|fK5Nr%cbdVc!avNP}DMGVLcZe)G;l>kp} zn>TO8AISv6mTwpVZC_hj12^oy^81GM1Pou0j~*0?Uh`{LXVIu;{KT1nKpDRtma|i5 zQbvTu71Jw)mV6|D`XZ@>Vn}(AX<{KIlrHyyN1vU)k+kJf2)qrTS!;X6Ji5ONHrLVR zp+_OOqUDBEq6;hzA$l|D%Hh9CD?&vayNv5@ndRK=)#V4o#VIcRp@L(b?;r&PXtQo8 z={bDnd)}j>_qNhC3Bu2Wv3l7OnfOVC+^tpNCDFg;< zwh^*GGVeIX7KQLkkge$5nSOrzMPB~5y)Nk5ISG$u(d)MSUOQeFtekBfcwN0%;(`iLXfd#-oggl* zaywW+(~feMv8DVaS!;TvbK7XlJq8)11rn&PcKi1w<^dQV!^|4i5$5hb&diBI!&A&p zI52v*CtCYz3>i{b*Ycd`zxmW@86tr?Krn67>6I=Uo#xgyr)bqs4nw^`sgF!4&T;7i z?__0;36Eiga!Q5^t_g%LdViHK3@o$2rs`jbA@?4J5-JPtg&@S}eFjU6(ABEDaM^HY z<<&u7Jj|uL1Kn|*ucq>CW5R#tONNK5r5oA0IDZdk9X>51eO{;YBDC=E$I#kS5X`^j zJ%iG6dVUHt$YXI=O_MC=z*}}MWaY+=v`cLJFX^tZi9@yi4$7@6#44=_P}x2XIenOL zUgw4e!({$dvEj#oy@6&QV*`R2>`Js$)O{}C3l@(?6))*p86Tn5=N>{d3)YP?5Cn?M zZ6Ws|OCL>^GdUIfBs-Nb=QkK;m;e*{hSfA7ioC3Vn0L6FEx|y*jixU@n9$N6Ga$1$ z#PBcUvH=jrl(D5J8X!F37+ZhUVQhsH6}N|h00->`W4#*zC`ng=FYMgU>CL=At8H)T z{I?-!h0%U7cUJY6!swgkfKAAyh}QCQ9jL<5^^KB1*McGNtH3?@-9q`t<^4_{-L92Q z3cBo7W5vG(0bJM{kzTpZ9D;c$a*3Oo8E}d7twQOLUsVN=7JG6+`qtd{-C)LR42mDgW$c-6HDA2ap@kZ52;j$VUt%>?! zu>-1gVHPrDOf(S%6rC6|H;~1b$XyvxMVIRQdby&}mr8w~%`f!TgGOJ=Qhrx{HGuxV zK@nI~y%m1SBD4|ED8nE9(}Mz5^Xpam8nF2{!pO%O{|e;(x_}Nt1RZ&DrmgsXxuspS zTRJA7yYlx1grzY+BidCFvTSG@m3u|5{l6L;TGRT|!~H$}1{E|!S`pCe3MX&A0y+}K zsB7>N!@~Ld0za%mw6-1lG@}pL1fz?648i}Rp8wsize6TKW`L%I(M{dMtnm5!0>aW) zK*wEABD8q`Cv+cEOx2H*D-9n5^Y7^amJmJqtO=_R=-uJY8*2JbU-fst8r0DRwtpl&{$7=TTQ+z99e}?5u_m`c zfXF^5TW(z*NyNoL&kb#Ex%#q$`5)K(b-leZ@Z43#uR`;`*XqCCP|*WC8Pb~^{Ckal zZy`V6p$;zWKmY#yz5xQbTq?{i>)#*!SDTlg0A*$W`1^PJ|9wkfg8m;&Y>hf1UH?xv z`+MaYG%n34-rE0b_r)N>#=z<7?eeIr(U(s-p>u10zWm~VFgwqPT;TrQz#mI3nudOs z>k!OwncM!;X3_7r2?4XIK}8#2|Fh9wH}G2~0-8P+dl&S#qyO`bktCqi#0kc9{IL;! z-vCf20ggE>_aW-Ht^Mv{gAxFwb_nGhDgJEn@As*yK-0^o2vT$k`QKh{AON};&B@>W zkGrU07Y&NMX!|68cjk{CastqyS&6F6;@{f^K5-KRnoiw<>dF5OZh!ZRVTKW~6t_@l zCEkB*x`!EPde>X}#(!Ma8nlMR>A(Q47ZaZb~kr^Qwk(Is4-YaEiq_jwgic(}Gd)F<>NU}#(C@T^v zqyOh?l*Z|K&N^girI3k97gbWKy-P1)z%f;5i+{(rbi;?HR%5RL^oR;>^E{r_VjNIHt4i4<5 zRz?<1Mz+rEc4jVc2tK#9H?=aiGBaH{hntg|myMH)jhp8v7Y`$kB%Hz{$;ZRa&&zde z<$NOxGrQFrD%yKl+1MB{av$Vl=Y(7B(cH)_*1o;j4 zc~_1(xxsC@xVhLlS>O{{GZSljxC=KY7bpC1!J&gzPG;uzaM{7-vsNB$=3->AGE7NM zf#Yn}(ua*)?M!SXxp;VvxE@_O$IZ;i*~;E-^=-J>x!Aeke#ncucsiJ^9Ja8xx3Doo zj`GMba!SHy$Pdqf)yaWB?2K$z&sBlA}+}>vO^;XYu zfH(iVhi{;8uLB7@DA!o~(Sy@=FJsH+^Wt6r?YbUQBaJDou zwRc~+e)-V)Wm%cXwdr)Sw}<_v<9fn%Uc$xj1>kZPzYeTjEulX5->SPDdW>{?$adI0aS@TCTGXUjEgC zMyqyd@#Xf&^}lEB;GET^|Me>%(eXPA`T4S}y69hhS$H^CUu11rgam(F6@HA?KW>`YgGl{T?4a(1>d`PO5;b+3QuEM}(P1jS$5 z*-u?%og=J;%JpurcFf7l#>mCW?b}%UvsL&#L-;xn6?-el-d`M+o9{cz5ai@z7g)_h zYfp2wcXcu`TfOr4`Qpk0|KVeQFvy=h*2T!l!p!Agcr4F>KW1=Z5f?HQd$L{3f*W+W_8@ zGPQDJiMwcM{rncjPH?m35&xr`BB5*@zyc72jA|WJ9@N3Dlere@|w zuFDf-WMgH46dfi263qa0Ng=4p3c#}D>Ib$~rlts#`zdn!oyq5qL~1jP?60*V)C$1q!Es!*W%W)eN-$EvrTDp#5*%L0MH^M(L>J zQF&#hAMf+G@qfi0UvR(|)bo>w{+G7?HR9mc=C9(p7P{q;7-f#sj^G0TmY_D2`iBAh z-;a{31?XQbMDnc_L|>|=wX*fM;v?^B%v{4H>%kCWm%q(IECbj-5R!5W{(wo5to`l! z|Gm;{D-9gv>Jt9eQuF-jmYN5a5n0J~!9dN$$;!?Gjv=%rlK9Ml7~YgZf?eJ zf|TWdwMfCm{T-MRSVK+g&1cP2)*~qa&M%IXVSEv6H73z++B! zUMQvzHU`Nf4RzW96UUxboe2UOPC%^D%J-fY&6{dY(r zHo>*Tw-z4-IKgiJn>9F_;F?E$83NxghwzuJ!FhfxW&T0+OylyMesJ^Os)^V8*FW;ezgjdGT!U|4Ol{5f)_En@UniRX2>q7v zznu}HkgetMRgn^Nl{MxR90b?WQWl*qeH>VI_wAKvi*bu9}#U@tUH!3Y~w*$p1pVT-6_WzIR!)f8$x- z-*nb!)uSR&|4ID5%ImMM`~E9b|9>f|^RBwVs*Q39v9I?LAzq%9 z7_u$_t%3ISDSr(btmT0Jdo6(LXGs3HjZ#_t=pSvBUybx1+4bKRxxT%U`@69DS2W3= zIxp8M!}_cI{^u($1=iB&x)AWw%98*4(6JIZ{zM_+XY>Cd$@3 zeE$La`@ZPo=4AgOCL+J^t!Bk_MaZAdAb(!;{SM&ye_r(Quc?BoIg(9~7Zk-GTVK}A z{!dH5wcD(r!2f$1Xf2R^Q*-|i4>ZgytxQ0${a>j1*6i)?st8(Ir#gk zzF!*7f538otonXAg1@fmu1ZATnC=fL_6NxQhgN;dn=F4DbpLErs{-QBF#CUFf7$I$;vc9c{=FX)HX-~^`3v`VUs;JyY;`tfv@^8VHb(?AU`Qh7ozS&H}FSvH#A8e*s->vcc_RTCm@B8gHE12wu)?N6P zt>tg+_{E+WuGIa|!&G+x^0czH@}1en;1DdKLFFzW5hd z?sWlmtygw^`_kI6|8W28S0`fUTs7J?o95@|Wfu_oYSiD^^nbvs)*N?j1OgyS{bUQr zU&*U}x~-At57$%IrKUgOKF{wQ?XT`Eg5Q5XfdJPpw?zK&o+2ppZB@f2Ao!hk{)sZq z^7nlGgRhTV`^K=Z-_*I@IF>(M`)<&c4^}ULECOHYgM1rhiq_^gSXgK*d1=X`9{OYP z1Pj|=eE!@Mw(SC=>htIuA@~M5ff;;ZW3-*oa=ByoFR^*I+@HxlC9i#dTl;+u0|wL3 zYJIu;44ayJrswL1PZf3BPQ=HEI1jthY(3W$FXUW5T^lDf%=gxegeUfM>#4nTXq=z? z5G&w>qxBcmjoStJ4xywae)vImqsX;QNiKSvpYWC_p}-~~{_e+5i!9QEWVG+dz)!B0 zKr!gQruqJoUGM8bm!A|?K9CEf}ZITBC$5t)lNE72}1QJ94ZohU49@xsG?_(8+mE6x@#3!S#?_&*gd8 z$nz&(%(#yPERRN~nRV5&Lm7QNrrJ!-wWl1psk@s=`P!It$aYQL=fPWd+l6epY93kN^f446!BR$l(QGSBDH>5TwuMs^~9$)x9^!$oi)oos1+k%e{D1&3_rwo z`A1?ufn0IOaDIyV>Tp%maoYQFBMJu2ii|xZ5A+|7kR1{Nr#!#%Swor@eg*TV5Pn7KT))G91QUK`eMI&(2m^bmr2` zNOO(Yr`Pm4Hd&P;BNF?q+A^<`Nj_!MakTc2$an=>?VJkO?|Rxot6{+)47vqtvK zb7CE#+wDs~-pa9h@%SaoY7mByl`Z7@RG0I!g3%9S?Jq)i>fY44K90$F-CguS>h#b& zkvn;p9`Ub-RXxamA>+uYg{jk`bG-p>3sY4hd8DW_y8V^pIyQZn(-zk}9~Zign%|Cd z3L|583Z=jlqHp0E6AZq8p`2%MYSERphOkR6vkU$DmgZ9uj`aNgP4vEdrg(oj7ke1= zd!_DA6pnU$=s8s;5xj-lFA_Hp>q-G8oJ@KD^U4g5{!nI{PTRbkXJYfUcw1@W)QWeA zdOdiKr>nMSfm-vj{6(f_?SRR*_)Ml^1>?D$aWz%%-@F(tYYE^f2QuCs}EZE;Me#z&Dc zD?JdMZF`i=-ffZK;}k}bZ;CcNLnTn(k!Szb-;wkYUGyMsyJEr>2C-|%DF5rXf_FPGk`4VYPk)U7VveTL}5M&z` z_@Sk`2)BQk3J+3&g>@cS_8IF1sy{xr+n&lMQ&Our_3I^AFe-ek8+U)2P4ST_nrypk zzV?LmS;Fss^U@(ort*NBbl<=lc?Ql?OA8GAI<`tefG=3eu$va&;ZOEjHgcScuV0NX z%X5=LgMH5=wMt(z{R~Xq)44Zemq;6u5*qz9^JKojAaozlR({-l^AUs$!fj4j~+dBgZregNHRR|wo7lb?bnr+ z6NI6YiTSeFm14rTZ7nU-7sFQkG?@J&=Ii6oIL-8lt~N7mqfakGnmn>oJU(6(9;hZf zIg-1Uqvwyz6GzzjowxRJB6F52UKe}!gi4RfM=*$FpcwXctpPV|Dc zxkuxzSHO)!%CUFgcDC9fc;KOp`rYJTtluw?tU;K+Z^M=TA%$YOsovDlL9&J<12K_! zmamVaQxS!=(KuI+~qfhBe9B=%v)X-54dkICkl zcd*JF3GwxiNbwWOb6WMSt7yE_nllV~^wN&pJlIm;Y(XXLl9FT7&TYQ|;y1fUukQw% zwoIeko|E%z4qYAA8Mb9hpHh7QMwAL?E`1v9jCX%&6_=1PRysf7R!w_a>%&Ahh2X2} zr)lmx*9%p>JTYD8-&#CBQDpnF*s~?uLfOD}AWqM%y>ezW#^TzPYx)-C)%}u4;id1; z-*!_dZV*M|m=VJ(;F zOxTJ{<75DO9j-0GI7GRS3zT6)>_1xRi`TE%oYYxDR!#0=@lKOZ4pWgyF@tGgb%N>l; z>6mFXSjJJ8jXkGcdcWQI@TvIH)K>42JGXUrzkU`cc@+U0 zJ1=YMTh%r{=<5&XaQpns)$OIH#fMIq>o=b`&7N-SJUKr;-gkO;u626Bz!=K4Nf?U^aYaX?q>OzzWoz=blLb+m|&V}P`6*)Rf4$D zd|HA?jt=HYeivVkHmdOeTBmq!VfxuU%?$mPd`A=JW4qkD9CI5F?Q7p@5oh$`mSQ$v zi`FB8O#OB8a5^Xap@LDnq#F-t?`k8kuE`El05vee~9q)FZ{;c4` z*_Yn-%zL|BUMS{{rWbX14#!&zC!8uR0jrwuO3f%O%*Jbn#IEbJ`{dkpGNMDc*qCBZ3Hj=4NBpHG6ER2wcQ3H1cY0opWl@2)HLuJ z>^LVEP<{UGu{Q;sO=^fOL+s(VqQ5okh18Pz#D3iP;B_0WDv@B5gq*?Z$E8}pBcjZ= zHfG6!dPC&7(PecF$JmpBK@h9Mjzto=iwK$p$b;LL@CX1`g6kPg(|mB zTY(Sm{nGDWKYygHs{&R-qq!9aFA5)ERNB0I8Xd8ezR!&MhuyBuY@5ip-aD3afA^_{ z=LL4#!ae;4ap^YsW0TY7pJ>2Kljg1@9FYo?xN@C%>i|?0d5!~*g{F*C@8vo@L@}xr zmq!HV_8#LgdAymzdc!D5{pZ%Qr9sTrPl}%4`L@mKnyyu^iZo0E?h%F};YJyv(I?cc zMTci=`q}jeXJjV`ZP0>ZJlNu;>E{ z;=`BPY%7iSBk9MD+QF#}52dj5zHXiTHeKx;q_hiCkPed0T9_!ORpd|LubRK4{+q{R2!(m)-vV22)B`zJV9olAshmB*l+r6i7 zjK*fn@t#C+`MZZTU4D!Q!1~9>^LpGSs0XUfx$jio(e!3(hF&Gi!x2ie z{KU&zM~L11VP+=nqxIZk6cfa{3GncVuX*^H;nG!JR2H*>Ld`kWt4oY{a|oj&p!EVR zG73GH^D}RIdbDL(OWrrKn-IKFcKnPce+&@(l`iL@2+t#O#dCwv23<3map=h5D_kaN zQ2PX5@vEuz#on+fj6y5Okk&s4?~a<4I6DLWcFYHWskT+!L02f=$Fs73Out-Oq$CO{ zL2eTgkhxWlTku1ID`b{V+>C$^-&<<%j&#Ud`0+`Fle29m+0aL;Q+!3#JGZm=qI$p; z(wKf+E?#2BkZ?)K2Zm&2uWy8R0Or;!CSs+5GKQF`g-S~djTPrLa%UF8s|f9e@GY!6 z_HO40$&#c(sz-`g|6D_YcToQq@b`X_S~w>aMIpZUUM-$#lN}wZKS``!rdn|Dl3E<+ z!-Z#`y|G1q@7&WxhGRLhhKx_PTO$~9tc6-)N|ZNn_s0*B zxIjRhMAqk@cnMRi0zn7KZ(CtHU+}c#w5WwrS4Q&lqXu6}Uum=`f-s+HwH{Mu)NkqtxhsAivh{8Pb3JL4bRr)9 zE}%&cQZ_OvUfr?taJ8z`)8}{8ZXYXh8y_p4uFp=_D~jq?8@Ya#$}a!!Ku-NoC_iErnhO6Zef>qIM$dGH~TJ^61zrg9*{7{9t9Icpl=LDL6mF1Pb;F&#n zo8p;~qY39gu;iZp@5gEiWGrvV8kL zJ((!NgqxmAIA*{I6O@Y5kKPVzyBV96tzP`|2?R#y#(K|=y}_ITE^PbwBgh`MTBgKg z^hK}cBMR+{#`!xTe}%VXY(Ul%U58A|^lRp#x1<~wL{A>~xr{6~sfV5he8`0gag{tA z4t~&`^YI%tfa&lRLwJ^jUJoz=Zr!e$qc3is8wUs!YLpa=lo=9y2xcZ4DKsaB6o`^M z;+k|8=A?>-izh0pWhq@ESbY)@;IO8<2+Z@E|ZH)icXRuB~D=QoxpclLP?%G z4i+LKJEWmo*2UI$ z@<=(3Tk$qWSHt_9+nmD=Wq70t&P8VBTmW&;#g^s<3TY-kBu5>43Ypln?kb-%iyA@y z$6B6eZI7w!b=`G^q9{VBcb}N_BNJU^I^F>+*Kn6rg)&%fr20;`I^gZHyK}CN;HH7Z zmOZ5<=h<_>KGG{uV;CRPrj#QGC6tYi#lb|i|@k+G_miq5MA^N;+VQKL zA`)${BK2n-k=1cwhABF}1NYA2$-_yJyz0@&>#zPu90jL|2z!uD#tVr_l1$y_4jL-2 z?T^xwzE;>0bx8d=6wJ#+jyew|SjXBhgu0$N4I(+v7a<|)n3(NGU=y>M$}JoOi(oh)e?Seq2N zQDI5MNO_MmM{fd)QRAu|Nm`rEzs+jJvi>$J#JK()v#JBp_xea=D`DTs4*lvKdC@+p11f!oDm$T}N(8c}#s5 zH^ea74H`2!5b1!L>nP4{-bbPp={qFcO@agt6>oT})|XdqCCid>G;%kNSR^jr6#sp& zsu>-O|LQ0*{wUJ99KuE;=L#J_TO1PS8k*v)h*n2H=MiB(3pkXHWI*m z=S42188aaKXMvnG4^4RvKnl#!G#>!|{-DHT^#GXTN+auD#=v#0q#>KEH%>xKA zVC-JHryOI(sUU99rPwUnyv(XP(j^v%D&4%EI|sGT!>w%8`*1$f11jzhqQN7P+kjo? zYPG{`<_P3ETX?VAwixT%WdIc{Seq~6udG1d_$`G{iDp=VR7x*BEhJh?(Zfx7AKVo* zh1HV6OB=Di=*#2-bL?r6s!5lLhPxKhBPlR(7T?k?ql8OQ;T*(Qv!v!{5-T@-O_cmp z8phAtS_$K4UAA;z-=Wz}y>FWC0vUfqeWB1pb@|-$P2QU0l4x&*hq5#l0o7)}j&Q=Xzdg;WLo zGR1FIa+Fa1w21}mJI>yfLEfuh8YVXG-~rnysM>>e9~0IcbOfvMuv0Ob)bB1ygRUmu`@&^24?GAD zu(VWntcH~f+^p5y$^;Gs^F3X!ks)m;2~Tj5MbfIoIM}1Ux5?Eg8pv@qa;XV^D>rSR zI&?V{-o-LK9C?@JH^j!S8%Kyab%0Gb0Dw&acbK$0!egr8z}9x5;W!gSL_^fqErPvR zR04K!z`A+4=G7B0#~C8nn&;X67waYCi4VXmY7f`P+rDnN-g4he0nsJLKsL@jwJ@;T zb+`v8O#Tyd4x>F~Wwu4*xf4CZfYPUjpJk*D!y|L3?2DhXm(Cd%y}1=@^Z3lp+?He3 z+HYT$j3)+c*|f-ia<1?6IB0k|%3>ce8v34Qqa8V^dLDg28X5YcK<<1PJ1a7gslZ;q zkm*)#`|16g+*cPA@H8wUx8DP%)eS`D4nWHBc^weZy0-|uA|8EN;;kKB3QX7-B5gul z)Y>hgSLl*8{Y<><>=R(}-cD2nY!%B-UwU$px$~-Zx)mb-6}9f^l64-g&jHc6X|V)m zVzy%A?zT<)(hS8HCIh&lRd%$#P!{h#E7*@;)ay^!a@Kuu-B{kT#UcvLoq93}5Bbmptwulz#wo;2a{gsJNc+n4^D z-zNwY4$RU038nnS+0O1E!Ra1iA_ zQ6Pt-Y@sn!7Jp}m1ra50rg48z>)!bG?lK3)W&dbthA1&_1N+Zowl|ZFP z7fWzzna7xo-LGL!Eiysqz*v6UtJKc5TxMM|$?^WU zU8+pZUai2>r9Sf=c|9jbQTY75h+L0!H4um*Rq4G4jfdP2VziFi3@TneOw7%*!cSkG zoVU$uUxMj9lQE|ijlQ&hrb*3CZF9}3#ZiNaQL!i@j?%dswpo?r2%9(Bnqd$>6V}>N z^5JEt_G9#(EO}YWk0YrWag24)#Be;X+buSgVqYNev26%L<4sIF+jHuaCeg#qEHz-rO6lLw{=StY$q+?$)*l}5#7=O0QS-FZ`Zt7uCa23V#dy#`Ov{S}U zFR=s%JysT9G>xDVa8swZM3~ZU!ZSe$Hwx;P(*cB#CuoT0I|q_9*jFXuWu9*&gyMve z*xTaiuE!+qp+Hq`e^A_;>JD{O7n9wq>hs$}DWK&d0cpdqBda^;04nkwo@R~#^1UCo z@qe;T%L*PZdRwZ*ib%{|(Wf#@kLjQIy!b)MLt;;_AO^)M?I0pFaE52|^9|uR{Q3A) zCnBnnv9b(X_1s<4aWnhHm5UP-LYPlH*)8ahC_059n}wDOL2|{byjkE^O`%K=i2NQo zmps|Vk*VteTE;l=Lhc%S^YhVIy>5Q7xKg+pIG2D^sbErydp&a99-qx-#If~y#2N7k zDlN%L9hkv=`i9y8d-%+2uLL~~>6s|`pzr8-W5@4sPuyr!mJBQa zDIlJTPzo#7gB6{rTfL3Lu3(USm?ofHLNdv+`R?WH3+0FWdW2m^UY&e&1Lr(WG!B2E zmVlw71Wbp+3(0NQ&xP!^$#k^sy#h=BU_wERi>fzHp}4mv^U_DK{Chw_ri-P^dy_~J zSlB zW5%xuN5q{1$KLb<2_c>Sf*PztGSC>&u>j3Ng61I_nDBOwfr#!!|DZEHU}+p&)KHG2 zai*BT(w0n;$cLbml5!8mzl2nsu9bxR7N}a&=^D++THp8>E{^-Bb5*D4vP+{4PPQuv z4Q|UTsNa&N^_Wfl9Sc657ENYMRVA%jF`9it3z-&!DLE?*F;1q_q;b%shz4;}P$Z{S zBLju>1)y?!nJUcV{kLzlqsk9+U^NcZY(pEkJeLuwB&?Twk9J_}52E;tofUAh!5l-# zKc(L*pT?6Ze7z16Q*FB0IqhI(@RMc(43%`S69hPxer3bONIeJ1hwpH(+3=?xhI^j( zi9b{sUi-;mcg1nseR0rIRZXCOGHgg#{DDi_asUf}0A_mT;;{lwB!KN(4Pg89IZbW! zNw#u^TbN9kQL!-S6X#!y*pI!t#J_XHtxqQ^J{(H-jYl2C#QW|b)29{N&`uk2gjk~@ z{)W%|YES-8xbx=b7 zzFTI+>>>6ke~wnvX@*S3SXqBr8lU&i*)I~r)@5#rV$8qYz|?;fEk~Soac(zevHYF? zgDV-u@4}o}J`Rh2e#2jq?`^1F@%CELBVZRt{P{t9Xikn~T&mR2u^rR_|5fHxl%`K( zBR&~o;HRTd(2vU^n|0>8PY46V+??#Ey_7Z$1Y_->nYoKLb~NJgPk;QRlkQO==)^%1 zfWw6!PU=AtyF}eXVWd55__6dZ;y@-$)FBLEAj7Ai%v>OB^G6+@(M6HS zHDxRHQ9_H^Ljv1!ubURnt|^&5B*iU7`CJ~9(NexU0^H{ETnY0FnIuOY0@@3WvmLPi z?yXZesT=r@Cx=m(Vhjhub+EJ*@KH}RT{yH%p{HvHg}%d~H1g`+3cQ#)UMdVHp}}*4 zs8C!CD54X_6b(`QwjKGb)HazAd-Yl~5E<{z?Q6%O*(s+JYk=Dq7kg@Zb=X4+A8DMk zv|{*`8H2bFv3Dwk?2|leN*hmtdc=rKS>lHYozOpf} zmKDWh+mv!X`;%cUi8HrCu1AG^2y4~uQ^jP%4gwpE9o*gw}Dj=pjhX~6NDL*UeB zE>%KjvdF=piOrx@N!(NZH21{)O=G1kO%{8#`tf+lop~~Mm1s-x9Ctg&SNNbPHt~Pb z_o_cyk!J{3c2gWW*kivx3`DE3m%y>nLABC&B%(Co9Png zkzRg9CfDIJ9G%}Y&zm+JSDup3*HUY^nuw{+?2Tf0QzUqZC# zH)$?O_)R1sFOrn8DnK0;yY!qkkzSIv#6P5B=yY!=hHJ8jWK;6_yGNn;bO^0wev0jXOa zl9_U<{G`>yJocd!Mk<=|bi}*imdx=ks(Xm&JZx@xR4hpO9>KXjJJBt2$@;D;Qt=V{ zS^B4kXX6J-!f&(2R7)#kM zA-C_)F;!_|Rj&)_=eI@2+T0H%+R2SsEQ;1AGeSuw0ntA}d78#6rG`7JUU)R$<$O%g zX3C^XuNI`rl&|e?2go{8IDO~T$H8d#o!sfApGFF;2M6Ss&Vme4xJ3Hdq-3VWnM*=$ zP`&_K5yqhzRWF6cpX7oDpFx?`h8W?t;`1|s+oBP^U+YnUO36kVkHi6G^Q~n;W9&(X z#zEN_kc)~)>`)Q~QAEqKcrlkNWxzRt1e>`aTHd5)5hE&jtRsTIquT5}I+;D_L2g&g zwS7;aoG?eaCw3a&?^er?F1W1;?Hc}N7i|L%bHALHt;KJAc|rY_=k!q2XnSg)jFvOC z4_c?*zHD)5h(bD?r(W=pS@V$<79Bqtv9SDm8sl`6AC^qcmq{$-EL|wewZkXbb6J9t z$%j8#OTVG@@biZzM?vy=BN(5-+4js76 zwY?!DHtI-I)FwAQ-Z!Y@K8?&FGzQKjock5R$GG-kddIS#xoJ>g2GG_7pP$_d-h<1%sb9-VhUb6(`GJ z`s$xz)m()7Lgao%y~tQ*fb*iHmL!A3VM-yVq(x}0$bF$CQqVIe@|o7~NSi}~TZfg> znL?<3ZII@k&kMuiN>~**CeXDToU=uDBUF2=%$a+QrVnpwhE|n=tb>!14*fk@yovf9 zeP;V&Kmn$rV0FoKx#K38^pxf%(rPjjduFG;WN)W=tnCP^)7*zW5RaX~6N+jjLQb|n zqj{*s$atn;ivklS8ahGg?|YY$;t6Xa@#T1g#13cgdt9ogzST;Cx(|xY^={AsglaR= zbf=zoSA{6UMp3} zDSt1*87hS>BM7qffto_;6+scwnw&!bE&#GNLHZW_!}y0DY`Bi&PSqj9gs*F&amrSOM@qKU#9?McHKsf4UxNvv|pjfBC>*E}AE z>RmzvuTXP*%Vze1$FHxLoOLuN%0d%c_eP~k)9OcV$EQg4nQgC%IFfFWK^lB@bno@s z&da3JH6Wd;oOPS-_5*pd4KyQZLs}s37dLtvlT}lEgjU?PO0PBq^|rd{N)=yFF0S7G z0!Pi0zx$)HzDJoi7Y8N3&1?36m`~8*(Z^p~nZ_yQC^3aK zFB&hNeMj=B(Qj_EzAhdB1*Q&#oluR;(J0!FA)^7e-z7j3B5t&; z&ecdoJszokRP>)}Xk$SkGZMZ8@4stQ6zzh;!q9vEid9 ztv>y>hCwPRuShvU{QUSNS)wSC--|qz4b`H^-%)-Q)_Pyn5n1ZNrFtgHXkwf_bnHkI zMkvX~T@JpAae~jOFS6-RHt(jO52Gk2%ffdLu01T6pY$Hu=PM7;ax!b+Y{Q^c-+J|f zyXYE0gPv`CA}QnUvQ{blM>#{l%(xd5a~6{Z4dQ-4v(CSNqb3l1a$e=NN1f}M&_=u! zDdxDg91l7L8GnK2#ZEdE&KC!dP0o2vWgTG>?4dum|2`H?7Y>j@A%%%CM?xs{H)PJ0 zoB2h!U%#MGT)yb1Xp(h>B6(KisW+bH&NLYZ??kkI02}ZJ=-#0kZ}+vRZIB`4sH*;fly}!n`t#XjOB7Y{8EBk@!FaDHIT-B9ca#pZ6Z)bGn{farp(`c@)JZ!Kga^&f3bc^2J-)+2%@t*YD-GnT8o1 z(b6y9sIXl(3qO(4<+&~eNMp;r1kF$|mV7cvB)^1gF!1bh(Rd21@CRmnha$r}zE+|Y z+>Y|-nb*K_4)n8lB&*aCa+u2u-ILYD>!NIv8=8;5Le_pq^-h4trV~@LXZ9Q==BR!s zc?NrFAax#SzoXIWIQ#He1@Vu^Xoqa@+kSv$@$4r@DLFd5y?#eYI5N?_Z?$wHKbis$ zTK~?^Uj#>kGIRDR^g9gk*M4~EouTJ5(_H@$r^4?7i@9NeYyNzj4oWk+jm`8==r$r(|%f}Ksm6A>&lW^e#QJt$ijcAS@63v|eE~kso z-+@rZ;QFd*;x16e22}l(#h%k+&`(wG&5`A3|NMrG`r8~%cBL}MQM8T%P`I$229uB8 zhZjX^+Jc^z+lK2-k|uPO-PGW#h3ZeKP+g9A2Viku zs)Pk}ax?gsLt}Mph?`eLQ`fbWK2;MQaE6BQl@{r5G@w+Q>fY9yvge-eecleaDvOEJ z7=s@xe(Gde=ap{3y75p-c0P^eO^PXJmi4R4-IBsOs0)%3&U$783@4E$a=J(qj{J%G zi)}*;U4+L?=1``dBw{9d*YTFMsUrb-IfV>3gowt|u1NdyD?sUarlpwz;tp7BQ?Knk z_E1QbzHhNh_Tk{&9g#){qb|(FF=y3r?egNgvk9?U>_YY!WNQJVKEr2rjjhs&f?&0k zJGToS9Vo=P^A}Y;AqiiLI~oNH|s0nQ^!n zA~D1JLv7FYHKfX`qd_Oy)ZqH$)rxHR(LKuxjZ+Dl(!P4RD2mHmjqutoIP!7K@Y+c| z%*Ri0!k|!(j^re*%o9t|U~@NYwSbo{4>v$IOU&RS+k}w)e+#g4kG>Nq-?0k{>{$Tl zoB2kdh{%CHpO%rABFvUgQN- zxXy>S(?fO6RtHPGwuru0fsXb8WT#RVBG*tqvbWAC$wlICF6`TCL$=o;Ac6YCWQD6M zQsp5%%JZOEb>*-$iMUU66?(xQBNL>7Nfe44H;@K8KtC}CCFDMAQEX-}o|H<3HaG3^ zi_2?aMs!x`t`J~=<92e**Y`l{2#C>@KDVqxBywP)Qf*L}4A|OHB#gRt;+;ISge}@M zBuXxKV~@3GBi-Zfr%nxC)=abQ%vCD(7|W`%fd;7b^C24nC+6SF?+;T-giT>8Wtu5S zgZzWqQ;70qgD5RX8TzR7^O83e5Z{Pwck6JBF;oi&B&Qyx&V#0v2fcFL1eE%c#ciO( zb@P#^emmh*FOjkeFdFg58PjRR%x?CyX`IykZS}9@lgI~ z=#pLnEE88g3B977P?T%sdN=}a)DBB+20WUVUeC)@pM8pku3C=;Q`zkyJVBZ;&5&D= zR`FaOI+OZk>WJn0;yLK7i?4YMdQ-ocs%cbh z4{R4K#DyJYPOqC&qxn{LH||XFm;!CC9hXAa5X8(>pXoY*mSlx!kjtQwe2Q$!%nM!y zAR>o>*VyYHzdSAJmXR(_b)dH$;W=>UtTfWhbm@UB+j1^o0)M<9WM^eJ1A3^5ERAQ9 z52$T~M(C3$^6hH3m2&3ami1KLn&e-?q?c1~5QqdBFxhwt2Rj)d(}_+keY{P!i}-LZ z>{PtL#ZeqQ>_pWku(SwG(eCMc4NiVcc8;hp#)J>|xi74k*vTrHD$sv{0uo9eQLhHy zJq?O5vROrfh!_?I50A@C4;?ZHn&zmnB$PV6g?=f%jMBM-GW@p^yAN`Ya*$_cU-3Av zP0_-DwEv6Fk7cv_S_m?^|ML{Vx&l(vy$!s_VNLO{zqrbd2N~WPJanpp_{!wu2h((H-@#Dl<3}H{Ti$DGz zt>o41;C}s3WCy6k`JR=d#Ae-t@8k(v4LV`pL$<_pXaLvAg}Gub4(_c?4#VKTb|9pl z>=rlhdXqe{a9HyJ!Y}hA>E(OJ!A50Q@e-hD>iu!@NHc)Qp8HVt*>=1$FHcQLD0w1Z z$a$zuRe@~~>d%7*fo2NYlsJeS@#F|cbSJ%Bs<9V zw%HYpckSwHwN>bsA?|TOajf+MxciRCQTrWBc&{dqII_|NAR+NKj5zV`(*}Ywus_#_ zWUyuo=!1TK9{pSrX@8A-#|AgI#k6yBdt_SeN94EQ*RZWJ6VVLK-nT*68`;7pTa13q zRB?cS@~P$k#GV$U;g|W)W}5x^i5^pc-wNAy>AHh}>B7!}8hsTmY>-#OvYf`u#Z3Y{ zBu8`~&m~<{a{54kOXBN&VSWy17^nV9sJ(|LZX*=#Kvt7w$AddH_XDh(K*2Rc8jIuI zI-U^dYU%gdVMObFO2E$BArzZZ**-Wwm|a{xxG8+AH1?qcF&|wK;;!np02wmZlDYoy zWzlp@6lp1)P)yo*YXYiCZP*hPG5@(>_*yn7u7%zS{J=HOAWgpo)8~Xo_B^@~vSe~E z3;G7w0?+Q?ejnO|xr9(E`L3fa_3>f36q}r4+4L_xjUSVNK7k~YzT1F@4=7ZHm4C)> z`jnL6x3YUICC<0U9oPKHiSEa*t~`iD5(KEA^=xNL9=4j3x4axe$h~{>ciSLkjdm4e z6HyB{9m}=R=A4lDytJryS8kI*=SR9XKJliK5Lh$b3Us?%)M?7bn&>Wa*ETP`eDNsJ z5i*aNW=-32U+f9}S7Jjo`>1H_=`Zc;lryKYsJm*`Wn$BEOF6;LCa~?2(Z@(_Xb%=l zsH2p^#UU_gB`Eg<-LkdvN$-^tTpXOwnP)~Yp=7q;4DPW9(xf0?sgawPJ@eG2Aec*H z$Zg+>qg>oeBbidJ82C`MTJ!`$bWGt)Z8E|l#2V5bQcEKx#=jDtGMHvIP5}=Ey@o?YdF@HRX-kP-6t&B(Ra9pFpb<}BGao} zDs=wRm}pECMbehX!eOJveoe3uiQ*<+>lSI^PmiY^I;?KDLyyI9V8NKb%(j_}+9Akh z65MyN@#@$)8qv7qZEs_xB<-9-DJt{nDbKs@v?jeW#{$(f(&{uRj*}zy#ySyUR3uP) znR`Fz!M5+o8Selheg{I$!|p5~f7~i%HD)<+Z@?NID@16CpYyMwcc`Uo-AB-M7TM{2R5Je&DGqHz>@y(&wc^BX zL`T!ujejbHK51k9iU=!ECJtKkL`U3#*ia$ABN`gm0(?9xKGopSqqeV+965C%@uDI8oyy`^X zfVCcDKQ0}vYx?pL=w0fy$2plWE$E`=LX!FTZbi)v&Vq0oq z7YA|~tuxRILd+^h%uozg#<2VC7&;F&xf^$h-a@)zoPDOG6ICB;^~Fgc5g`7-f`f&{yLg~w+1!_-o z$=2749TB2!CKkZ4+HJxgb27YVxXXUgXjcM;f4(mgEK(+To@|=Vk1kZSFO;Iz8NcN7 zhf|-2?^aP_)ocVfA^7Os!_I|-6YrkbJyLd~G*%~`e6g?T+`f%mT!<-DOGHXI^m@U* z!vtg}J^Pk3%+Pd#4k4b$X(g|(2&3zK*AMu{VPaSQM%Bzq<$UHB75N>x1TtG5Ic;ba zKl|kDpuf)M7EdjGk7wSrQ#Z6CbxwMySl zEJm+J2m5H{2*p(9ipHjJ!CJNs8F=28}k#`b>iNW7+90%!ltX=(FCW|lTq{W#v3oQQHNmz>b% zQ%MF+iterhpE&3eNZ)TNV?b?v*LNe+$n!jhfxTD_A>C6_Gx8e3h07`r^O?jBO7}gM z4GY2!Ct?-g#O>cvdKK-_z;w2<>9qeyAeLVNvlJHYi^B_tKZ>xVT5_~@L(NJ~*}5_x z=JKes6@~s%ok!lkCZof%mF5fFXnrV1pZ87cC;Ze?%Ut-f*Gl!{-m7>Fejv7MS0+#!g!vlF|w(_ui(BhyC@vz#skNK!RQCf^8 zmByZ!pdHk61{OLsURAyGbytIOOx;*2Z$xF}q`lFiQcra_M@$&Rk@oRWX-1jkgBU(u z7Fv0qr@~h_s=L%MS`~*V6zsUSJDkc29;)FE%1G-Hh^vVnVb?3n=i`*z#$tM?ob?)4 z@|N0nA4S#n*EFB>r@AkWp1UzDHuLJDa^Vb5tMTHQWKwUe36pC9i4a)#zaEWNGa&8M zRT6hqI*B7o?Rv>skwrkpfzgJfA?6W#;cL2{W`;w>5}OUBx=E=b_Z+RW;ZGV61` znf!lry>~p-@%uN>LTRvAjM{I81H7eI3SKg1-)JqTqBUWrY$1jyIX35|$JI z{UYGc%%6*qAB|xWx+KCTU#?-cGj>O*ETBcKZ@xd~^)n>#)1d1pn@PF7=bmqlsH{-* z(|cw!@WoQ(Spg1}$n^J}rkRTnj?dK_EUwQo4XL+^yEy?A`$nesAsZ~#sL{d@g=*7? z7Ge(3L6C2_F^6wI>^!wk{QEiNwVqP2BAuqAD=L`r&SVnoZ*;>J7oPFKVzC!stp`kelQ)~n!GS9wWZnD%&wtE zD7|&BA0eRDqh{>tJRNK2+os_dObsS^OPt`#n>0^o^<Lu5m6l!ElGcii>T??00FRUA_P zGO&QsL<$r@8W;(NweyRDXEnruB+z)sXHr-Hlj-$Pw9Ks3aDe@Vv$$1vkR88Hc|MsQ z)%L?_fL#e6>pcT_TSe(5M&5j((Mz85gv=jc$33-0?@iy`#gopL5`xbYjx3Bf*;_P5 z&enZQeQqY`d;9Xb*zgxU#zH<0NnR8AlfJ-FtSQ<=pDLy-dJn{6T!f%~!f{;lEkU;CC$3gxApXk^;v=<;ugA&c5H>TiaAL%f*{ z>ytBk6YehJy7u7lRSi!+K9Kz8{jhp3o^RnU8U!NY76S$7+245ygV3aWxu~`dF6dBU zAKZmd)OQZme`G#j*lGA$jq2bgL{h3=^GL#N5!f&bv zwo~4}ON(C;;mKtP-5s2^RXg<=&;F}#03I?bb-h)77H=NpzRPk1iJ&9FIVk#x!wH9# z_4rYc?XCQ;)uk{UYI_6yJu{mym~WI!m&kBfQ{8qGT^wq0+I8Ev$Emq0)OrjN$?0eeX!zvZ|pSJvi07T_lU}zEPgEGHP91Esu0LkZRZ&CpZK zt7>cav7G@K2liP?s$2YbG3b&;uo&65xtH!d@Vsh2^y2DMO!3&oOS7f*Zor8pWNCr% zdtpR_<-r`mVHnvn`cE<9`U8C9z3{Thp#rcK7MdP7;J_;&uu%mS8mBEpK0Siwn6BB1 z-i=hW-af;*-}K5)n&FUqLnlU=2WgfqxY6dZq__q{Cm93th8?V{(eqfZrtq#vm7(5N z-r-#8;iq2cM#%s_8tw*|E6a;n&N&(>1D38n@_AWzpH=(KqQyFGmed~tt#^4b?tu21 zv_pY-pJ~v=2OVFOOI(?sJHG&wH!(q|9mDXi_jXUyt84&;+RbEt?w$_QaGq8isA~PN zbvb5*0WThR*2%)*6sZJ0CEWARNr6sh{CDJ$aRLuNB zT^E{DTH_SliK7U7fh%1JU&@E<*-m))gIlrmZ1r~p zC5(gOn68qf!<~VqTQReWV;uQj-@$=J6o}92R zcugy&_Gj_9^*xcZ@GtStRm6?(*w2{T$G-T@Sv#W}B8R`(cvt#N))}N3Wv#9JQq_-$i55SwIucZ1EX-tUzyD(2k2|=d z!|kQS^8k^Ia2bUP>*SXfz1=B}4DdwWc$mvvQ#rg4Y^XO@m zY}b8Ct|jAW?gPv8lIRMS*}KYSgQ`|Hd-JcVXo*$a)$sv7~v6hf(XD=B0O!LCN=dikCTq=~5^SH{rVSq%bU_eL3NfO>A31MEIdC z2r&;JJ3K%6X}{U$!lqyU09Uq?alT{)CvyXW67yn}11RhDfj11S`t=fomOyWUG7WH$ zv4S?_3lSWUkt}|rlo@ak4^CGEBe&}Y0uC)ULJ37I{22L*gqg6Iz+8soZG$ae{H8_JnUZO|!^{8jzH1Ju?36^XJ6nLuj`JVJ^`yZUR&5FQILX|yq7nLRTZO4@+FFVO_*!k-su8rbtV>%y(p1*d- za^NwdNo8Tz*Y5Qz8-7rdTwU>*|K3dutJ(GpRFg89C-DtA;HKwCK#T8ZEmQJ4skHb} z<3j=5K~b~Jc4leCNDr*|@{sAog7`&j6UAu{cKr5Wwjo*g)#an+@W)p=^os7L^H=DY zy8hDNn0ExHrWnZdD_b~6mU(`Q z-X*mL^)G9W-|u+wmd>}S;QB2-{JAGf zXQl`gJVyEdJces==1!uYd2a9+2WLhlZV2=m9)18dM|o5hw6MT_@MDDV#Y` z1DY{b-_atph)k3+YZmu;%gS)@<``OG0#`E)Up%`Ow6SeJ! z-6K!Nqqwd0`ff3WU89xZ+0EeLX@b!a25FPp*qDWpXSIs%Rf7{btHih!M=b&FkV<%N z!~=S-w46S<0}9@H?xB1*P}KE&7I*K})h5k~2yfW+OAD=NG3|&b`~nJ1K;}6d@auX5 zSKu;oJ~h_RT6nJ&0D~Aunn&+iufBTFnAMWVeT~mppA5c}X)b7Hi@TGo@3UaGY0Qqd z=k5k<0^qLmQJnSEw^AxrB{^mG4Hr9pj%sEv3rtsu^w--qP6L z6?L_4ThmkJ?lRT++JkrRvS9l7wS!{j-?`!#;&pydJu1qdH^a59eR9~Vm>sPCX!4+J z>AVeGx^H^xhT+wDoXo>d3IrOcLPL-uS@GbIi;$q1C#) zZ-2D^)gCWnz^iTJaZ4L)#u3&vA)8{&8PYf$@bOxKP=|THD9T;%QAe!bIVbV0obtju z^^Y=Q>*9Q#C;{k-mR0py78B4l@=4PB*kGdFqhtH;|Etj4fp~eL2BBLpjgksr6@oJ( z+ss(1;Z{~-q>pn4mLa-g>s?&hwL!NGD(#lJ1vX`XH=d`HSw_NoD@CvA1S9Yn2RU-JJ=pJfV^*7DC%BK4w`%KrdR(`Is%VlQK*5 z#jKZu#m4I;5&9by@wz}l_6We?uJP^pd-@|U0nXvLS2g#FF@F3`fJ;8Q);N_eM~v+CVWoAf*_!Pfm?+XUP_=Y9DF*wo`ScxbM?~JGY=+aB#tFG^n=%4Ur5xpO z*bVFHT;TVCI~?vnjH{lPeoFNjXSiu=nUoRe*8D%G1L~u@PA0~1)beXP`?5m%%-8!_ zPo-q?BY{A)yk}eLG(hp@w`Bbzobw-CZ}hbC6lVgJuYJt*=H06w8j_W-0rR22Fg0eg z6s+_(9pvaqrS@+tAAKuhm0bONoiVxviRM(jy!2z5G3awv7fybztMoScFsE_HZUHsVA14fKr9CO`I*qx3=_B5x5)(FK z^?A6~%EbBj$G;Lbf)Brj+M=v`x$SF7TaCG1O0tt@{QL2Cf#GTomz9oC*BgSB!EQi% zM`vi06=xKxCmK~TX|CT_WT8kLRLUxdZhI!9$~)-~8&bYYyGbItHl%W{_iWa-YX_J$ zyRM6g?g2c$HV-Zs;!@h$5!1=l*rtjL|@atEuhgG<_m&B5hnM1ItalTF+xC?wxxb zF|+7uh16_(CoyN!C3&JIQqgh1V*UJkJk`ddzLi%heCH8eCq#5Gzd!iP-Ab@^?B|<8 zdL)r~WIPZAKkU-$Ss5c*p9MK8*%m--nO{@f4_0Y^lm)QTIzl(=;U9WyQR;LuTvet& zUI5g899S%`ne#h!-MjW~v8Nf5J>-VbkkWv##d=0AqvNi#F|uVe5?R0uK3iDl_ERExyS@}i$G*Qxg! z!t|p_&c60aeO(>=RP`YH#HV$V@AktRuBMzp+TnCs*|Vau`!*2g9cGH+kdNW*EWqzh z&&iV54P1Lg49ctJGbwGoJNKyh5#HNaj*f3fkJv-_Z%8j>{-;24<64@V!qTP2T*Y`TGUp0b4?@ zU}P{&FdHBh_(eX*|9w12CDkymb?-aC8@o=&>{Kl4jvkJ`M;d-)x|m4E{==46W8dA-8v-ph!TLvYMS-nVTIOOwg18Gx~?fdbMI zOAyn105h#P=H7f*ZVAPJ*1tGhh61)=9DV*MvVyeQFoZEmf7EI3$NNCx6bT6DJe&HQ z%7ua5=SP|TN1~n3`||mZ*^=V@)`^@soL08YF(`E-;Lph?cZIg7!U-i|DuXsX0Ft6? zzK3k{gst}#pO-~<)mnVU+L4Zl9&0uee**HNVehUR?3lv2R#`eg!%hG=#RdQ{IVz$N zDCQ$WDY{*mh>9l_DY3~{LBU7BO?IFOZj|TFryu$Fu{P-|C4tgK(!Xz5A9enx6z61R zO?Z&SGDXI3dUalvB@JT9ktzWc0!S+M+XJrYq)MJaw|fABQr2f?#-pUN8GiYnv6K=N zD=g zO^4wm=B9 zFd4vwbX~K^pK9+3cG9)oneS#PYx(}45s-;Y`j=;9Kw<7Qt}GchP4wKg;Jt*Z z;I9N|GqXxm{u5ZU2chY#@qp;=}yz8C8md5tQi*_^@ z&v>k=nSNDD#5N%x@(|zsm!2_jB3i0gM-n7Op&I*C|8rnAG{^Lmu}#|_efH`T715#@ahp{ zjA-mAdtKQRJ(1?2Qr*TH`e#0hMegkhhaja=bECiiz-yBax&qwnJ`kyo$EowTo&%ER zkMq|yoHbccmmAc2L?$X;+TSfe!;6(O)=ECY)O%+QFsoR7y~J&EHfC}DhY(jYeP3md zi<6bA1iR-Z$Hh^@+V;gwK?gXu5@3)Q7m#i6#^+wdpv=FM0jQCdsvvu|Lgt8aY_pJ! zq;Mzb-LV0DOPp!x^PSt=(?R;BDURhz-Rm&??M4O)fJF!H>FzEv2a;TD-_RyTR#~SuMA3`YUzFN3}$j0DdE19J#rC zx}Uqui(y>r1p$>srn3jY33;8CHW&#UU;?RFpV9r~f2v5&`3Y%HI_-c(zin z08Lo*m0<&Lz`%Gb*tXM&f<* zQURQf0-HP=lSsh|+ul9u;%QflaPlpbnF}MDZ%KJlV>97c^?8q*w|M~g1Z*eEjtfIC z`6*JwRYtkXjDH0ULpr`a-!K??gNvmDN#KdUfXJGFBdf;i4Tm|ENiJ}p5i>E)8P43w ziv~ubxXd6e7n780nXH@>bS*QJ~^;-ysUBc#-uDT z&|c-a0qA0zUMdvG!nQUerl`wyAsq2e9(gMI2q2n*smsrDu^L!715??$@0LI#r_RN) zK|PF2;;7=!smODACgfRs2D0!;U9S8`W#hKoGkUo!@dX1mdxpkKtX|^` zOmgnJFkiG~PeaU`{Z#6FvXBjVQ;mR2apN3Oy@83o!8p@U!}SZm#~lVy`E6hq6CUY8 z%@bYJwKEw{QQt;cuWKn^cnTuosoS_32Zac2nAwHo*KNVeox=c-syMj8yrg%{lQ5dh z&7t@w>Z8`h*`9`OVS2ytFeDiMR| z6SW5QJ7xBc1P@O6e5T+?Cr~ZFT^`*f>%_z93X$+QsS10T3Vbl$0+)lTS|!&l%HzXmmDXF<5b-|KRs~4I#P$P{~7Fk%M&8l0FMuOn)E3Cx~RPP%P+&3COQ_b zG5@4i4`DxzwP=2QvnIkCZT8s@P;l#U7vH<~h_7f$j}hm`$v45UQgm?!__b@ca?}$z zm29{`ex&4FCl!AolMm1}KiyTVHa!nFjZb<705zqf^*A7Y?cS{3Q|~-02O-)N2^^4G z-G-arThG1Q+j{wDB80bzd|TP-xALlRYvF4vhhsn?uP7D6!BT82f;)v)IH#hwp6-Bp zZCfhtBH459t0`aSMFaG|VBt~T`wpn$>NXrT3V?_CP6?a*i-tc)!lumWrrCfU$T<7Q zxl4h74^L3IkbSn_QXqt$$=(*PM=_r<-QSe%DeX+kAmOgU%18Fc!Y z9H)hoNpYzsi`ZbxkW~EOy$W<;C`BgcfUgC2Gt&S#tGB+ph4Xa!b8%EV4EUNMNU5uL zo=Cn{h?;SCk>dLxXP4;P1ZVbFc}~&Q0_gYBjT?AbD>($2 ztiE#LBp2+kAl-I?JSa0TQ6B+?`=I2W zzp|!hfH2Q$yl`b?+NuiirfBoM=ce=O+*f$*6kzZ7Tww+9p^8aOl-v4nw?5IEA1z

$BaM$tc(3kxS)535m$7Qj~P-8zhx0>+CmLY{&iC|p^+Omd7QPLYAFf6izA(Cd2% zHHR5(j{ZX|hZk`4_IIE?OwB@f^TCOBRR(bRY9QXiDZX(AP1ooweJT}YIQhk-+%fN6 zIVC5RfFVzipDr+s*&b;;g3tNs(PmjPvT$AFN1k0Mtx7o0%Oibb$WC!a^B>$wy}4+G zVJhD}k1l6^0M%Loo9Y|?P_Wg#|@?DYoZ`eP|KtkteYb`T+=L_D6v`KeAEOd#Vf{K7k6b{-k)rJ*EZR1C}}6FcQ-zQr|wwc_XsH@%|x10{9=`j`t3zpOwn%?W+{$g|uqqzmR;>!?!N=@x~ zg-SQW>fZb4i2;If^ZFj*=aXCaUd+-Q*4>E=POgNE6L}N=*E1H2LT2XRef;aGiY6t2 zo)jF<#y58_pF7$eCa22AtUZl_;@r@VcEajXk_c7I&A*w&VViBA*QvkeAh=pEpm!9% zAPVmHRnSkt#q(|*uzlH1CogLJ`ktc^vN;Ady>>wa!eTAu)Umb~@yH<(0#O?vktvqk z>k{UhP%5BYbh)}HhRo#-ak%Us0U5sl3~b6iTHw4c2Hj|W(bDgkv98O00?Qw{d3oFsQ`AI6){vb|20UoXl&-YRT$6ssLu4J zl;P8*2ELq91+DKS$#~BFP9vxv*M@7LiH|LM{!lf&Nqbva9MN~V>pWxwx6U7e#!AUJ zqBy-Dz9T)rFO?QTHD!#%g|O~~f)JHU{A9L*I~lQ3U$LFk{1vogx0=IWVI0*Kx>Rs8r^!SiQ9ts4*pjk{nyYIG2|ygQ#kb3@KxzL7Bx*X zh>>jeaeo=yoqBsh%c$(Ue>i_IDwgtLxj$>2)dlc=cA?)zAe#ahbC0FEH#z(U$O1L@X zvmHZ>RI6tv&Aod#Fo<>k+04Xd@D6L6VY-r})}C2$lzs6$@|gmh1@kz&yZn#A<%$29 z3~TVYzox@j6fIalS^pMLD9MAm`;eQB9&j0p{tpDkPi9Exu(Hjvn*PI5p5@4bB8Bp}QYGO5aR(uR7ST zxIoJV$Yw?+qjKeNp)*1Zt2|AUI$8Vr{G!ow*t%I==VeITuilxOuQQLU&vmVeYyD`R zGuW$*U!`>Se3%ZN^d%(j&dS9X+AjKg_AUaF{1}z%^~QsAeTP8VlugB-);6O7?5995 zxe<58%EHC}00igP>V|G{;ux%**%oWbuv*1&0TFK?WbJ4E6jU4VUoWb#PQE zo9x#aTNPyH?N0}fYDSU0C{CWI*1V=>o)UI&J$vRqZ)?d2I&$0@hHm2;%4~qp3&_rf z$g*F^qkuu^eq&m$1P(w?D}zA~3XY#Gz8Ln(yPZF`yDkn4xuogM)W(158Ws+vB(n)@ z^_py?B)l%BHJs9M@$USksoCoh8x=FU%ymKy|2wjPmko$z4WPLfaI5S{xj#aQeG2yH zEq^RPH_#hYi9jm3;U<)Mm1w6RQTj=Nx4RvI;a*`SOR4scP@pld@mRI~Z8O08lH^JG z#K7kxk`-3L?3=+Wch(%)i@LRh2M&N!gd^Jy6SDNiGMJ}7ycp~IRC|mMyQ#3GBtON! zTrx$R`J@~sd;jhbEVDf#ysw}$EcA{SGDmrAF--k?@qO9LtmE?^3?epbRjPwecypIS&?uloe)vid<|AGG@_YA*ncwGhk z?5CN9*9$6|lnwRs9DDB$`C0Q*HMv}M2rGRRmRw$dWCD*}U({tdsKHGBwK+N8Q6<07 z8LB(Tk%5W=8F`;k6AHLYYe44}(Y+pzEFiT&5~=&(-h}?AF(ldV(O;)|Iu9b%X|dU` z5&%WrOg>tIB&67pISj-gC$941TpFz2Un#3oVNOmaPjQ@$azwUwWcjiw3)Y3;> zr)Tr*9@5htFL10EBRxPcdUm)&41FB(H9q&#pEr~|n(Ky@*IH2Q!Ql~NOh9IlecdhF-BiU5D+nI2JVOl!Z6MKuqkgqF4+r|%pUPtNMnYq!VUT4R z^r6JwtZn|o@{_si#+@cD7`_n_h)!T#rwhULYU2l?DW7W}9;isL^*R+3$?`*FF^M*t zR~PrNEXHTYrl_YW2xV#mwfypR z@{*T_D<;IX?4kL)$GjsXJ3O!eJpA7Kpt8q8(%e3D6RNYQT_9xY3Ml+zzxoc6s%NG0hZLI%4e+YeRy)zFUTiD0c z=_-2%4q-vpE$A^8M<#@sXW7v}xS!^UgQ=Th$_`cIMId%%31Vk3Datd|%-VilZ*FdL z@|B>`qTVe0C3=tNHZ))bvrA-M zTU#Dl&HUurr+IFX|9#Z0h~)DJg(|{y2U(q7Iv70VD}Tq@tmI(OXwKUNR{vFVShH{7 z-zl7xzN4qHwe=;diDSfeR?W!z|6Leem+nl)zM-T|(l~VfYWR_0{vkpDAL6DfalhF1 zaT{Om8p=qWv}iYZTsaaoOEaHm3ob%{Fx4Dy9{SYUb#_+#rl!UT?c zjauL0Yk3!ps!1W7va^Q+VzIf?-lWXe<3o5t>F=|Eugr;}zHPvUd+lIP=`k71t>q|} zF^B*CD@#(ramg3eS~R>CaUj0&E=*CTE&x`+8?L&V{DLYl(pompyn`CI`$?&rUvd0> z{~>9qDY+*FYUwXKHU{caD*7P!WBY0<82ho}z*m>AC?okXnK$i*HGvK4D(Iyad(%!Q zb@kWqXU66*WF!d2(ZY{=GJ!}@D*#4uyvL=Lp*hzOXaic~#fmwbhtdhMPJQ=Gh~ZRQ zbNS;(2)70um_$Is(Hr{3?0?RHo$mhcqbJO_0bTr4rOV0y=&oc9EbI4|yBUEHRT4Cg zeZ_FgF~500=%)qnrh;A)u-d0z$TzAK0^M%0zcx0`zmr!~zc>iZPzbZ?ulc=l2lR?5 z@j7|hktq0~zUJW<%sXIiqWmyV=STG)Zk_xzfW7sD-X(PFPnSCSfkD1kA&>DlkuQUw zx6kqZ2JoSd+P}LRMo_e7$&3W!=wevTy-7eePuB^g;obnWef?&yRQfM*@0!8iUB&J3GT(9R9lZ zxc7Y)ouIiU7$6&9txbK)aT2w_Ru1VP(-4ft*XGsERHCNyNN3ywo*rBeb04T$|GX82 z)N2Gf@60bIwYyBBwnM=MnD5HK^O-cNnoF-REiz}Z{mmZ3@cE?xqfqMW?hYb$!{Hfs zBRZf+`}g85Ei1n*e;I(=R4-H2?Okh;s3kK$vRQegbez` z1imcbo3vxN5fz05^3&!}1<>;BNM9NFLvmhZ_alItu0!_dXR- z3~qXTB?Vnm@?_>cpGi$ukX3bvMm!I_apluMr#=NaY8lYkTPzt`;W%BLM9HbD1XRf@ z1SK;%RnHw>zrfcI@IM5#`c*I|OdJFgVMxH}5I!9&FupC}G>dmc$c_fa2&0q0h(pX^ zd8squL7omTr;d{}EC2QuPzv>dUr4maw?SM}XDXIoY(D>elvDyBJIcs=wOCZP{#VWg zOXXeJ6@rVxS28Vx)E*QFkDXs*!m2cFZO@hGX&fPb55_@Wuck;%T-3EQ)EPH z@~w)3ao2t5-OGb?)ZBi)goxvvG5@QMT{1aFlf1LWMntg|~qYRC=ld}{&WRFk); zUg7W8ZI&$l=hX6OhgaXbD&-}{i~tj_PD05Ji>j`h*ofyf+%D(>}RN81*H2p;_T5o<`*O^W+n~v%@E1aa zY?g9d1~~7CJXbp~Nc*3D-XDTpi5Mj+`$G}O7jsgQ)B3Nphrin?e2*#k38D_;VOeVh zbQ_$^YO}NR>!Nc88%N#?ga3vHx1>F=jXW8UZBd`4j<}c6NLZNSu@u8J&w_&ptLXNy zID0g*Fo@vRvzwQ%-f2rFMBPnOoSmIbYu?=*31ZpJ01k4o+`4>g{U3;#^(f|i+weuM zc;Ks1$bUqs;S;;e1`s@rVZSHV{Sl3YcuAH>I*|`t+GlKoBPfFXP7XI1?d+?4&Xdo1 zjVhvEUzO~z5$)5(qlcUmiOBn!N)2S*Ys?WtN%tYX6eP)z#NOxgKO|MvZ&{%kveF{4FmJ@8~5Lr&=cZHd=(O$PH*gb5ws8;dR>Zi62KLw;5Xf>n-f{Le7&Gv8|& zcMz+7o)p;h8MKuzS6^~tM0SG93N2O9_#9WB4WocI3AqG zPRSbob}0Aa$cw3D4U;>)nhgzmvW}`eFbJ*u?AMj+yLW!Pi_sA`tH%V}$$G!TL5dBORm~wg-_`f2!eBQ@e_+T%xx~`sDi}aKFyWRe3Umd@K+<@2B=+fxo zN1achB&<3>mo1DSs%vU*4+$dtPAvcRC+oFm7B^o3zyd!dSQ+hlSwx-!r9iky$cm4k z@q3j`iqPs?LJtw0`YH=rqoh$hA=U^LYs{=KIfWYR@tSU)%uFbfzlR1B#Rq`UE9x#X z&!P&_XOzP5?`7Jxz@DEP-_L&PcMuj#Z!GtWqKzdAU3Y)vjP$v=n#K7khlyqcvE8V& z+8f{Dl3%XlJ`0ksOCNm>(_1yLTi2^5fpFfPee+v}Lq}RUf+03zn2fAC1pYTCyS)q93n5^UJ&gWlcF6<3UyX?&{j;zC&y&3s z65?&p$HE1QEjUrc%#z)Nva@)F$gx5>Z$JB47pLl!UGiX01!L-kB1SyQ$P@Jwxh%Ix z*u?NjHL2kREciKE&a&~7Myg(0|J2mG&TiR1kW;$TZoOT|Be{O&QGZ8;BJ*ak8w8nD zm&Xu?`44g^foEs3%CH}*4$EZ2FP8nG;ZlZFRGI%t8Po&oY!M;j4;jCrREa0rw~x0< zP)ta{&6SmF7{;L-JG;+m9ckGXg$-dahF|u;ibFY3lc;>smwKJ3eiVCdm_3%fc9Afc zY9GI!k&lnh$N{RwlgJpW9@hp}@%E3{`|af7$T2LgNI@qaQ6*DA3C*X*-)fy`>vWJ3 zPdH&FvCA?hdovZYGAbaxAu4E%+mS6>TU!GQQ#4$U-c9_^eNnd93@+Rgv=xuX&d!3O z6w`ucWG)9HtZCxuyEIGmBB2PMlXcS^bu{c-E>g^C0cD*-vV%^{I|~8~UZhZ(0d?hRZhc zL;6B-DOJ~`+m_mPgma z$4v6!gbpwE4h}e;N)Nn^(XCC3klSH}B~oDB%4r9#^-{%VZ+i*9pct%IsAKZ z>t2?rGxDtRc)_hhi2cKY%X{+XRvv47s}+Q2JwHheP9bt^4y31m!BZ|k=S{Ii<{Uhw z1azpMREXJ)OZ$6A-C~b8g(FHRZOX#X)`pQX??TcFOzwO4o*xPqo^xa`6bG(LzAk6` zEjGrbb1GUUz(g-cr}*FMNNB3(13JY9iJ&J1-=`!LxxW{4fZ|S{spv}|Z@Ht9kLg1Z z7@L1d;!THBMC|_lzDQ@Gj2`+(X5<-r3%Jhpj`_2RS-w8g4kv~9`Hms#FvL@2E+xK) z$l5qe*WApwJqV!`LtJ~b&EHk#f3_Q7mO-__s_)ziZkQOmKWR_k=k;aM)CZcsDn5Mn zfrh;7Z_?bg{AG*z=ce*w<-x^w|7KWk@M9z?-EyuUHYOg#GJAyAVV8o100MgtRM2Qs za22#*yh90?Px1%Cl3G4$6s#Dh!I zHQp-1R$XZToF82KkyqJ42=-YxTZh)#IJm7VBw#l*L_mn|opBmakbZ}w!#d-w6_65; zso%Qs(0=OMkCo&Sn?a#fNUb%<48d_H%8{_0i@*TaxkFskFbYpS4|P!4GHGX#2Z zU#RLncU>LGvIi+tg#piZ)Ct;Vh)2H|q3l6aiq9PG0NioHX_^eR#)qiMvJA8`9tLGy zw>Joj0KczQ?Y;w}d8%pa25@7e7j6L5t@##QoXbTOrpyOy$d)OyL5tUAyR@7B2W23g zRF37MP?VazjTOrNrYvX>aVvXfFKwSY?VN+_x&7SumnVF0s_gEA5F>;4l41)A22yZQ zCIxD4V{Vy7QVM~UX~)3EIKD9iZaf8(YHQw1m=i0HoaNC@Hw6*lcF#-DD}5ix;I&Jv z`@N!RCEWc(>j9*DEa3KQRFD>*P#m9?AT#*Fa2S{`^}oJyyTa>QaClde@E{oK9K9lQ zuV8M?G`x3`p7H3r&>=L&*K)Nloq$T9f;eaiBm}QRUNEPBjM1);FO%x|a*_DTRfrY&Tee%~(MBk(l+VgG}=*=*$y)+szS| z88PR|n9rI<-0A~fM)OLSOXi!yT$%`j$x49tx?rXA&b$Jy`0yk5VHn79D1NH0zd?Qr z+o5HG5^0-(l|+H^=+Aw~4J3KA5zNxlts{UJrf&l6X9UT`k{rk7PhcqL8$aGG1<2m6 zq33My&isZCfVD|vL&U@g2Ep&`GM)pMDj^^dko^S6;ka_gRXgzaa$f$qcmTo^T8HL2 z`sZfM6jx9$og|lO7>1OuyJA*eEVwXI*(;tGzEc|cL)7QBtGg%q|@<%|SR)M&>HC59>R z8fwfslo;T~=E_?be*mb-^wL|O~?9P;~zj>C8>g9WRB>&rJ8|g&&4!5c4jqW>F z^hUAx%gFot*MqmtR*GiL{LT0zEt=M^QJD1xxaSn^#jwvcie7H8Mi4gRYw8|AgBpAX z6rNo7&k+3CFi8_iLRHPQ3!4R6j znQTF-J4rYQQ#=qlTA|(FiaGm9%gMun!@==W>>YKrY-n%cF+YeX7Pgr51@;Ar#Db z1n)VTOuQg0tEIDOj4z-lQa4OiqlY8hi}QGz+XvocaPaZ^>ajqU$0t5JWe;#L%7L6M zf^&aZ)>C#E@lxq5iR}qvfD(%+s}qqU*U4Kp2|T4wH9BsrrDIdMlbSgJwrF znvNAeGOGEJxC}Dh--w%q-oKx73Z4yCXKSWIm__=XV6F93b7ypRz=ku&q}FF~cGgrC z$@9D$BqbG*%+fxKazbrzYY#L^Ys5`n3%FPMzkU1G8l1PP@+bv-V*BdVqZCny5EM$e zS^IQ}w6db2kcj9RqZ5Kq+;6zX+fzLUUmeA-ldmTOG)r^6OjoWX&H75Ufvr|)-W^2F z%=7Av&TAP`te5-g{`GY;Q@+BF6d{>2QLKS5j!dpqsz(L~gVNF;Pv09py_8;GUOfvV z`^xTpi4+o9M#I^9$AU}Q&rhaOt>6a?RGV)?3?>tqKu)H5Ip+`2lk44KB4m=+-x@!N zxD(3gf`d<1+u+sMR>sFa4Z+j12i8P=;LDUUsqylvYr)b#`AT5N5>dTv#~e)n+8vmm zLw=g-x%k&Wwq+$QGqk~@$WJ+3cMh2sWx-u3{t%zTIYq>#VG+Tqb{2zFV#FR2CaV(= ztO}v{!Lc&uGJ4mN`0{G-fUkEKlJ$1qv-vj?&i6qKM{cK5DcCv*)=PYQ1~~e57)JPI zF1=KW^qYpaab_l>=35y+sbo2okYZg#x@BI*6MEIYLq=VJR(rAHr8Z@jJ`z$~ZaCtl z^dwvnER2OB(*C&egqDvF4z6k+D*@5AqGQ=*Q0~74QcC*3O~{RCbT;B4L)7{gyWlSL z3xeF6zLRw_?jYg9@bMuJI)4BJ_D))KN_i* z_pL1qgt*QI5Qgm>kPdumnbnL4rb5u8qS2QL*++BuXG83wZ(#SCMVP+HXrstL;#I1p7p3M|j$%;3(9o-Hit3 zo3xoOKSlV_>S+d~`x0m5@-Bo}2F(WDEI18{c_^;%y#E3*F^~@jtN*i|ROYP~ zT$O)EFYsr_ZEi}fP=*99*;wf(sH{Lo+=Afsz<&xkx#&~udzgzJ!t;6M|JDa4IY6O_ z4-GOohmqBClY(jE-R5K@mJhy`QB2qd7h$S%HVrrooFS6Yh@u8nwFN1bCgvdZ(I&>d zY^~4I=Wq(vpZo#LG8mLGel#yCB+U^2lGbhXQbZ7h zbghZ&g@fnJ5_F(A`ltc2hqYytFplwUgU^!ALu@$BvREWZY2?82ToATInAPE}YkgAm zYfAz?nZS5ZIjKM~Ma_J)GI{;9GUv-qJ>~z3h$+EjpbI6_Z zf0|;%4b!^`afGwo|MqcOevZw+Rl3o3#qtSxo-p%iEAL|9T^IK!YNX2Ab0`2RseGD*!PhKcx1>KIp$qJUh$Dop0 z^NhJ`c31y6MUO72t|Q;vGI3ux4l&pcl)GXiNM?C3k?ow6yY zXS)Hl5ziK7?FQrYy{G#Bu=SQS^!l1FNxVrJmvrel}1El6@&BK8hWjOzs-n zOuT)Sija3xhd-Py{a?fm%H%Uug$v6NZbrpAK|qF(UVoLj84Yl(Dev<*0FG5WWdWBu zUHUKY%<>7o-$z#(;1L%t4zlO-e?}x!P05imxKw!HjaIbjQ6zPZvD7t8dbJWV*N;&v zRJj%2kXT)w7W=I^d)zdm75Idy$eQXUm; zyQ#kN5%^62q0GK>i?bXYNQqeB^V5Go8vB*gY{o?VGzQLQzC`_WD-XJL!+a)^Ht+3* zlq~H=ZjhOkwUY?FnI2Z%Gx#3~tw|ZnzOJl|{qR5fOE_S(DYI35@*Dbsjnu0sS! z5BQ`N4HW(paBQ1l0`nOQ1c>kw>!I|}KIZx_$Gg++ndC0Tn1Pg;`)cyQR} zn&%3El*ZeL^ zavFO7_gy7>&R3pYTDqZ^lzWWMm(;Q)6!OiNCZv2z9av-%hoU?Cj@*E-cE z-Fqd~iqTg6Vzc@GJgNr|r4!)vVY#q3 zxKV4+r0W?gx;;t~9$)Xs(Uv5$C|E#7`RL^-)^=|e&1}NDLRXRG8Nv_nQ79Gbx z#`xaa4$gIiFABoH&2{^_T@4HPuHA>ceHG7_@~Lz0<|n4M*ymLjm%py0E{_EAP_wOU zM>G;mm{U+nQWy@eQF56ec@38WM--n$axa*|gd0BpZT$2(wXznXiD7iUUGe1gppg}~ zmUzd6gQ>Z~{7g3UH#uBHca(v~hAJd7_-%jfbb#g22Us4>Gb+P=w?cgSz1Ao!=jEiB zkuJ*Y@=Ca@r2khRr4-jw8ACmIux($`Tql)w%t}-aUOa#gV;CTe)U^WWWG=qI&blxw zi|d#WhOGDg(V1-$A2WqCUuwlzSpez|f%j&Lim=$maX6LMw;#~&0m`la2oydd1f7uQ zLmQyVfV=@WLwkXSJzgp6j%CdVna{w z%2T;aD<=~RR>`B3encaMd&9fg@7G#K zv96~?{t=UCObPqtl*ZC#;#hSr5%b7xU?Qq5hFH1Dl)L@ZLyyvYYM>q$E^RTJ)EZP#J;K^(Y(+3>MKa+le@Pg*C(i)P*JVhgnzd2MV(L~)dFkryOI^NX3-vh z(+Ak5PBY?L(MbXIK5j5$DZH#$42HmOqtJUaSPDhCVZuQV4!orNz2D1z*3p%iqlG6k zBh{b!T897jLFhs!vVowu`*WAdG4g@>S(Z+9iQ*I{vqTT@B6l@Bj4m}%c#D*o_=P0i z9gzOUhQp&mD`7cRKUghUg`$i-VJ10eCiXuXH(D6Gh@An7D}VVKxQ2nX?3MrEWyR>{ zDq61LPLMqT{n&tycrh5m!9_5ApO(EBH}9j6FDTG#qZ>jzUo#+S*bK%5Fr63!+teq! z-S<|kg*r~1*7pUh1R-0;L1?K>Jyg-JXoK|bey^W$LFYQ@L9B)!ACIPgRDHgP)3Kik z^JK{$5{0gIV9r+Rhudnu==4shCz}DzPTu>tT#WXyCE)t*U9#-^c30FNUWF9;zds)P zgO^V|6-QP@$dyKPW=9h3Co`_k#yN*dNoK``3q(RoBOlEvN+rc)jaYP>!%Eo zO6@bJrmB4s%Jmyxb6bkhL$zyO#pQGa0VIDVeAMJTKd~uN{%q>{dl|5m zsFAL<6j(~YAwr>Zc7697coC`(ZF;{9CDwI+RmtTe!c9{Jp0t0mdk<*dq@+z*Xg^tx zm3m-(s}f$)Pb588E?|h)2&0Tu?o$6cn?zXc{is-PCUcE1T<>`xa^`y(u0i`0k>yS_ zbam@tJ5*sfZrlm6YduzP2*_b*zeRvYNFt0x8%P%6d86ML_u2PhsPDVG$3y}aCA?u5 zI3vrGOkw=3k*m~qGmp`8#ZNa}4+Nr`jQeZtM9NvM$yBn@a;y401usYUCoB5y0ZG;F9=hsz&||@sG;tT7NPDkD&dQBiXg? zrvlbD2h)M9qc#Jf?QgZ4!kL&X%-62o<}K{SOb17Wl>2qpyMvz;6tF~V65g1QfiA(n zY9ufA&Iy@}JD-gbEhl_Wr}bmnhklFUbv|Udt-AHu`GUJt`YngiP(iTUN1)xER4#M6 z1Y#JWGJ$(dNQwj>dvthc{cwfAEw*~SeI&$`%!WKlHlM%IA0^F2SY5)5_7pSkI9XIS z&S)I3#O%JHLM+;B|OLtYy8usmquuQs6pZ%CWfNm2U zNMUk^B6hwp?WRM=0Z0gyPa80s9>lY4LR$%zG_y)i7!Cf&S)Nf?N^-#qp20`_AP^1* zU}ujC$23k~wYjIK#)fIyeQa#a z)Dyo-D-A^aK{cqO&;D!;_H3ng`jY2~T!y1h%E4`W)6Q?kcw(YTh)hSTOJR8KbIlH| zLF@8p;yqDsy45zne1{MzMvbC0;_rxba5Bp8}g> zoKW|+1{Ky`!Z-3y!pJ6IOsp6dKufW~*N$P9dU@$MoEUV8i6|>LmfVkOD=`p%3jSG&vMfR35 zDptu0mAO3V$)B~E)=8o)VXSbY!uQ~ats&F@D%$P##8JYi^_2n~gy5A4x%s32eWYY> z`=IHa)QBbCnn6aEt3mXMg9Bs+&#G5V zY9)Q7p!e^%!6>wYpd{_e5dWYRy+zOLA4PcWSZPdYOiH627)2%1;p*a-nT6b#5B1uP zQ*qzt>MW63!sPIa*ADe~LIz_b54N4r;@6&^QN{1iF9hY@6rt;^mdnH#xnHi^D&!Qw z%NBk3@VQiCR)3-q{Zkv8|6mL^W3p>1GV!rTg?jvJ5wHs1$=iY(rVRuPbuM{V9mx1~ z!ZZs{Ei>;?DQ@&{v9C`qNAA)!1vaNuENnj?_0i5k)Peyvv(Ox3Uy=yi3BA6A}X$^_eQQ;3CS2{ni83W~Q{w)aRrhUFu{dJEWRY!(Jp4 z@()l~BfL>>X>dPUC%&zq641XUmfx8*6$Nox5uhHGPszJ)O245s(bGZIqe4R53{#yz*M zEdF9<+5Cds&(3lBSEPk` zjY>W?HQ=nbgh*z+e~?w7iYpE0;E;wL75256S011fAOx!snt6vradd2Ay)+~G>8yu(Dk`YI8msRT@-&Jgu?41R%%p_qlva*K zif@mQ)t*#FeNo`a82qW()Gj4;f-JtqBShmZ4A_uUupPf%lX6M>5ehrFj)xyv!lOO) zN*1*=)JawG2H73H*s9pI`yYs4sMAPQP=Rl<0uhBg0Iq;-;`)j1UcS+%dVs8qbW7mO z>?--PN*zKi8QPhMA&$lVk5FErh8qsZ79XEUaZmz_797}2@%yeUy4PAlRHXY4^)3-l zQuD>g{_UytJ4)9PE|rpHyt~uT6KsDnv#~&ZvRVjM}vZ*0A@T{M>f# zK!)lR^)H=NXoR<+7N=z^a7)s*;S$qr@Qs3JQHhd!A(=(^m_+Ds8w-iG(VVKwbJ$H6 zEGDd43a-o-D?>p3(*tOOjG9jP!iz*KD>>SkCntMn-!(k;FDmj`*G>)TzO`VGu1`?1 z*!S8*iKO#jy2)bjJ2Oj)o)QZquy&?sF3M4)x79D>jPZLeV`RVVl^XAOn}9>_TI|gv zNSmopDUO7jD0sun_ns4Jk2+F!GKp~F;hD{^RxRl96PChg*|Bh)C4pV~vtRQHZEmAE zlT~2K%`gw&onpd5Ot_d7!g>7j1-Cz1&FY3_uO`VDYRw!d%ZuL2UfMCKxAMH-I&^K%ogf#NcKv>E4(iFjq)n@fI9YZFb!=%| z>S0c2=>sSKq5XZ_)rw=wEMw$Y!@|Fr(+IzYyk-6nZd~&R>Cf*G-m!{{W#Gsk9}fBf zH!@Jhv%%-DU^+9&AHE=CbZikVoSpKecD7T;Q6MOTnLzpHtUK$z=Cfn0UTBL^ z43bDG^uF0p(k|c(KZrV?Dfs$qf3mbtP)!zv6RfUT;-1s~Auh{{T%5d9R3o7UqoP$o z@#bv3&-kIYu_ypguUNf?PJN@e{j=8?jWyV_gom3NNY^Ls^CH_GfP;*pTs%qH1&h@9fn2VFyE%2{) zlB#<}#pW)G3`Vph?p4%$d?#(?_2#l23G6>`9a50EHh`YG0Pli0*DY1J;)|JDAEBD3 zYNdrYTH?nHAbF=5pb=CcrBPrJWBs{$d?~C`L6h{$yf8FuJ2ad%yDQ=3gq8lJ%txSE zCX2!Z1+E&1U*yYS%>-QhPLl7oL5zp*z!{#pEGRo@krfdTPJ$O5jNY_(@odQF{cH~c{VrDDhe zc8zSc1N*{(7b(p=hMRy>ZLV1xrqjpoeibu+FmHmq=QE?{x7Yp12GeAWODgPa6sL{P6lt}c)r^oHAWhSO)YIW zP4qG>-6%&Z4acll%`7)yh(iGv@m^9)@l3*U9TQ&eU4)5Mbn)0{iZ5K8My!xC)S(&M z#eXmw{3Wb9elXkvq7<$Ds!u2@^dj=Df*z3|dSo)Jw%MJxnWJZWF{}8E2UEpq>#G29 z6LETZClA08KG~X5M_^!6sYyJZZ84YWUl4PEP_GY7&)}P(>P5uWVrkn-QUxQpcqxi= zHsha3yk3u}<39-T?Hp)3{PS4K3c2Kk?U94)fE+XksgiTv$e6UY1ENGpQR)m%6C5f%9-7r5IK&ApD8gS3tIT6Lkpl70iNlP}iQ4HZ zRii&TY&UBnM~wd48U~em^JiMt5=FB?E23w;-TDgvZDjJ+avLYQ9Kt+Rs-ysPN>`L@ z7qh`hgnTkljQCo8=#kH`a4FcL=QRQFdgBe&_GnO{^hOX=T00JgEL?$3Crm>bK`ZT!13=j}H1Lv6?DUb!VdzZ3^0r zC=H!4&3gGnAb%K2;31?YOsX9aLDD0?wfX*fxiydzUnIEAx+Zs??gTkt!jSI1i30+d zj~JWfcB>&+!if8TH& zOQyxh476pp!06V$;ZvuK_;&n)5>SltqkR>5B=4em=LBeN0XP_gb`Jb?E`sS7Iweq$ zg<^@3v>OhflVr*fg#+XQ1Ni69n`lnFN=a%^XsBYGtAT_0$medTY%r*6uKeJ6eD3HZari)t&^0+p z70FTSoEMN5a*^XThx4ed2WL#RXq-H6PtWONJXqK2Vg~V^09M*4Fmll7TNtFgT~Yya z$_@Y`hrK|5RfH5KxG3xF2@EgpKD0BKe#zwJC=Gmm`gn{+JJrhe#|WO!h6 z3gHKW(9g@mc6SJ*40#CTc|Wc);tf;rkoM9#2q!yA9q~y#ZVoKUEByX6Td}+rKZ-~s z6qi=2!Bj-iP3?R((Rv~bsgb0cx?y&_IJ)y1`bdX_aWt8wP%%Vo4Qz_K0}TVvceA|( z;U|8}jc|4sI~pq4?OPmXVUKE%yTzE5xsT}-TawEV$8DQyX`~VtWcG{9jG0$=0fA`R z73?_Z91zoFVNiivhC}f$LnG8yft{C}&?Li8NXLP{~k2+^HlcU8-kp5Pi3yP{R-~ zuPME3^v`M^j3Z%Z*bLK_iy;?Z4?xk)>14k)f050fpcqS_mh@6J4aLHr78tMuvN{VE!gB<90#6fCcbpiucT6|9Lp)YX8Jrx+W^(21-Ur`n9x@C|xzZT{>fr zz{vxKEA-4D-nu~|&GR`8ezD|Qju$~_QE3@x{^ion(W@d3YU}mu%5TmyyV|JELWgnG z8o0~lad&DcgrxbK91K$EMvS&`!9ng@gXh{BZO(mc&%VFdEq)}6H#WNg!aL@p|DV3x zrHj}7C3@H7!bMDeG^Of(Lce3gQ(2{HuD%&Y?522gXn+0ym&MgWE>M19j)iCZI_Ec~ zDAwT5YDi#Z>#p>2>hFM3cgDT@l<0X2f9Vp;uW<&%;{)Vr8HyMS?q_7iwpcU$K8+P}x4_ z<*~;;XsX!bJ}?;(H{!0+3!=j~k`azjEtHQA9oyAw5$U7b_|mT=+b-U6 zCEr&08`^wG2#W?{l==^*Qhy4Uy8(K^TAav;uA=N6uKxm6Wq$Jx%LzO?bT5u=6FbFd zB3OYhkjw)cV_MzO$CTEdgOl#0Uxw4{T5)W5T=0p{;*m*(HlY9|N5f-Tv(_`N+^D%h z=`Th%n2Wnls@|X?B%MrJlCo1v-1jvV9mY+7<9W7Xqu#v1t^=P*D6;qK{4eq0Kfr+p z&iF(WHC}Fm*6%OwXuPs$Owxm%J$oVyP&kVt$>sXbJQ- zr_)vdT=cfgYMueW>1SgUpNJuxfZb@c51k3I!~In`8j@B4Z=5YBxZZ5LTdUMspxyqXZHV+NE0a5>eJlp>uVq0E5I?FZ$4wv zFSH1OvbY;pXqnnVs0b%kgpD-jG%hnc8H_41gg+eCOsPX|S2BQ$|Hw#SmEjCtozzj5 z?{`-#08HpB_^8MlnUANYFY#56=7YQL#Nzc7rdwsp9;w(4QwR$Zyb@fAqdsrSckU49AB+Klv<43#-BqTcJIlGR{z;Ikh0(5j#mJloj+GMhTV@p>(p z;hkPbX=GXp(^MbU>rw+20EWO(uD7s*UH*1lwp+g@0j38Y-n6P&Ob>uZg2i-Y){icU zF3wh-(eAc~0`xR`ni{|imgn%scr4+-Ko2z}OdG0>^mZx3ETI}jls8BtWyu}pN+Z;?Yq8wcWu4bK_>Fk?MuA{BkBD?B>{=6GdtS8 zaKj!lK1aI|0PP|M6;S^^xI?DhT@2m5R;^LM5z!baAgNy`OK2ga{BBUBg(pu5qpu?f z5Lv{H+GZGJ_o8PYa7+(HHmgHQx)6&0ue@tJ zTm82*Ft2_2M8So||NRT&B?jw~?I!acjlgD4_8h`IV1|@ypgy>g$b4_wqS&_uwGw3!WrEuqJ zfy9gB{>T~?^cGOQRcinCZ;l=NxG;qStA+=-fZ>lqT*al1)S$(2Ko5G${;jmdX&{er z5Pg&IB`II_%L-%CFAm$iKLPq{L>sxp2B1?*M89-t^FFLo(F2wbSU2d72srJ3L1|9i zUSNP|Wr5;T#)h~X;*1SJ^qX*%{V3T3oFxs3nw7S%jM%Nm!W$V*6EOpsw8FnIlg0<3 z8z|7Yi@k}^-&3bw_NrlSZ{iZf(;Z+QRX%9zWu(W(XE~@~1fbi4kM68%jOn2n#0AmT z6+_`UoeTo-g@5+2or~$dJ^#I#N~om__VpB!HPX`2C*7%8DfUtLgs_D7RJ|Z2>i<~W zxyUuyD<3i_ALX^*8pCARSMrM@SVrd9p6KI;%mQv96~}i{0%@-!0ydE@SYP}!9qNa| z$eiz4uHX97(_t5VU>j!d2o~71oEux=t%C+_3Jk>ddi}PPWl!^52C)|b`J8PZOBkRI zzF}lTufMByqVnun5CM)RbUCPCSIG8xB})e3Zo0{e0#4^uGUb{ z@A46vnwj7ILE0og<>8kUK(5e9mNeYI8-QWzzCq^yy!8Cv*L zmkj94Z6KFIFV!Ub<7=Lrs)lNr6^cL~iT61rCl|25aGa8&8wI}tU6{~*AuE&pNWqGq zbJ(RB%b#|lpv4mO^4!5u$bF0ILB^Ssxh23Pg|R7}@dbA;_Le1tX;G`lg3YqzXPNFO zZz{8$H|tmL-+%OgFFrHxOx!4wi04r(<#0UgZs#IvX{1>Qezt(yLA{kV3LKe8=zJFS z-iB1nh+@ZSI`~wJ#`xuu*Jjff4LzV?D9n`#XZj1_%AolAxn*j~Lj>{55O3)0HhFd( zf2_Te|B>L8ta-X6-mQ0xk^&@IaRUdbff$3-nk}bBe`KIKXYMpxiADMMdr+bu|N1YFm-jZA0`x7B@byMr|nc6o9`rsZkX@edibTh)T0L)Dy&>k1qBwss?RJ6 z(#wY!!AP3%dSa#GHgB$sYPT?$;O z0IsF@>)F}H)x&oY=qOy;5YAaI%bFg?Kif<3`w8X#cDr94(6B74Lm-L_zEa4QU$!T& zAb=0bn08KY!Sjxf@dJ+K zo8c$pE{g;W>_-LayOy1CfK?xwd;!p3B&{c!8YSAK{UV%X089(zIT3f1|aGzweFyb zpMY)s8lePGO2@E4*ysLjN&qP8We_GN-{kpMWd{G_NjxD*Bm@<%zLKI=GrHNihR>9K9DH8u(0UtRvXy3|sCtYYJI~Qv&Hu z3-ZYGqd&D04lNA$Ari7i9|G=`itfC$Cb10}Q<$Q-%ik$#vyB;k=AaxxfTVQrN@CMw zdwy~S)24!Yh2E$`+eOZ)a(}Yjk2q^AdSzWxbGxU6V7M<6k)EDP zfh!%?iOrk_d?TO^Vks~)Z{a&xKDGO{`U?9H6@v{BA$U?#+Ena8w+w75PYUSz|2Ye; z#`eI-puZK7)OvrE%EjV2jjX9ueD0(f7Lf3O23lL)EZ33XAcX&=ABwfIk=8#N6=M5> zn#v6C@@Jf$COam}?3ZyTKYTQy@Uy#H%@1%{-xf{R7Z#gr=j7%l)tST!{-Xtm!RHdx z(d3Xux0w|OG*BGqW?!|jQFu1{8!m+y0Uk>be2iwNU9co42tq;(mChC^+8wBfF~F&S zKnH?Q&=U!3=*KieULussW`}FF=!)3RIq+u$MOX8d`{!o43`GDAk%*3Q9gv{h{^k=q z>XhVcgv=hjS3<_~L39*9iCEl{(gsTnykEO4Fw?v0|{8)3lNqx$(4{5@l>hjVQynnNm2Z5de;MrqyfgX^Mtf; z|Bd$~FTP9SzmgFP7NdxLEmma!*4hm5jVNul5z8Q`>e@Dz?i@v@S35>i9yp&Rf$Fy~ zyvgN_6}%uI|QU3&Ht*UNU=2QRnFH{?(&f88n)r@Pztfmi2-`J)iC z=y?%j-L%9mLLq?7=<_z3SL5t(AHTxLv{UvO5Lf(ONXuVi6A& z>4yL6X<}#p=qU<#`1HpH=;^2ug0cUvisv_&$fjKML;ZZ`opZ;BBZ4iBG?ZU`(c)oe z=646QrS%8;|1_4Kyqkh{FpH91cG9z3>dmlArF8q-IWEE}+`p~W3(DW;2X;WM{IPdg z8mIz)Qb|oBR()TNQSUVP%4pk5mGuXgz9mS^c`%07u65enBXF(JRWJqbe>5H;n_=+_ z6VgV-th%{4-x3s;)0Im3j84j>jy$;~OJu0^0T8{nw)`HTcCBZc!rV=c?7gqQWH+Qk zRjs^OA1ZsvsnU`p;x6`O_b2itOMKQuo29YwoXQKL!^N~*b);W)>hNk*h!l$6xLIzC zP;LFo-nLS_!>_^+&^ie6dJ^|!1j14;s;DCGcI#D@<@iZRcxc%jzm#;+? z0VHi}Kir?XF3zw7aInOr8V~|Af$=wyra1GuKlFR#e|2p{A_YW*&AQaUjS|!V7;<6Y z5I2hNhq}DVWSRj!K+EuS=TK+Uc|Sf>-9MS8TNZ(H5Nl~WZ8jc`jlv36ZPqf1 zsv_}(7riGwsn5C!0XTmEowRK@gJFP|u0k%HneRlc^7(8mKmk()eiTD|4xtBz{+ygB zb$S1ieyLvM?2bHD9mBZV#wC>Qyw$VOygbD$Ex7(84Ei+Dy8OwTZW(;Fl9IexBW2_m z!O-#b>9Y=2cf`P>8h%$$xLlI$s1GMNiAG0SU)To^U+qC$vQO4MqMX~*dH&fTkm{aF zIHk`|h2t|7r$@gY@C$zJeq0f4&z!=qR(7f(FJMqVyvdsakd9{8e&Bxnc>;Yg>WUJI zcGm0qJJL!MCQ_`x#`pP)U!cH$qgr?A6!rsqqeNuXb}=+6=m~gU zaptPcUWplT@3JQXW2U!;Bb0N#L+$}x+igd{2{iLF6VjaHY6MJCqr9)RaZsQhQH}}p zfCI<*omB~hu;Df&6!--(gld~eh!07Y!RdcncxNf&`-Aa|03R-!^YsgDa`I}QxZ)}z z4RkQ2v5k1o3s^t@T&=&@6evg%XaA8IP?*Rge(robTtPFLqFwR_{Ow~W^T|oIwps4G zG~FGAlx2dDmN*~=(WQ)L08OO36uSPxjQZZz#?-rn`QT?mg>@+5}flB+!>V z(6W_2EyoPuv-R+*86K}}WzV#zW9(N;Z6z=V!#UJX&5CJ77@uuKDZm9`lHRT!i)dAl zB9wRIe6KZZPpy?#eAh;Id9aW=_?%QiW4l&C2-J_8NQfbXVb+V}FgAse^T^?J|LpMz zFd^;&(kKyiJi#IW{4Q%#Kt|{wvR=L>v)}}bX{e3qCFnlsDVn{1?iU|qRT(w(H{TNyhh zfuf5K4oYN6W!2|%vJZ6Dg>+({Aq(+VN&<{3r#H0cxzh?blE^q!vVU8@QbT_1yixPU zpMhf*P$j^llTmjdY>nn6ougwvhKD!Qnsiczh8jqPP8Q}s{ol1ZhysGG3<6c6{jY)~ zt6xkEB1PWl^i~jo7WE{nJJ9Cspu7$j%TBmwRIdt#-~4-&>41>}X)YJr`!udhP(mAq6_D%R%%+%62U&REW4*zf)i8jk|%g4XFX&_j$9xNV-Uz`Aavwb4ZIfoy;aG z72DD^iDbS=fN7zsSDL?3o44XzgIHJ+;PJhuW;6M&77z%^vnR6A>VE02R8pNi;Lw%nfoBnMa(vf)Cn(_>tCs^0(?nkHb8+fo8msz_`@S^-z zn|XC1us9;UM~=&uRGf3Lsw387E9;|PK96ZAIq+mRJstU)5?vvAM|9bHc)EB*3J%Ip z{tHIX^SU)B-Odh84Jm)x`&BkRQ>v~^FfMgaM8yWssFDnlOR#Fo`P&&;i7|fust{ejFD4D3RS(phZw~l7{qYu^mCnGYqh{I|1Y6Lm3sz z#UT1h$j&>1yOnc%ozst^E{WY+N`>QaUo^Bx_NEI_<8s?Wr}<4t7@2$wR9&>>POjo^ z+DYD&Zz3775btAh?_nLmVI))$O|vg1E2SCYpHUt9dRY`NQNko2w6TJiSp_c-BlrWV zJJCR&p8gQY8hz~O#D29m2t7=SRx8rSSDg9?m&ndf0s3jvGun+t) z;EQSsjLA!4$Q};zPYjfWUabaxp%K4_%N&>exqXyG>EwZ`eBs4(snWitH`=Qb7PYtl z8w!I7woe#Ob&bjnkL8)db$009P{f2e1~_2yKmRFP7M^_WO@^vazY00Oceq*QnF@u@ z?-3`LN9{--mR)yRYTemgD^T^79p?DZ{XqRvPh8nF2vjC%Xl>=xZ6P+NaM&d%wfMcwFHKAlsEV zO=3VbOZk479;;2)|5qc&GK zG0eVis%9FsjMGS!VIV<_1fnUgt$SUqLzpSG7A9fWPU3y#^;IQAuMb5EX~C>XAey@! zyaSh}#WXnJAT?Np$3*Dz7#gHiqJ&K>0v3r=3&2u|7=Mvg2Xw}eH5(V!zrY7fsQxy- zxe%C-<)Swm$?-_IWnI<&Kht-<{h$ISx{3f75@ie=?C$~4MaBk71L5(ba0;F3nZxD` zlKbAR109=^-jDL-33*?&6M!hV6W?9#6BurP-O?EC&tBS@WDKzEoC<=~Xk;sqzJdB& z65CiHYyk}#Xf=dzyL?45w6@Tan@XbPSXhD(q#w;{T%(p}Z{N!X4sumxIBzoFSBv+x zp0YqmVIQ75;g&P{SNJLq7$4~wl)AM`aWxc0DGU*%l+JuXyI1vbn-B#>BA{c7Rx(V}I0LlwbZqXl z_32fsW8f$(6eKX*DILpm?oBii5H<;gZz4Ksu;ly{9#lM=AhzsM!DCSkl{t}|YJq4W zc6jIC>k06M9Viy<9mQn=x|G?9Vs%6OdX*xR#`-F|4dH9)sfM*g5U>rLH?6J)4=T`auoPHNPBN|lgN%b3KpV{Q+o?%oUwQ0N2N zU$NI!t1Sth8S1tIZsjs59Uyq_=+fS~tn@)sQKWL`5rWB0wJzkO-2*aToZ z9n39aKZMX}f#lP%!qCYJu(1zb2!3<@{sgmwY9qk@&IM7Jyn>5KCj6SZjda1l%a&7s z7bFVBk*Q|TnY-|%-~)mg;rrNs__9ohSh?W%&CFu`n9gG_H}#uK^${6pVbeB~lnnLv3HhpKg>l zDDmt@QgMkHeQfAd&spHG0h%jruSJ>LDh<{GTn@!q-*`MmDp~JWnm-0+HjxN=jsSF51-D56isr(oCtdi)>t;?UrODF^Au}B-4Y#@w--DoKkM(`=%ny{@zR5n!2 zv6@s%{bsA%2!9QSFQQywU$aozxlRMk|4_c~Y^B)(@L_$SumfLP}Tv_>47JjEXbp0o}mB-=qJ5!r(keb5Phmc2%N3aqRF zbS-q2WHuq`NG!^}0X+n;x;|)$WT81smFZKV_@1$seAJQTp@>nAF6B3|wLrk6c1CJ9 z#fbZ!7-nJ(H;G|(IQ!GoqsQy_MvVX%Fb?c0pgyf-ji+F1y-}EOW0Y3bN(DxSn>obt zXZq+mm6cd&$BBt;O792HR;X-EaXa0})acF^+BgtTlpyh(h~zYsTL;xt6aQWSG$#G( z-$oBWg@O*Z_1h%SL-FAk6+^UWTia+KB%aZ(-xv=3l9$?gbF_;;Y z))_gYym(XqLBHqSEdd++D59cUsZcNb63SquH5Ot-42eWe$d^pW2RoZfbgPV7_kS~F z8A?Ej5yE_3DN>_pEA2^<;Nu%-mF3wpV+8B+Xa`HSQ?sY=ldot}vu0LzzS-(p6@L z0}AJVbDO4sKUU-~l0#Rn5OzBsr+(e!pp^n3#pa@7rU}-Te{zIsL{o^vefOE}Z(1pU zHekQD!hNh4);s+~MX@_>2Kw$EEnpDVe^;FkUQ|Q`K#JlRA(9%fiQ^kvuc{(r&E{5Z z_Ht*buwH?a*!>NbTFg1SPQ7JmZCiguyT}RUHM%1l849O-PlJ)9McgB1{jn;5fY8F! z7nnao_aJ+hkW>2p9RZfqFZ!p}q$Ty`cdv#g^T45&QPMDz^wIROjI;>*K(4-6w>cZh=$%3qY`x}rfSK|#|iS? zkv-!wMMh-Plur`xzj4`xbI;hUe>i*m6QwH&RBP_P` zU&#H>`3`-F_+wR1!Ko&;%zSoCqYdS%uxNTpu~L$P{mg7BGEeKXl=$&=aXYgX-WM`D zdYqkdv2nH|I?VBTt#W;IZ%>Fv zBoI_BN%&}wngEumW6tPEF}D$eEXUVIvi{8bz|0cg`dVo$h9nf?pe0SI#%J&DboTdyso&-`RqkTr}L+~#G{dX{>UTm&)#1}MAt;e zL|)u*H$7|qE0U?ayH$*cg_%XEgOZ1pLIMd$8f?6Y>+lyZ{$=(9+p#{hbn{N0tZj3$ zc_P70Qxv@X8l_mx6<4ihLJO?&FeEBfJnB;sV8a`7y1cH=l(yLSSiguB!pxgkjZkg# zCY@qQ1V;@UiI2``F`z)10=0hPwz3a+v_EdZa34MT0nj=F5p!~e z?fWtnscV3*^p&yXW>}?yW>gIdl@`Yg4@#DZaBD~uo}>uI8~MNL0A4(Mlqu-|*g$83 z@KTs1*p7qZf7hyqa**-6wOc0Pr{F+&Ea&ctgsJ8iLNxD8fJwkCFbrgYYIXy_K$`4t zo#Urxsv zn9H=Rh0XGlXuxA}sNV3|Q|pQ7&?XY}rMAsdt|R&KZlgbIu$bH<=7H5KNuxu-55cVE zKHG8RJ-k>yUA8wom=cAUQ-tU7DiB&yfVPKD1_&%%1g7ng;K1s)T(8hoOg5$c zXkWE!*zo$^Ut|_Nd#vp`53GSM{P=Udi?Z+xfSTdp_PXW^0!^%gp1~CpcLZZ)xufu0 zu)dk{!O$qaW&(+$Xh;c8Y5k_Ww;I2rXa2I{EPpI~z2f${aqeBhW*%4LwbMA+j_+cl zYy*qAFqKX`r#}N!W&9&gNA06P%LuxFL-BmwAc|q9%-V8+r3GT;w2ah+v(dq(SGEqK(`>?Da8>c{S_>E$O5A=(=9^tdyYXG z!#wWS5BftU5Jx}M?WZp}qlh`pNH!^el+VeaM6S+!5a=WlqP9m zDE;X3vB)hF@HFd0@OW%*zvgy*eg#)qMU-=S3w-mbqy5(-2$KqO^#8PXp3!iAZKEfV zgot1W5(I;2VWNvfZ=*yPo#+w-(I$G2UL$G{qIY75j2bO^uZc2cCL3pK9Sl6{d@YFv0Y@mqOT*sEhQYMjnJv>`|cPOHv=_pY?OE$uz)T~4#xOjuMjgUibJh0&|=-j*jix(qn6)}V! z*Nt>eG45MurJ**Yyep+y_raNwuwvKL(OvGXuTj;9RF#!(9qaG z%pOs=FWyfXUzq$BwAfRy$Ld2in^`NYe8;BDI=g1nP~~r&pRJ^ll2wwv zb{Z1&=Q*Xvg*Z_9SsBOe`q20`7<)yn^*=7ie3cC&5%7rk))O|iW%Iae;jCd4v9$F5 z_2%|g{f$obtn07Sjz!13BPu%*`eY-ucaId&xx3tLia!h|gY>5>vHgaV!_t^7Hcum{ zjtn~VJZ|YuJ_)0I9#u-<#cL`l&|;^2BCBdi$0UAb3reDmfjYKl9dEaJDW@K+49>z+ z_(F$|mdeFt>It3vO_bU-{~9-FHp~In!x?YZ8vjs zL+lMCC;WHES!lq1I3`pB9L?hL$ouRMO{Il5W&CwFsf(YJF4NhK$?sy=oLn+W?v1yH z$XB`iZu|Z&dZ1z(-brwKbp{@woFORA*jQLG#dvYum8iXcnJX(~_cC=}B$mNT_}zKN zQ`)VVj=Xg0+3P1~b~mWsG{DHbS-b)t>Fvn*T$h-sm^ zOr0CcRBN=3tT757hi_;{myRb>DvsGo_J1OCJU#lRFu>}|_T8PncfAaXykPP9Mjp`K zr|Na)_oj`|G#!1WzH2@hoWHX^V1xI(z!7SeXSxba5G+AcI+txEoqfUF7--L1-}_l{ zT~?>w@y65vO)yz1bUg zrl9?xLbp|ZIe`d4?8t1BY#39A5m#ACmQWa~K2o`T3;z|79LndL>(!-mJ(KkzHpKa8 zqFVi`_D<1SK$#wY1Vx#qZnrfCy>_6_dM0mHoFRDot=JjPX5;;+8$M^DU&*e&!a#E` zrUm!paUcMTj{LI>i`<3uuLtG{`~g+7h`*TZ$4_XP!u^Djp16j`5_1 z`1k4q2dAVs_ZBTPrgWFIn1^k#ka~jXqJi!c(w!lS1l(A1d`F#SVJ-IHq-^bt1&1Lu zLw%$4^#o?s24xOIV+kI5rr}KSoZwoQH)QI$2>I}<7wjB!wGiRgGYtXn*~#d^X{thp z3wPh|5w>2GtD1N38Wp31@aZN;MWFt8Y|V3>?Qf@o+iBgu=V_9Dq=kB={i=uZT&H8_ zmb%y#v;QOf`MK_O^#!R?!_(+Y*{VK0RHsfpJB89sS%L(9Xrg)0XD~~$tc|7+7;^J* zpt6_TYNGpBrg)R0laJ=fLTi^xnrt+?B$yYamG(!4ckAjyt0)%ocpEdbyKo#^^?8i9 z3T#7SL}?FrTaNmEold>WH`NG4Fx({{`pwrSS-LK0&uBLP>UjMp*DqtGbK=#>`>TYS z&C&_=>O~4~nG`?pwO4TJ-UX+yjJhaDw-Q*NR(Bcv!SjoJW#g?+p(5L0FiA3fTyUti z+KG%rgz^q2YBnSqaHO(9(<|CSs(HpM(nC2tBE@jOw}yKxQ?$?OAzu(&fi-S#A9yWTxBj7=fvJ7?_0ftKOV>4vbnayo^x%H= z*0@XYmSe2rV1YW8RZ5LBj0wJz(Yb6s9bqoo%4F0zmXKqTTA^g2CtcstFuPjr^KzDy zm=qROw8<6!?r@;@S#rImofyIR87<~RapKR%l`F*`tjdyF!y6bhViiK+^;(unt;}st zeMG2oCtF7@g%UU`Y&R|2gf)GP3T(OS6X3`W>3V7WKL$uhfzpSEuWUnvMoVA*f@9Fj zAG>1&4s1f~&W>f-?%4=p{B11sRk{b$+>Z2;*L9-!?U`RBA?8bbtpy2MrG4W?JFWqE z5hO#TBN(P{pB4EO8%D^ZgmUokXCHi|hh>wF6YsX9umzkHomlYM&MJnJy$MCkk5_Vd zPc=wi(rBS#Y$LZeQ>+;zevkIF6`7R?_b$FortBw!G3{lgLk!JR;jIbZ{Kaxv)x?bk zG#U>T%iGQHri8YYhMGwPjM#u2KGD*?Pqg(5KODmz0zRf_yC8>rFKysT2Fq$*BxahEy<*T&N6MAz$^mHgbz66P ziGpu@gBGJ=BfRN89-7hUsZf=a{a%diwLeAf;n(?{O_WI=auqbryvw*8eTjiavO~XG zcadnaYUx-!vK@jQbO>+`B;!?#qQDH0suuB7CSs)eZ}$$gUTSdaIBQKSO7~279M=}U z9VF9FR2t-mt>xQspWe8v|2)J{R#?@uFf}a z4WNTs6$1l9>=@%-s&5SEPJFJ^l3t9xl3Fw3niv#+V?BkEJCp^=6^f{Qy(MIL_C=#n zEoHbz%`3~xXY*!DB>zK`qf8S!EL(>>zZF-%)7_v|=QG(}>Ah~mIRJ0&580%O+8~{ml8$x? zlk?W<2PdjntVd*Dm~~4#GTrY)5|LgtMRQ*CBsD^=cARZ9#i(eh_55IWVw8_-4Y^mA zpBMR1uUaibdH3|AM`7(tQLm{G+vSiFA2!RA-k(c|vwktw6fsK6hz;80PY_IX%torO zYyD0~hEGAfmFNgdUqsSVIZB^96;Mf3%&FPlJ9LoDgL`!*dO+n;1o-$i9s~S$2#FD= z!FL#nzRD*GlA{;*t~?!W;`@}zP(d<>B)v@xaUjMmHT(5Fp`514jUS!e9=KK zB_*Vi-ZpcKUkN6g6CQI|+!&An-KfQ9O0A#-*B*um)-&T4fmt-E4$&K?vozA-E#2?h z!>f%t@VsR$@e^=W2(_T7h0~R^eLGQ{6;T4jV_-T)_r+6_R4^?M{Bl$O&n-U+K&Vv@ zQsSqIv~OB}D_T&b9w{!4Hcrrx!G_uS`Ub?j_Y!%+1KQL1cSKIymQ$W-jn3DyPh8yR zT^Iep6ImvGL)AJ@U95}yzs0M0>a?1FsS>Tne*{i-kqkeI^Tl@6CH>AeA0_D4@30#s z7zpTUJ(DV5JRY20_KGD53}W>#?9%Ax@*%$0;_P)}pGg{vaLU#oKjfBPT66z#6FXefon%^n0*?V?x#&grSyJ# zaS*)OnIOV@yCgLrqu)^c#dz~EX4$Fp!yiMXdq0W(I9Xv)yxgUqAx-E`l#QT$wtG$` zxP70*zoV~pI8TLiD37u5`NJTv&Gsj$GE$wKDbM2-QuYsA{*z{>X03~lg{RSxd}T!$ zrzYF;WVP-KANo)v+|Y8H^0iZF*kX66eejWV@cyc%tW3`Z2zQJ92}IpvuiL|t8KxSy zYqq7vB^Dg4&(o|cqRQ?6xaseVrM^_n@gzw5hS0S#GN|*s!};x2%O^822KzqZN3|aM zahS;VT%6bayjt6tqBFrp&=Kt{G}FB!@W%XYKbs{(*fYFv z;S4vKbQ)c5r}b>|Ux zfM}xwLEhC*RF+uD;-@MxLJM*w`34d|GPAmnD?@dpYoqyr`N@=|unhn(%{OvhnV8Q; z>A|to-Pff(+Iivl2p-48J#|Erd!`(jZp;@=@d@xkg~hl; zu_m-*^NE`p(v=>hT{&&xkcQqbtc-`HGqqPYHK|zyV67amya;n=MLU-t>Xo_3GDP#w zlpm`*ggX2-pDAD`k!P~Jq-tj9;x=V&kLM_%tkyJpFg0*RlHKI}i?xMMAk``#SA_36qn0pX5V}+>JP_p@n zXe-q|7|H2($=GKWI6_^h5OyPYyPr?dme4m3na|MxdClD6Fd9+sn@aaXQ(W6tYK*&fsNf%dwzEx$_7G9jp1J$wc>eLb}r#|x%Yx(6tOmoQL-LW{l zw}wD|9-mfrj?Z+T8^~pTeQ|~it9uJi?s@2TdEQneQ3a0)Mi#I7gt_sNqh&fYWR@(@)fULX_!#$Yk?WBWzO?S)Y3Y= zKD<>H!(UbTc#S!kp@E&6(O*q0;VZXMC?sx_^kC|d&lQT2nf#gtCH$>IF$kvRu4R81 zWp#oBV>0`jQTtz4y+=LwU1{ace`5+LvsK7pLaQ;pf3RNjf@-iO36${WI_dp{NajA% z-LUoy1cfj@JOef(c&_-7>$4r*U$0`maywzxbc&QoLpxE&yq<}DT`_u)OHI_nu@{ap_E=G;McA_pN85Z0%`t{P9vjHT- zB{uqJC@0-TMzIi_c($wR;4T?9Bc%)1fe{fvA5oVT3v63yXQPeLT7}JP$({f=Bes(61 zg22p0n42((AM$vwOn$nXa_m!B!`4|3wNy&2X<@^(lv$9xvFwiUes$WuTD7cJ>F#96 z_r;6Xr`}FlI#gjPj1f95z7?-`XIOUMKe|%?6+E!Wj#XW%C00zGuw_uKpwe)$Q9(5( z+n6w4GP~a>?s0|Pwn=dk_m!iqOTQt0{VdfP8Xcjxk9MCIm+?N*EAeEp%~IAh=ypM>HnGYBAj*?Gh*3 z{Pb~JA2h}`2&dX&AzJKGoyEm7B#xa?Lit|omG^`RR6K9i3=C6@tL@IHnt?QPj&vWl@fEX9wd__0_UB-*o+80U5K&HwbU*15*9P z&k1H?A0+qzl2qS02%pd^aHdJ|}Rt_~Vw^olSHp9R#G zDg(tV9sSWkx=@KH#j?N`70HZKHmIA+h#R3{UkC+U&27bKeh3YDVU?Kzr80u@&;f6Vg`L^cl)!YXl zcPRKSwP1?<@9zP4XmQ_oZ<0Pi@dH0z|V2tenE&@$b9SJjGNyyh!2{x#59Y|u#L^r3W=#cyZ#AsBS}QvXbe zBIw10CyvNE}#G;Ljo>}lF&dsvL+E8Bx)mMp~p~QF}l=j0Vgs# z$scrHBhXJ4K@G5DhJt%a@d^`3f^SU-Wm)|R4{}R1aEGw5B~Q~tiFS;vtRe1Zs@r|> zJnw80?+a~I@}W!M{%XitLKiy2=R#uozElODZIx@yftB~lY)-<(5mMwG7MS3%y*zC< zUkuH?MgU~m*_mf_Ug5ve-8k)EGouv1zN9$!v#tBG3x%y+olc1dTGRuQCu^yb2%x9LxC|J4c?qU+Op5QT_-xOS&KU2K}CX6b8JCA%-zk_Pu4r* zMROMsm5{rz@}^E0vyDJO;>q2KFV>K)86RKNo%MyLDf?KxP;y;`vDUQ#HP%vOmNVh~ z=;3+Fa`4F2K2dTQ=eOIYOFbk%8=NRDY;I)f_!(S6V3L$2&H47fP0mq{dgb{b$Yux@ zhkn52uSTeqN=x);XEkjFbj)eg@B>cw`z#+X9p0FITFNfL>~UAj!iKfdvMU638fa}| zx;t=R4@KhP^Ro^=6v8a|0m45Ab^!y0)+qT;isjM-`~Vu8L4A0zZu*6-zVMwgR2pRT z-lfXGOPz0Fzg?`)X+EnYQY0k&dbaj_*fsS$3t@7IKPYMsOt2Vh^%|%-s{a}8?)K%bePdqVH)K6kLDxV#P0UQV zEaQ(yuwTXdyKDs75qynozkk>B@7VKx$1d3MYQUFc`f&hHj*E-^KuV(L#f$)X)J>aM zP8wv21WfB*xt*V3^iWApOz1_q^p$)LWw)YthLLUX^OK@M)2)>TO*W0WzWP%HWAA!x z2@gzK^{eB$TTcdgaa%apo#ln!Ne!sMsiwB)t9_;Elb2QaC2(vp%g(noKs~9ID;s^g zci(GIyI8Js@v+{Sym98u2Zc)UdRkqKx|N@E@4oBj4Mex?;bWX2A=Fr_Y>!lVn+~k9 zzr&XT!2qn%h>9;o30S3klYVRX+1JWX&*Furww}0+c6+5q?;HiyCw+Z!Cs+wz#xx-` zhI8*5`}BN11&m2wf(K@q-|wS+)IAnOTHn|@=g13?2M4T|(AtsPCW+y`^<0~utD8g| zeXQBH+WSy8tPfA;oY1XRFsqhXP^@2)F7GSP9JxnEn- z%nb>lTsj}QG(ICLP;Ve#H9K*(fAO)4TsIdlonb-B@oYp_Q~gSJtsgiN+)H@ahY>6% zic0ehC<4}6&+*Hi{3*}^GJQwDpkiDj$F~5<>*6E8%ILJn=hp!Sa#{k?k(M7TJVtMs za~T=h2Xw~rlW`d~gh92$hS5Q1Pe8p}4@b!5@F`ex2))0Vx+V;X`^xQtQ28|mj@YcY zd>7C92^?_?$}@i|3n>B(KAuL8Wi?d+vI5VRTyS4`PY)d*Du0&52fbRS*4UnBKB}8yy!{AmU7qOQc4JHt$GW0Ho7}gqPJ@*Sifdl9GM;B z%b-i6JKGfE8qwNapuVyxXxRwHD0M?0(xv zJfn@4uy|iO37P+d9p4<5}Wc8&r?n@43D7CLN>rjQA|$aMlCt71JdF!LBh1=Tb0{8UYlC3T`487ZLY1H|Wu|D{j>(zUtutyMAn5Be%6&`zTj}b5FSsBA5GMC@_*%_C{))II@&qXaB*S_C1>rY2OiN31 zj?ES$AZ#z&Ngkm8|DXmEAAp(Y{us^}aU~F@mTGsNgAY*rZ@x@tgUY1`x!4mHTu2BD zpaBeCX9p2LlNOSmjn-graka?+Cz_{?FdQ%LHI*VL7gwMrsX^N|SR&z6LH@pvMOaX> zi^I~})qg{8;sD-o{?synUQ_uef1HQ`7Yp2~lQGCen8b8e3D zYCo{(I}wAXw&uH@%LC|&!nMc#Aa*82uyuAhOqCOa-VXtCAyO4z&z~efaJQO!lx^YQ zV(RO^pj;ZLayb2Ja0QqX>h9F~SR8`d4`5v5yXH1q*f{|t$PTQ(n|rPVJN-6`)-`fK za7V1(uF4<=s9Pa-}*@4Hp0V z>pv~w{eLf~QS-o59Gt5z^3oET|IvuQSk7+SJjh%BVG!WW418{NIB&*Uzz&IBe-NO< zanAUcS^d+#dX$0TPcllM{_~T6y=nRlT*iba&VUy%{{Q-V^aQkO8BMYY{O@1BO(g@y zx`!)%1gd)fYVUu(Z`uH@VzDIpFJJlM*gZzJ= oNk2HIWg~d#zmqFH?kVJ0tY>_pgs5r~7YF>w%P32iO1|*_AI&f7i~s-t From 9b6060a9fb7f7bc45869ca476917a6e15fdae540 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 5 Feb 2024 20:03:12 +0100 Subject: [PATCH 40/97] Add learned model parameters to the simple_planning_simulator --- simulator/simple_planning_simulator/README.md | 38 +++++++++++-------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index b441f9f903d0d..513e956f86b6d 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -63,27 +63,33 @@ The purpose of this simulator is for the integration test of planning and contro - `DELAY_STEER_ACC` - `DELAY_STEER_ACC_GEARED` - `DELAY_STEER_MAP_ACC_GEARED`: applies 1D dynamics and time delay to the steering and acceleration commands. The simulated acceleration is determined by a value converted through the provided acceleration map. This model is valuable for an accurate simulation with acceleration deviations in a real vehicle. +- `LEARNED_STEER_VEL`: launches learned python models. More about this [here](https://github.com/atomyks/autoware.universe/tree/feature/simulator_py_model/simulator/learned_model). The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves based on a 1st-order with time delay model. The `STEER` means the model receives the steer command. The `VEL` means the model receives the target velocity command, while the `ACC` model receives the target acceleration command. The `GEARED` suffix means that the motion will consider the gear command: the vehicle moves only one direction following the gear. The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V). -| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | D_ST_M_ACC_G | Default value | unit | -| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :----------- | :------------ | :------ | -| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | o | 0.1 | [s] | -| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | o | 0.24 | [s] | -| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | x | 0.25 | [s] | -| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | o | 0.1 | [s] | -| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | o | 0.27 | [s] | -| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | x | 0.0 | [rad] | -| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | x | 0.5 | [s] | -| vel_lim | double | limit of velocity | x | x | x | o | o | o | o | 50.0 | [m/s] | -| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | 7.0 | [m/ss] | -| steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | 1.0 | [rad] | -| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | 5.0 | [rad/s] | -| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | x | 1.0 | [-] | -| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | x | 1.0 | [-] | -| acceleration_map_path | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x | x | x | x | x | x | o | - | [-] | +| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | D_ST_M_ACC_G | L_S_V | Default value | unit | +| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :----------- | :---- | :------------ | :------ | +| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | o | x | 0.1 | [s] | +| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | o | x | 0.24 | [s] | +| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | x | x | 0.25 | [s] | +| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | o | x | 0.1 | [s] | +| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | o | x | 0.27 | [s] | +| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | x | x | 0.0 | [rad] | +| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | x | x | 0.5 | [s] | +| vel_lim | double | limit of velocity | x | x | x | o | o | o | o | x | 50.0 | [m/s] | +| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | x | 7.0 | [m/ss] | +| steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | x | 1.0 | [rad] | +| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | x | 5.0 | [rad/s] | +| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | x | x | 1.0 | [-] | +| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | x | x | 1.0 | [-] | +| acceleration_map_path | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x | x | x | x | x | x | o | x | - | [-] | +| model_python_paths | string | path to a file where the model is implemented | x | x | x | x | x | x | x | o | - | [-] | +| model_param_paths | string | path to the file where model parameters are stored (can be empty string if no parameter file is required) | x | x | x | x | x | x | x | o | - | [-] | +| model_class_names | string | name of the class that implements the model | x | x | x | x | x | x | x | o | - | [-] | + +_Note:_ Parameters `model_python_paths`, `model_param_paths`, and `model_class_names` need to have the same length. The `acceleration_map` is used only for `DELAY_STEER_MAP_ACC_GEARED` and it shows the acceleration command on the vertical axis and the current velocity on the horizontal axis, with each cell representing the converted acceleration command that is actually used in the simulator's motion calculation. Values in between are linearly interpolated. From f6d40dae604636952729936133ef38b564e81ece Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 6 Dec 2023 16:52:57 +0100 Subject: [PATCH 41/97] Add a new simulation model --- .../vehicle_model/sim_model_pymodels.hpp | 150 ++++++++++++++++++ .../vehicle_model/sim_model_pymodels.cpp | 123 ++++++++++++++ 2 files changed, 273 insertions(+) create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp new file mode 100644 index 0000000000000..c83934cf9a9ec --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -0,0 +1,150 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +#include +#include + +#include +#include +#include +/** + * @class SimModelPymodels + * @brief calculate delay steering dynamics + */ +class SimModelPymodels : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] vx_lim velocity limit [m/s] + * @param [in] steer_lim steering limit [rad] + * @param [in] vx_rate_lim acceleration limit [m/ss] + * @param [in] steer_rate_lim steering angular velocity limit [rad/ss] + * @param [in] wheelbase vehicle wheelbase length [m] + * @param [in] dt delta time information to set input buffer for delay + * @param [in] vx_delay time delay for velocity command [s] + * @param [in] vx_time_constant time constant for 1D model of velocity dynamics + * @param [in] steer_delay time delay for steering command [s] + * @param [in] steer_time_constant time constant for 1D model of steering dynamics + * @param [in] steer_dead_band dead band for steering angle [rad] + */ + SimModelPymodels( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double vx_delay, double vx_time_constant, double steer_delay, + double steer_time_constant, double steer_dead_band); + + /** + * @brief destructor + */ + ~SimModelPymodels() = default; + +private: + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant --> TODO what is this? + + enum IDX { + X = 0, + Y, + YAW, + VX, + STEER, + }; + enum IDX_U { + VX_DES = 0, + STEER_DES, + }; + + const double vx_lim_; //!< @brief velocity limit + const double vx_rate_lim_; //!< @brief acceleration limit + const double steer_lim_; //!< @brief steering limit [rad] + const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] + const double wheelbase_; //!< @brief vehicle wheelbase length [m] + double prev_vx_ = 0.0; + double current_ax_ = 0.0; + + std::deque vx_input_queue_; //!< @brief buffer for velocity command + std::deque steer_input_queue_; //!< @brief buffer for angular velocity command + const double vx_delay_; //!< @brief time delay for velocity command [s] + const double vx_time_constant_; + //!< @brief time constant for 1D model of velocity dynamics + const double steer_delay_; //!< @brief time delay for angular-velocity command [s] + const double + steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics + const double steer_dead_band_; //!< @brief dead band for steering angle [rad] + + /** + * @brief set queue buffer for input command + * @param [in] dt delta time + */ + void initializeInputQueue(const double & dt); + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle longitudinal velocity + */ + double getVx() override; + + /** + * @brief get vehicle lateral velocity + */ + double getVy() override; + + /** + * @brief get vehicle longitudinal acceleration + */ + double getAx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with delay steering model + * @param [in] state current model state + * @param [in] input input vector to model + */ + Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp new file mode 100644 index 0000000000000..dffd0d3a70312 --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -0,0 +1,123 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" + +#include + +SimModelPymodels::SimModelPymodels( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double vx_delay, double vx_time_constant, double steer_delay, + double steer_time_constant, double steer_dead_band) +: SimModelInterface(5 /* dim x */, 2 /* dim u */), + MIN_TIME_CONSTANT(0.03), + vx_lim_(vx_lim), + vx_rate_lim_(vx_rate_lim), + steer_lim_(steer_lim), + steer_rate_lim_(steer_rate_lim), + wheelbase_(wheelbase), + vx_delay_(vx_delay), + vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), + steer_delay_(steer_delay), + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), + steer_dead_band_(steer_dead_band) +{ + //initializeInputQueue(dt); +} + +double SimModelPymodels::getX() +{ + return state_(IDX::X); +} +double SimModelPymodels::getY() +{ + return state_(IDX::Y); +} +double SimModelPymodels::getYaw() +{ + return state_(IDX::YAW); +} +double SimModelPymodels::getVx() +{ + return state_(IDX::VX); +} +double SimModelPymodels::getVy() +{ + return 0.0; +} +double SimModelPymodels::getAx() +{ + return current_ax_; +} +double SimModelPymodels::getWz() +{ + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; +} +double SimModelPymodels::getSteer() +{ + return state_(IDX::STEER); +} +void SimModelPymodels::update(const double & dt) +{ + Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); + + vx_input_queue_.push_back(input_(IDX_U::VX_DES)); + delayed_input(IDX_U::VX_DES) = vx_input_queue_.front(); + vx_input_queue_.pop_front(); + steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); + delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); + steer_input_queue_.pop_front(); + // do not use deadzone_delta_steer (Steer IF does not exist in this model) + updateRungeKutta(dt, delayed_input); + current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; + prev_vx_ = input_(IDX_U::VX_DES); +} + +Eigen::VectorXd SimModelPymodels::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + auto sat = [](double val, double u, double l) { return std::max(std::min(val, u), l); }; + + const double vx = sat(state(IDX::VX), vx_lim_, -vx_lim_); + const double steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_); + const double yaw = state(IDX::YAW); + const double delay_input_vx = input(IDX_U::VX_DES); + const double delay_input_steer = input(IDX_U::STEER_DES); + const double delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); + const double vx_rate = sat(-(vx - delay_vx_des) / vx_time_constant_, vx_rate_lim_, -vx_rate_lim_); + const double delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); + + const double steer_diff = steer - delay_steer_des; + const double steer_diff_with_dead_band = std::invoke([&]() { + if (steer_diff > steer_dead_band_) { + return steer_diff - steer_dead_band_; + } else if (steer_diff < -steer_dead_band_) { + return steer_diff + steer_dead_band_; + } else { + return 0.0; + } + }); + + const double steer_rate = + sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vx * cos(yaw); + d_state(IDX::Y) = vx * sin(yaw); + d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; + d_state(IDX::VX) = vx_rate; + d_state(IDX::STEER) = steer_rate; + + return d_state; +} From a326e1fe3f103d67f2f2ffc3da98ba172e45a418 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 6 Dec 2023 18:36:40 +0100 Subject: [PATCH 42/97] Add new model -> currently same as delay_steer_vel --- simulator/simple_planning_simulator/CMakeLists.txt | 1 + .../simple_planning_simulator_core.hpp | 3 ++- .../vehicle_model/sim_model.hpp | 1 + .../vehicle_model/sim_model_pymodels.hpp | 2 +- .../simple_planning_simulator_core.cpp | 13 ++++++++++--- .../vehicle_model/sim_model_pymodels.cpp | 4 ++-- 6 files changed, 17 insertions(+), 7 deletions(-) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 87d0f7e5fef0b..06fe692c8e3a7 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -14,6 +14,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp + src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 58914d19552df..4e738776d475e 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -210,7 +210,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node DELAY_STEER_ACC_GEARED = 3, IDEAL_STEER_VEL = 4, DELAY_STEER_VEL = 5, - DELAY_STEER_MAP_ACC_GEARED = 6 + DELAY_STEER_MAP_ACC_GEARED = 6, + PYMODELS = 7 } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp index 9fa4516c1a522..fcc4baaffcb93 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp @@ -19,6 +19,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index c83934cf9a9ec..b0b1e24975106 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -27,7 +27,7 @@ * @class SimModelPymodels * @brief calculate delay steering dynamics */ -class SimModelPymodels : public SimModelInterface +class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ { public: /** diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index e99f6d5f2cf00..41b60f15419e6 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -281,7 +281,12 @@ void SimplePlanningSimulator::initialize_vehicle_model() vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_bias, acceleration_map_path); - } else { + } else if (vehicle_model_type_str == "PYMODELS"){ + vehicle_model_type_ = VehicleModelType::PYMODELS; + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, + vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, steer_dead_band); + }else{ throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } } @@ -478,7 +483,8 @@ void SimplePlanningSimulator::set_input( if ( vehicle_model_type_ == VehicleModelType::IDEAL_STEER_VEL || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL || + vehicle_model_type_ == VehicleModelType::PYMODELS) { input << vel, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || @@ -579,7 +585,8 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED) { state << x, y, yaw, vx; } else if ( // NOLINT - vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL || + vehicle_model_type_ == VehicleModelType::PYMODELS) { state << x, y, yaw, vx, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC || diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index dffd0d3a70312..e8a533dd6b824 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -33,7 +33,7 @@ SimModelPymodels::SimModelPymodels( steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), steer_dead_band_(steer_dead_band) { - //initializeInputQueue(dt); + std::cout << dt << std::endl; } double SimModelPymodels::getX() @@ -71,7 +71,7 @@ double SimModelPymodels::getSteer() void SimModelPymodels::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); - + std::cout << dt << std::endl; vx_input_queue_.push_back(input_(IDX_U::VX_DES)); delayed_input(IDX_U::VX_DES) = vx_input_queue_.front(); vx_input_queue_.pop_front(); From cd229e295ea07c07bb1ab297255ec1a4d29a1cd9 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Fri, 8 Dec 2023 11:29:36 +0100 Subject: [PATCH 43/97] Add python.h library --- simulator/simple_planning_simulator/CMakeLists.txt | 6 ++++++ .../vehicle_model/sim_model_pymodels.hpp | 3 +++ .../vehicle_model/sim_model_pymodels.cpp | 1 + 3 files changed, 10 insertions(+) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 06fe692c8e3a7..58e00e3a2517f 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -4,6 +4,10 @@ project(simple_planning_simulator) find_package(autoware_cmake REQUIRED) autoware_package() +#find_package(Python REQUIRED) # Python testing +#include_directories(${Python_INCLUDE_DIRS}) +include_directories("/usr/include/python3.10") + # Component ament_auto_add_library(${PROJECT_NAME} SHARED include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -30,6 +34,8 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_exe ) +target_link_libraries(${PROJECT_NAME} ${Python_LIBRARIES}) + if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_simple_planning_simulator test/test_simple_planning_simulator.cpp diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index b0b1e24975106..16e2938133ff8 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -23,6 +23,9 @@ #include #include #include + +#include + /** * @class SimModelPymodels * @brief calculate delay steering dynamics diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index e8a533dd6b824..3986c2472f782 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -15,6 +15,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" #include +#include SimModelPymodels::SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, From b577a4521f3eb353bcc6f8249e1d961cceda3b5d Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Fri, 8 Dec 2023 20:24:46 +0100 Subject: [PATCH 44/97] Add working simple steering model programed in python launched from C++ --- .../simple_planning_simulator/CMakeLists.txt | 4 +- .../vehicle_model/sim_model_pymodels.hpp | 15 ++ .../vehicle_model/sim_model_pymodels.cpp | 146 +++++++++++++++--- 3 files changed, 142 insertions(+), 23 deletions(-) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 58e00e3a2517f..bfe79488ce899 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -4,8 +4,8 @@ project(simple_planning_simulator) find_package(autoware_cmake REQUIRED) autoware_package() -#find_package(Python REQUIRED) # Python testing -#include_directories(${Python_INCLUDE_DIRS}) +find_package(Python REQUIRED) # Python testing +include_directories(${Python_INCLUDE_DIRS}) include_directories("/usr/include/python3.10") # Component diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index 16e2938133ff8..fc7bba2a8476f 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -25,6 +25,7 @@ #include #include +#include /** * @class SimModelPymodels @@ -90,6 +91,20 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics const double steer_dead_band_; //!< @brief dead band for steering angle [rad] + + PyObject *steer_base_model; + + int num_states_steering; + int num_inputs_steering; + + // Create vectors to store system input and state (C++) + Eigen::VectorXd state_steering; + Eigen::VectorXd input_steering; + + // Create vectors to store system input and state (python) + PyObject *py_state_steering, *py_input_steering; + + /** * @brief set queue buffer for input command * @param [in] dt delta time diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 3986c2472f782..b15daaaed5c34 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -16,6 +16,8 @@ #include #include +#include +#include SimModelPymodels::SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, @@ -34,7 +36,81 @@ SimModelPymodels::SimModelPymodels( steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), steer_dead_band_(steer_dead_band) { + + // Initialize python library + dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. + /* + More about the line above here: + https://stackoverflow.com/questions/60719987/embedding-python-which-uses-numpy-in-c-doesnt-work-in-library-dynamically-loa + https://mail.python.org/pipermail/new-bugs-announce/2008-November/003322.html + https://stackoverflow.com/questions/67891197/ctypes-cpython-39-x86-64-linux-gnu-so-undefined-symbol-pyfloat-type-in-embedd + https://man7.org/linux/man-pages/man3/dlopen.3.html + */ + Py_Initialize(); + PyObject *py_module, *py_module_dict, *python_class; + + char *model_save_path = (char*)"/home/tomas/f1tenth/ws_testing/base_model_save"; + + // Import python module + py_module = PyImport_Import(PyUnicode_FromString((char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis")); + + if (py_module == nullptr) { + PyErr_Print(); + return; + } + + py_module_dict = PyModule_GetDict(py_module); + if (py_module_dict == nullptr) { + PyErr_Print(); + return; + } + + python_class = PyDict_GetItemString(py_module_dict, "SimpleSteeringHyst"); + if (python_class == nullptr) { + PyErr_Print(); + return; + } + + if (PyCallable_Check(python_class)) { + steer_base_model = PyObject_CallObject(python_class, nullptr); + Py_DECREF(python_class); + } else { + Py_DECREF(python_class); + return; + } + + // Load model params + PyObject *load_params_succ = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"load_params"), PyUnicode_FromString(model_save_path), NULL); + + if(!PyBool_Check(load_params_succ)){ + return; + } + + PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"reset"), NULL); + + + // Set number model states and inputs + num_states_steering = 1; + num_inputs_steering = 1; + + // Initialize state and input vectors + state_steering = Eigen::VectorXd::Zero(num_states_steering); + input_steering = Eigen::VectorXd::Zero(num_inputs_steering); + + + py_state_steering = PyList_New(num_states_steering); + py_input_steering = PyList_New(num_inputs_steering); + + for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ + PyList_SetItem(py_state_steering, state_idx, PyFloat_FromDouble(state_steering(state_idx))); + } + + for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ + PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); + } + std::cout << dt << std::endl; + std::cout << "OKAAAAAYYYYYYY" << std::endl; } double SimModelPymodels::getX() @@ -72,15 +148,43 @@ double SimModelPymodels::getSteer() void SimModelPymodels::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); - std::cout << dt << std::endl; + //std::cout << dt << std::endl; + + // Model the delay of speed vx_input_queue_.push_back(input_(IDX_U::VX_DES)); delayed_input(IDX_U::VX_DES) = vx_input_queue_.front(); vx_input_queue_.pop_front(); - steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); - delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); - steer_input_queue_.pop_front(); + + // Model the delay of steering -- TODO remove this after implementing python models + // steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); + // delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); + // steer_input_queue_.pop_front(); + + // Simulate the system (car kinematics and dynamics) // do not use deadzone_delta_steer (Steer IF does not exist in this model) updateRungeKutta(dt, delayed_input); + + // Simulate the steering system using python model + input_steering(0) = input_(IDX_U::STEER_DES); + + for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ + PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); + } + + // Call forward() of the python model + PyObject *value = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"forward"), py_input_steering, py_state_steering, NULL); + py_state_steering = PyTuple_GetItem(value, 0); // The first output of the model forward is the next state + // PyObject *py_delayed_input = PyTuple_GetItem(value, 1); // The second output of the model forward is delayed input + + // State vector from python to C++ + for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ + state_steering(state_idx) = PyFloat_AsDouble(PyList_GetItem(PyList_GetItem(py_state_steering, 0), state_idx)); + } + //std::cout << "Required steer: " << input_(IDX_U::STEER_DES) << " State steering: " << state_steering(0) << std::endl; + + state_(IDX::STEER) = state_steering(0); + + // Calculate current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; prev_vx_ = input_(IDX_U::VX_DES); } @@ -94,31 +198,31 @@ Eigen::VectorXd SimModelPymodels::calcModel( const double steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_); const double yaw = state(IDX::YAW); const double delay_input_vx = input(IDX_U::VX_DES); - const double delay_input_steer = input(IDX_U::STEER_DES); + //const double delay_input_steer = input(IDX_U::STEER_DES); const double delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); const double vx_rate = sat(-(vx - delay_vx_des) / vx_time_constant_, vx_rate_lim_, -vx_rate_lim_); - const double delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); - - const double steer_diff = steer - delay_steer_des; - const double steer_diff_with_dead_band = std::invoke([&]() { - if (steer_diff > steer_dead_band_) { - return steer_diff - steer_dead_band_; - } else if (steer_diff < -steer_dead_band_) { - return steer_diff + steer_dead_band_; - } else { - return 0.0; - } - }); - - const double steer_rate = - sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); + // const double delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); + + // const double steer_diff = steer - delay_steer_des; + // const double steer_diff_with_dead_band = std::invoke([&]() { + // if (steer_diff > steer_dead_band_) { + // return steer_diff - steer_dead_band_; + // } else if (steer_diff < -steer_dead_band_) { + // return steer_diff + steer_dead_band_; + // } else { + // return 0.0; + // } + // }); + + //const double steer_rate = + // sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vx * cos(yaw); d_state(IDX::Y) = vx * sin(yaw); d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; d_state(IDX::VX) = vx_rate; - d_state(IDX::STEER) = steer_rate; + d_state(IDX::STEER) = 0.0; //Use python models to update steer return d_state; } From 389b94182842ebe300e69d4c6958c6cff6d24003 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 13 Dec 2023 18:21:52 +0100 Subject: [PATCH 45/97] Fixing linker problem (WIP) --- .../simple_planning_simulator/CMakeLists.txt | 21 +- .../vehicle_model/sim_model_pymodels.hpp | 23 +- .../vehicle_model/sim_model_pymodels.cpp | 196 +++++++++++------- 3 files changed, 154 insertions(+), 86 deletions(-) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index bfe79488ce899..74c75db70fee5 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -4,9 +4,17 @@ project(simple_planning_simulator) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(Python REQUIRED) # Python testing -include_directories(${Python_INCLUDE_DIRS}) -include_directories("/usr/include/python3.10") +find_package(Python3 COMPONENTS Interpreter Development) +find_package(pybind11 CONFIG) + +message("Pybind11_FOUND:${pybind11_FOUND}") +message("Pybind11_Version:${pybind11_VERSION}") + +message("Python_FOUND:${Python3_FOUND}") +message("Python_VERSION:${Python3_VERSION}") +message("Python_Development_FOUND:${Python3_Development_FOUND}") +message("Python_LIBRARIES:${Python3_LIBRARIES}") + # Component ament_auto_add_library(${PROJECT_NAME} SHARED @@ -26,6 +34,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${tf2_INCLUDE_DIRS}) + target_compile_options(${PROJECT_NAME} PRIVATE -Wno-old-style-cast) # RCLCPP_ERROR_THROTTLE() has built-in old-style casts. # Node executable @@ -34,7 +43,11 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_exe ) -target_link_libraries(${PROJECT_NAME} ${Python_LIBRARIES}) +# target_link_options(${PROJECT_NAME} PRIVATE -shared -fPIC -export-dynamic) # + +# target_link_libraries(${PROJECT_NAME} ${Python3_LIBRARIES}) 3 +# /usr/lib/x86_64-linux-gnu/libpython3.10.so +target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_simple_planning_simulator diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index fc7bba2a8476f..1aa65c338d9b2 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -61,6 +61,24 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ private: const double MIN_TIME_CONSTANT; //!< @brief minimum time constant --> TODO what is this? + /* + Specify string names for states and inputs. So we can automatically map states and + inputs of this model to states and inputs of the python submodels. + In order not to compare strings each iteration, we compute mappings between the states + and inputs of this model and python submodels. + */ + const char* STATE_NAMES[5] = {"POS_X", "POS_Y", "YAW", "VX", "STEER"}; + const char* INPUT_NAMES[2] = {"VX_DES", "STEER_DES"}; + + /* + Index means state or input in this model. First 5 indexes are states, last 2 indexes are inputs. + ---------------------------- { STATES }{INPUTS} */ + int MAP_TO_PYSTEER_INPUT[7] = {-1, -1, -1, -1, -1, -1, -1}; // <<- Value means index of the input in python steer model. -1 means not used. + /* + Index means state in this model. + ------------------------------- { STATES }*/ + int MAP_FROM_PYSTEER_OUTPUT[5] = {-1, -1, -1, -1, -1}; // <<- Value means index of the output in python steer model. -1 means not used. + enum IDX { X = 0, Y, @@ -72,6 +90,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ VX_DES = 0, STEER_DES, }; + const double vx_lim_; //!< @brief velocity limit const double vx_rate_lim_; //!< @brief acceleration limit @@ -92,7 +111,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ const double steer_dead_band_; //!< @brief dead band for steering angle [rad] - PyObject *steer_base_model; + //PyObject *steer_base_model; int num_states_steering; int num_inputs_steering; @@ -102,7 +121,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ Eigen::VectorXd input_steering; // Create vectors to store system input and state (python) - PyObject *py_state_steering, *py_input_steering; + //PyObject *py_state_steering, *py_input_steering; /** diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index b15daaaed5c34..0aee8a36a5aa5 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -15,9 +15,10 @@ #include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" #include -#include -#include -#include +// #include +// #include +#include +namespace py = pybind11; SimModelPymodels::SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, @@ -38,7 +39,9 @@ SimModelPymodels::SimModelPymodels( { // Initialize python library - dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. + //dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. + + /* More about the line above here: https://stackoverflow.com/questions/60719987/embedding-python-which-uses-numpy-in-c-doesnt-work-in-library-dynamically-loa @@ -46,68 +49,102 @@ SimModelPymodels::SimModelPymodels( https://stackoverflow.com/questions/67891197/ctypes-cpython-39-x86-64-linux-gnu-so-undefined-symbol-pyfloat-type-in-embedd https://man7.org/linux/man-pages/man3/dlopen.3.html */ - Py_Initialize(); - PyObject *py_module, *py_module_dict, *python_class; + // Py_Initialize(); + // PyObject *py_module, *py_module_dict, *python_class; - char *model_save_path = (char*)"/home/tomas/f1tenth/ws_testing/base_model_save"; + // char *model_save_path = (char*)"/home/tomas/f1tenth/ws_testing/base_model_save"; // Import python module - py_module = PyImport_Import(PyUnicode_FromString((char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis")); - - if (py_module == nullptr) { - PyErr_Print(); - return; - } - - py_module_dict = PyModule_GetDict(py_module); - if (py_module_dict == nullptr) { - PyErr_Print(); - return; - } - - python_class = PyDict_GetItemString(py_module_dict, "SimpleSteeringHyst"); - if (python_class == nullptr) { - PyErr_Print(); - return; - } - - if (PyCallable_Check(python_class)) { - steer_base_model = PyObject_CallObject(python_class, nullptr); - Py_DECREF(python_class); - } else { - Py_DECREF(python_class); - return; - } + // py_module = PyImport_Import(PyUnicode_FromString((char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis")); + + // if (py_module == nullptr) { + // PyErr_Print(); + // return; + // } + + // py_module_dict = PyModule_GetDict(py_module); + // if (py_module_dict == nullptr) { + // PyErr_Print(); + // return; + // } + + // python_class = PyDict_GetItemString(py_module_dict, "SimpleSteeringHyst"); + // if (python_class == nullptr) { + // PyErr_Print(); + // return; + // } + + // if (PyCallable_Check(python_class)) { + // steer_base_model = PyObject_CallObject(python_class, nullptr); + // Py_DECREF(python_class); + // } else { + // Py_DECREF(python_class); + // return; + // } + + + // TODO call function to get model signal names + // From the names create a mapping table for correctly pass arguments to the python models + + // PyObject *py_sig_state_names = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"get_sig_state_names"), NULL); + + // char *my_result; + + // PyObject * temp_bytes = PyUnicode_AsEncodedString(PyList_GetItem(py_sig_state_names, 0), "UTF-8", "strict"); + // my_result = PyBytes_AS_STRING(temp_bytes); + // my_result = strdup(my_result); + // Py_DECREF(temp_bytes); + + // wcstombs(buffer, PyList_GetItem(py_sig_state_names, 0), sizeof(buffer)); + + // int len_ = PyList_Size(py_sig_state_names); + // std::cout << "STATE NAMES: -" << my_result << "-" << std::endl; + // std::cout << "STEER: " << len_ << std::endl; + // std::cout << "STEER: " << strcmp(my_result, "STEER") << std::endl; + // std::cout << "STEER: " << strcmp(my_result, STATE_NAMES[4]) << std::endl; + // std::cout << "STEERR: " << strcmp(my_result, "STEERR") << std::endl; + // Load model params - PyObject *load_params_succ = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"load_params"), PyUnicode_FromString(model_save_path), NULL); + // PyObject *load_params_succ = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"load_params"), PyUnicode_FromString(model_save_path), NULL); - if(!PyBool_Check(load_params_succ)){ - return; - } + // if(!PyBool_Check(load_params_succ)){ + // return; + // } - PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"reset"), NULL); + // Reset the python model after loading the parameter file + // PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"reset"), NULL); // Set number model states and inputs - num_states_steering = 1; - num_inputs_steering = 1; + // num_states_steering = 1; + // num_inputs_steering = 1; - // Initialize state and input vectors - state_steering = Eigen::VectorXd::Zero(num_states_steering); - input_steering = Eigen::VectorXd::Zero(num_inputs_steering); + // // Initialize state and input vectors + // state_steering = Eigen::VectorXd::Zero(num_states_steering); + // input_steering = Eigen::VectorXd::Zero(num_inputs_steering); - py_state_steering = PyList_New(num_states_steering); - py_input_steering = PyList_New(num_inputs_steering); + // py_state_steering = PyList_New(num_states_steering); + // py_input_steering = PyList_New(num_inputs_steering); - for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ - PyList_SetItem(py_state_steering, state_idx, PyFloat_FromDouble(state_steering(state_idx))); - } + // for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ + // PyList_SetItem(py_state_steering, state_idx, PyFloat_FromDouble(state_steering(state_idx))); + // } - for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ - PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); - } + // for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ + // PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); + // } + + py::scoped_interpreter guard{}; // start the interpreter and keep it alive + + py::print("Testing py::print"); // use the Python API + + // py::module_ sys = py::module_::import("control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis"); + + py::module_ torch = py::module_::import("torch"); + + // py::object result = torch.attr("add")(1, 2); std::cout << dt << std::endl; std::cout << "OKAAAAAYYYYYYY" << std::endl; @@ -156,33 +193,47 @@ void SimModelPymodels::update(const double & dt) vx_input_queue_.pop_front(); // Model the delay of steering -- TODO remove this after implementing python models - // steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); - // delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); - // steer_input_queue_.pop_front(); + steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); + delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); + steer_input_queue_.pop_front(); // Simulate the system (car kinematics and dynamics) // do not use deadzone_delta_steer (Steer IF does not exist in this model) updateRungeKutta(dt, delayed_input); // Simulate the steering system using python model - input_steering(0) = input_(IDX_U::STEER_DES); - for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ - PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); - } + // TODO: define inputs to the model in a more general way + /* + Following inputs to the steering system can be used: + + - IDX_U::STEER_DES + - IDX::VX + */ + // input_steering(0) = 0; + + + // for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ + // PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); + // } // Call forward() of the python model - PyObject *value = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"forward"), py_input_steering, py_state_steering, NULL); - py_state_steering = PyTuple_GetItem(value, 0); // The first output of the model forward is the next state + // PyObject *value = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"forward"), py_input_steering, py_state_steering, NULL); + // py_state_steering = PyTuple_GetItem(value, 0); // The first output of the model forward is the next state // PyObject *py_delayed_input = PyTuple_GetItem(value, 1); // The second output of the model forward is delayed input // State vector from python to C++ - for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ - state_steering(state_idx) = PyFloat_AsDouble(PyList_GetItem(PyList_GetItem(py_state_steering, 0), state_idx)); - } + // for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ + // state_steering(state_idx) = PyFloat_AsDouble(PyList_GetItem(PyList_GetItem(py_state_steering, 0), state_idx)); + // } //std::cout << "Required steer: " << input_(IDX_U::STEER_DES) << " State steering: " << state_steering(0) << std::endl; - state_(IDX::STEER) = state_steering(0); + // TODO: define outputs of the model in a more general way + /* + Following outputs from the steering system can be used: + - IDX::STEER + */ + state_(IDX::STEER) = 0; // Calculate current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; @@ -198,24 +249,9 @@ Eigen::VectorXd SimModelPymodels::calcModel( const double steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_); const double yaw = state(IDX::YAW); const double delay_input_vx = input(IDX_U::VX_DES); - //const double delay_input_steer = input(IDX_U::STEER_DES); const double delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); const double vx_rate = sat(-(vx - delay_vx_des) / vx_time_constant_, vx_rate_lim_, -vx_rate_lim_); - // const double delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); - - // const double steer_diff = steer - delay_steer_des; - // const double steer_diff_with_dead_band = std::invoke([&]() { - // if (steer_diff > steer_dead_band_) { - // return steer_diff - steer_dead_band_; - // } else if (steer_diff < -steer_dead_band_) { - // return steer_diff + steer_dead_band_; - // } else { - // return 0.0; - // } - // }); - - //const double steer_rate = - // sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vx * cos(yaw); From c131e3b16bdcea18a532250a12589c264b1e7400 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 18 Dec 2023 20:16:45 +0100 Subject: [PATCH 46/97] Split python models to models and learned model --- simulator/learned_model/CMakeLists.txt | 86 +++++++++++ simulator/learned_model/README.md | 1 + .../include/sim_pymodel_steer_vel.hpp | 15 ++ simulator/learned_model/package.xml | 25 ++++ .../simple_planning_simulator/CMakeLists.txt | 22 +-- .../vehicle_model/sim_model_pymodels.hpp | 30 ++-- .../simple_planning_simulator/package.xml | 3 + .../vehicle_model/sim_model_pymodels.cpp | 138 ++++++------------ 8 files changed, 203 insertions(+), 117 deletions(-) create mode 100644 simulator/learned_model/CMakeLists.txt create mode 100644 simulator/learned_model/README.md create mode 100644 simulator/learned_model/include/sim_pymodel_steer_vel.hpp create mode 100644 simulator/learned_model/package.xml diff --git a/simulator/learned_model/CMakeLists.txt b/simulator/learned_model/CMakeLists.txt new file mode 100644 index 0000000000000..d3d4025b4a77f --- /dev/null +++ b/simulator/learned_model/CMakeLists.txt @@ -0,0 +1,86 @@ +cmake_minimum_required(VERSION 3.14.4) +project(learned_model) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + + +## dependencies +find_package(ament_cmake_auto REQUIRED) +# ament_auto_find_build_dependencies() + +# find_package(autoware_cmake REQUIRED) +# autoware_package() + +find_package(Python3 COMPONENTS Interpreter Development) +find_package(pybind11 CONFIG) + +message("Pybind11_FOUND:${pybind11_FOUND}") +message("Pybind11_Version:${pybind11_VERSION}") + +message("Python_FOUND:${Python3_FOUND}") +message("Python_VERSION:${Python3_VERSION}") +message("Python_Development_FOUND:${Python3_Development_FOUND}") +message("Python_LIBRARIES:${Python3_LIBRARIES}") + +include_directories(include) + +# Component + +# ament_auto_add_library +add_library(${PROJECT_NAME} SHARED + # .cpp and .hpp files to instal + include/sim_pymodel_steer_vel.hpp +) + + +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$") + + + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME}/ +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) +target_compile_options(${PROJECT_NAME} PRIVATE -fvisibility=hidden) + +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(Python3 pybind11) + + +message("CMAKE_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX}") + +target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + + +if(BUILD_TESTING) + +endif() + + + +ament_auto_package() diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md new file mode 100644 index 0000000000000..d490d175585e6 --- /dev/null +++ b/simulator/learned_model/README.md @@ -0,0 +1 @@ +# learned_model diff --git a/simulator/learned_model/include/sim_pymodel_steer_vel.hpp b/simulator/learned_model/include/sim_pymodel_steer_vel.hpp new file mode 100644 index 0000000000000..d232e8c7ea8c7 --- /dev/null +++ b/simulator/learned_model/include/sim_pymodel_steer_vel.hpp @@ -0,0 +1,15 @@ +#ifndef LEARNED_MODEL__SIM_PYMODEL_STEER_VEL_HPP_ +#define LEARNED_MODEL__SIM_PYMODEL_STEER_VEL_HPP_ + +#include +#include +namespace py = pybind11; + +class SimPymodelSteerVel { + public: + int test_variable = 123; + py::scoped_interpreter guard{}; + py::print("Testing py::print LIB"); // use the Python API +}; + +#endif // LEARNED_MODEL__SIM_PYMODEL_STEER_VEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/package.xml b/simulator/learned_model/package.xml new file mode 100644 index 0000000000000..0b93550680900 --- /dev/null +++ b/simulator/learned_model/package.xml @@ -0,0 +1,25 @@ + + + + learned_model + 1.0.0 + learned_model for simple_planning_simulator + Takamasa Horibe + Tomoya Kimura + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_cmake + + + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 74c75db70fee5..dd4478dbae6c8 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -4,16 +4,11 @@ project(simple_planning_simulator) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(Python3 COMPONENTS Interpreter Development) -find_package(pybind11 CONFIG) +find_package(learned_model REQUIRED) -message("Pybind11_FOUND:${pybind11_FOUND}") -message("Pybind11_Version:${pybind11_VERSION}") - -message("Python_FOUND:${Python3_FOUND}") -message("Python_VERSION:${Python3_VERSION}") -message("Python_Development_FOUND:${Python3_Development_FOUND}") -message("Python_LIBRARIES:${Python3_LIBRARIES}") +message("learned_model_FOUND: ${learned_model_FOUND}") +message("learned_model_LIBRARIES: ${learned_model_LIBRARIES}") +message("learned_model_INCLUDE_DIRS: ${learned_model_INCLUDE_DIRS}") # Component @@ -34,8 +29,10 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${tf2_INCLUDE_DIRS}) +ament_target_dependencies(${PROJECT_NAME} learned_model) + -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-old-style-cast) # RCLCPP_ERROR_THROTTLE() has built-in old-style casts. +target_compile_options(${PROJECT_NAME} PRIVATE -Wno-old-style-cast -fvisibility=hidden) # RCLCPP_ERROR_THROTTLE() has built-in old-style casts. # Node executable rclcpp_components_register_node(${PROJECT_NAME} @@ -43,11 +40,6 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_exe ) -# target_link_options(${PROJECT_NAME} PRIVATE -shared -fPIC -export-dynamic) # - -# target_link_libraries(${PROJECT_NAME} ${Python3_LIBRARIES}) 3 -# /usr/lib/x86_64-linux-gnu/libpython3.10.so -target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_simple_planning_simulator diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index 1aa65c338d9b2..3267221ac88d2 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -24,8 +24,8 @@ #include #include -#include -#include +// #include +// namespace py = pybind11; /** * @class SimModelPymodels @@ -47,6 +47,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ * @param [in] steer_delay time delay for steering command [s] * @param [in] steer_time_constant time constant for 1D model of steering dynamics * @param [in] steer_dead_band dead band for steering angle [rad] + * @param [in] HELLO_FROM_CODING_GOD PRAISE THE MESSIAH!!!!! <3 */ SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, @@ -67,17 +68,17 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ In order not to compare strings each iteration, we compute mappings between the states and inputs of this model and python submodels. */ - const char* STATE_NAMES[5] = {"POS_X", "POS_Y", "YAW", "VX", "STEER"}; - const char* INPUT_NAMES[2] = {"VX_DES", "STEER_DES"}; + // const char* STATE_NAMES[5] = {"POS_X", "POS_Y", "YAW", "VX", "STEER"}; + // const char* INPUT_NAMES[2] = {"VX_DES", "STEER_DES"}; /* Index means state or input in this model. First 5 indexes are states, last 2 indexes are inputs. ---------------------------- { STATES }{INPUTS} */ - int MAP_TO_PYSTEER_INPUT[7] = {-1, -1, -1, -1, -1, -1, -1}; // <<- Value means index of the input in python steer model. -1 means not used. + // int MAP_TO_PYSTEER_INPUT[7] = {-1, -1, -1, -1, -1, -1, -1}; // <<- Value means index of the input in python steer model. -1 means not used. /* Index means state in this model. ------------------------------- { STATES }*/ - int MAP_FROM_PYSTEER_OUTPUT[5] = {-1, -1, -1, -1, -1}; // <<- Value means index of the output in python steer model. -1 means not used. + // int MAP_FROM_PYSTEER_OUTPUT[5] = {-1, -1, -1, -1, -1}; // <<- Value means index of the output in python steer model. -1 means not used. enum IDX { X = 0, @@ -90,6 +91,8 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ VX_DES = 0, STEER_DES, }; + + // py::scoped_interpreter guard{}; const double vx_lim_; //!< @brief velocity limit @@ -113,15 +116,20 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ //PyObject *steer_base_model; - int num_states_steering; - int num_inputs_steering; + // int num_states_steering; + // int num_inputs_steering; // Create vectors to store system input and state (C++) - Eigen::VectorXd state_steering; - Eigen::VectorXd input_steering; + // Eigen::VectorXd state_steering; + // Eigen::VectorXd input_steering; // Create vectors to store system input and state (python) - //PyObject *py_state_steering, *py_input_steering; + // PyObject *py_state_steering, *py_input_steering; + + // std::vector state_steering; + // std::vector input_steering; + + // py::object steering_module_class; /** diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index a2038486029dc..478804e5da151 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_cmake + learned_model autoware_auto_control_msgs autoware_auto_mapping_msgs @@ -35,6 +36,8 @@ tier4_vehicle_msgs vehicle_info_util + + learned_model launch_ros ament_cmake_ros diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 0aee8a36a5aa5..140fddbd75187 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -17,8 +17,11 @@ #include // #include // #include -#include -namespace py = pybind11; +// #include +// #include +// namespace py = pybind11; + +#include SimModelPymodels::SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, @@ -39,9 +42,7 @@ SimModelPymodels::SimModelPymodels( { // Initialize python library - //dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. - - + // dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. /* More about the line above here: https://stackoverflow.com/questions/60719987/embedding-python-which-uses-numpy-in-c-doesnt-work-in-library-dynamically-loa @@ -49,102 +50,68 @@ SimModelPymodels::SimModelPymodels( https://stackoverflow.com/questions/67891197/ctypes-cpython-39-x86-64-linux-gnu-so-undefined-symbol-pyfloat-type-in-embedd https://man7.org/linux/man-pages/man3/dlopen.3.html */ - // Py_Initialize(); - // PyObject *py_module, *py_module_dict, *python_class; - + // start the interpreter and keep it alive + // char *model_save_path = (char*)"/home/tomas/f1tenth/ws_testing/base_model_save"; - // Import python module - // py_module = PyImport_Import(PyUnicode_FromString((char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis")); + // // Import python module + // py::module_ steering_model_module = py::module_::import("control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis"); - // if (py_module == nullptr) { - // PyErr_Print(); - // return; - // } + // steering_module_class = steering_model_module.attr("SimpleSteeringHyst")(nullptr); - // py_module_dict = PyModule_GetDict(py_module); - // if (py_module_dict == nullptr) { - // PyErr_Print(); - // return; - // } - // python_class = PyDict_GetItemString(py_module_dict, "SimpleSteeringHyst"); - // if (python_class == nullptr) { - // PyErr_Print(); - // return; - // } - - // if (PyCallable_Check(python_class)) { - // steer_base_model = PyObject_CallObject(python_class, nullptr); - // Py_DECREF(python_class); - // } else { - // Py_DECREF(python_class); - // return; - // } - - - // TODO call function to get model signal names - // From the names create a mapping table for correctly pass arguments to the python models - - // PyObject *py_sig_state_names = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"get_sig_state_names"), NULL); + // // TODO call function to get model signal names + // // From the names create a mapping table for correctly pass arguments to the python models + // py::object sig_state_names = steering_module_class.attr("get_sig_state_names")(); // char *my_result; - - // PyObject * temp_bytes = PyUnicode_AsEncodedString(PyList_GetItem(py_sig_state_names, 0), "UTF-8", "strict"); + // PyObject *temp_bytes = PyUnicode_AsEncodedString(PyList_GetItem(sig_state_names.ptr(), 0), "UTF-8", "strict"); // my_result = PyBytes_AS_STRING(temp_bytes); // my_result = strdup(my_result); // Py_DECREF(temp_bytes); - // wcstombs(buffer, PyList_GetItem(py_sig_state_names, 0), sizeof(buffer)); + // // wcstombs(buffer, PyList_GetItem(py_sig_state_names, 0), sizeof(buffer)); - // int len_ = PyList_Size(py_sig_state_names); + // // int len_ = PyList_Size(py_sig_state_names); // std::cout << "STATE NAMES: -" << my_result << "-" << std::endl; - // std::cout << "STEER: " << len_ << std::endl; - // std::cout << "STEER: " << strcmp(my_result, "STEER") << std::endl; - // std::cout << "STEER: " << strcmp(my_result, STATE_NAMES[4]) << std::endl; - // std::cout << "STEERR: " << strcmp(my_result, "STEERR") << std::endl; + // // std::cout << "STEER: " << len_ << std::endl; + // // std::cout << "STEER: " << strcmp(my_result, "STEER") << std::endl; + // // std::cout << "STEER: " << strcmp(my_result, STATE_NAMES[4]) << std::endl; + // // std::cout << "STEERR: " << strcmp(my_result, "STEERR") << std::endl; - // Load model params - // PyObject *load_params_succ = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"load_params"), PyUnicode_FromString(model_save_path), NULL); + // // Load model params + // py::object load_params_succ = steering_module_class.attr("load_params")(model_save_path); - // if(!PyBool_Check(load_params_succ)){ - // return; - // } - // Reset the python model after loading the parameter file - // PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"reset"), NULL); + // // Reset the python model after loading the parameter file + // steering_module_class.attr("reset")(); - // Set number model states and inputs + // // Set number model states and inputs // num_states_steering = 1; // num_inputs_steering = 1; // // Initialize state and input vectors - // state_steering = Eigen::VectorXd::Zero(num_states_steering); - // input_steering = Eigen::VectorXd::Zero(num_inputs_steering); - - - // py_state_steering = PyList_New(num_states_steering); - // py_input_steering = PyList_New(num_inputs_steering); - - // for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ - // PyList_SetItem(py_state_steering, state_idx, PyFloat_FromDouble(state_steering(state_idx))); + // for (int i = 0; i < num_inputs_steering; i++){ + // input_steering.push_back(0.0); // } - // for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ - // PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); + // for (int i = 0; i < num_states_steering; i++){ + // state_steering.push_back(0.0); // } - py::scoped_interpreter guard{}; // start the interpreter and keep it alive - - py::print("Testing py::print"); // use the Python API - // py::module_ sys = py::module_::import("control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis"); + // std::vector state_steering(num_states_steering, 0); + // std::vector input_steering(num_inputs_steering, 0); + + // std::cout << action_delayed[0]<< std::endl; + + SimPymodelSteerVel test_; + std::cout << "-----> " << test_.test_variable << std::endl; - py::module_ torch = py::module_::import("torch"); - // py::object result = torch.attr("add")(1, 2); + // py::print("Testing py::print"); // use the Python API std::cout << dt << std::endl; std::cout << "OKAAAAAYYYYYYY" << std::endl; @@ -192,11 +159,6 @@ void SimModelPymodels::update(const double & dt) delayed_input(IDX_U::VX_DES) = vx_input_queue_.front(); vx_input_queue_.pop_front(); - // Model the delay of steering -- TODO remove this after implementing python models - steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); - delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); - steer_input_queue_.pop_front(); - // Simulate the system (car kinematics and dynamics) // do not use deadzone_delta_steer (Steer IF does not exist in this model) updateRungeKutta(dt, delayed_input); @@ -210,29 +172,23 @@ void SimModelPymodels::update(const double & dt) - IDX_U::STEER_DES - IDX::VX */ - // input_steering(0) = 0; - - - // for (int input_idx = 0; input_idx < num_inputs_steering; input_idx++){ - // PyList_SetItem(py_input_steering, input_idx, PyFloat_FromDouble(input_steering(input_idx))); - // } + // input_steering[0] = input_(IDX_U::STEER_DES); + - // Call forward() of the python model - // PyObject *value = PyObject_CallMethodObjArgs(steer_base_model, PyUnicode_FromString((char*)"forward"), py_input_steering, py_state_steering, NULL); - // py_state_steering = PyTuple_GetItem(value, 0); // The first output of the model forward is the next state - // PyObject *py_delayed_input = PyTuple_GetItem(value, 1); // The second output of the model forward is delayed input + // // Call forward() of the python model + // py::tuple res = steering_module_class.attr("forward")(state_steering, input_steering); - // State vector from python to C++ - // for (int state_idx = 0; state_idx < num_states_steering; state_idx++){ - // state_steering(state_idx) = PyFloat_AsDouble(PyList_GetItem(PyList_GetItem(py_state_steering, 0), state_idx)); - // } - //std::cout << "Required steer: " << input_(IDX_U::STEER_DES) << " State steering: " << state_steering(0) << std::endl; + // std::vector next_state = res[0].cast>>()[0]; + // state_steering[0] = next_state[0]; + // std::vector action_delayed = res[1].cast>>()[0]; + // std::cout << next_state[0]<< std::endl; // TODO: define outputs of the model in a more general way /* Following outputs from the steering system can be used: - IDX::STEER */ + // state_(IDX::STEER) = next_state[0]; state_(IDX::STEER) = 0; // Calculate From 9e5c33cd0d38083f46756a299bff966e8c21f615 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 19 Dec 2023 11:18:44 +0100 Subject: [PATCH 47/97] Changes from Maxime --- simulator/learned_model/CMakeLists.txt | 80 ++----------------- .../include/sim_pymodel_steer_vel.hpp | 12 ++- simulator/learned_model/package.xml | 5 +- .../src/sim_pymodel_steer_vel.cpp | 1 + .../simple_planning_simulator/CMakeLists.txt | 18 +---- .../simple_planning_simulator/package.xml | 4 +- .../vehicle_model/sim_model_pymodels.cpp | 2 +- 7 files changed, 25 insertions(+), 97 deletions(-) create mode 100644 simulator/learned_model/src/sim_pymodel_steer_vel.cpp diff --git a/simulator/learned_model/CMakeLists.txt b/simulator/learned_model/CMakeLists.txt index d3d4025b4a77f..31f068529a440 100644 --- a/simulator/learned_model/CMakeLists.txt +++ b/simulator/learned_model/CMakeLists.txt @@ -1,86 +1,22 @@ cmake_minimum_required(VERSION 3.14.4) project(learned_model) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - - - -## dependencies -find_package(ament_cmake_auto REQUIRED) -# ament_auto_find_build_dependencies() - -# find_package(autoware_cmake REQUIRED) -# autoware_package() +find_package(autoware_cmake REQUIRED) +autoware_package() find_package(Python3 COMPONENTS Interpreter Development) find_package(pybind11 CONFIG) -message("Pybind11_FOUND:${pybind11_FOUND}") -message("Pybind11_Version:${pybind11_VERSION}") - -message("Python_FOUND:${Python3_FOUND}") -message("Python_VERSION:${Python3_VERSION}") -message("Python_Development_FOUND:${Python3_Development_FOUND}") -message("Python_LIBRARIES:${Python3_LIBRARIES}") - -include_directories(include) - -# Component - -# ament_auto_add_library -add_library(${PROJECT_NAME} SHARED - # .cpp and .hpp files to instal - include/sim_pymodel_steer_vel.hpp +ament_auto_add_library(${PROJECT_NAME} SHARED + # include/sim_pymodel_steer_vel.hpp + src/sim_pymodel_steer_vel.cpp ) - - -target_include_directories(${PROJECT_NAME} - PUBLIC - "$" - "$") - - +target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) +target_include_directories(${PROJECT_NAME} PRIVATE ${Python3_INCLUDE_DIRS}) install( DIRECTORY include/ - DESTINATION include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} ) -install( - TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - -set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) -target_compile_options(${PROJECT_NAME} PRIVATE -fvisibility=hidden) - -ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(Python3 pybind11) - - -message("CMAKE_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX}") - -target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) - - -if(BUILD_TESTING) - -endif() - - - ament_auto_package() diff --git a/simulator/learned_model/include/sim_pymodel_steer_vel.hpp b/simulator/learned_model/include/sim_pymodel_steer_vel.hpp index d232e8c7ea8c7..5237d27acc833 100644 --- a/simulator/learned_model/include/sim_pymodel_steer_vel.hpp +++ b/simulator/learned_model/include/sim_pymodel_steer_vel.hpp @@ -5,11 +5,15 @@ #include namespace py = pybind11; -class SimPymodelSteerVel { - public: - int test_variable = 123; +class SimPymodelSteerVel +{ +public: + int test_variable = 123; + SimPymodelSteerVel() + { py::scoped_interpreter guard{}; - py::print("Testing py::print LIB"); // use the Python API + py::print("Testing py::print LIB"); // use the Python API + } }; #endif // LEARNED_MODEL__SIM_PYMODEL_STEER_VEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/package.xml b/simulator/learned_model/package.xml index 0b93550680900..9fa8c511a858d 100644 --- a/simulator/learned_model/package.xml +++ b/simulator/learned_model/package.xml @@ -13,7 +13,8 @@ autoware_cmake - + pybind11 + python3-dev ament_cmake_ros ament_lint_auto @@ -22,4 +23,4 @@ ament_cmake - + \ No newline at end of file diff --git a/simulator/learned_model/src/sim_pymodel_steer_vel.cpp b/simulator/learned_model/src/sim_pymodel_steer_vel.cpp new file mode 100644 index 0000000000000..cce3792a68a19 --- /dev/null +++ b/simulator/learned_model/src/sim_pymodel_steer_vel.cpp @@ -0,0 +1 @@ +#include "learned_model/sim_pymodel_steer_vel.hpp" //"learned_model/include/sim_pymodel_steer_vel.hpp" \ No newline at end of file diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index dd4478dbae6c8..5d22a98ec8a28 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -2,15 +2,10 @@ cmake_minimum_required(VERSION 3.14) project(simple_planning_simulator) find_package(autoware_cmake REQUIRED) +find_package(Python3 COMPONENTS Interpreter Development) +find_package(learned_model) autoware_package() -find_package(learned_model REQUIRED) - -message("learned_model_FOUND: ${learned_model_FOUND}") -message("learned_model_LIBRARIES: ${learned_model_LIBRARIES}") -message("learned_model_INCLUDE_DIRS: ${learned_model_INCLUDE_DIRS}") - - # Component ament_auto_add_library(${PROJECT_NAME} SHARED include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -27,20 +22,13 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp src/simple_planning_simulator/utils/csv_loader.cpp ) -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${tf2_INCLUDE_DIRS}) - -ament_target_dependencies(${PROJECT_NAME} learned_model) - - -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-old-style-cast -fvisibility=hidden) # RCLCPP_ERROR_THROTTLE() has built-in old-style casts. - +target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS}) # Node executable rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "simulation::simple_planning_simulator::SimplePlanningSimulator" EXECUTABLE ${PROJECT_NAME}_exe ) - if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_simple_planning_simulator test/test_simple_planning_simulator.cpp diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 478804e5da151..52f8591c0ee48 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -12,7 +12,6 @@ autoware_cmake autoware_cmake - learned_model autoware_auto_control_msgs autoware_auto_mapping_msgs @@ -22,6 +21,7 @@ geometry_msgs lanelet2_core lanelet2_extension + learned_model motion_utils nav_msgs rclcpp @@ -36,8 +36,6 @@ tier4_vehicle_msgs vehicle_info_util - - learned_model launch_ros ament_cmake_ros diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 140fddbd75187..db4a96488d9d6 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -21,7 +21,7 @@ // #include // namespace py = pybind11; -#include +#include "learned_model/include/sim_pymodel_steer_vel.hpp" SimModelPymodels::SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, From 12f612213f5e8429752a076f93b55dd32a8226e6 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 19 Dec 2023 11:51:32 +0100 Subject: [PATCH 48/97] Fix build issiues --- simulator/learned_model/CMakeLists.txt | 1 - simulator/learned_model/src/sim_pymodel_steer_vel.cpp | 2 +- simulator/simple_planning_simulator/CMakeLists.txt | 4 ++-- .../vehicle_model/sim_model_pymodels.cpp | 2 +- 4 files changed, 4 insertions(+), 5 deletions(-) diff --git a/simulator/learned_model/CMakeLists.txt b/simulator/learned_model/CMakeLists.txt index 31f068529a440..be3e67d0cd114 100644 --- a/simulator/learned_model/CMakeLists.txt +++ b/simulator/learned_model/CMakeLists.txt @@ -8,7 +8,6 @@ find_package(Python3 COMPONENTS Interpreter Development) find_package(pybind11 CONFIG) ament_auto_add_library(${PROJECT_NAME} SHARED - # include/sim_pymodel_steer_vel.hpp src/sim_pymodel_steer_vel.cpp ) target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) diff --git a/simulator/learned_model/src/sim_pymodel_steer_vel.cpp b/simulator/learned_model/src/sim_pymodel_steer_vel.cpp index cce3792a68a19..98222cac22134 100644 --- a/simulator/learned_model/src/sim_pymodel_steer_vel.cpp +++ b/simulator/learned_model/src/sim_pymodel_steer_vel.cpp @@ -1 +1 @@ -#include "learned_model/sim_pymodel_steer_vel.hpp" //"learned_model/include/sim_pymodel_steer_vel.hpp" \ No newline at end of file +#include "../include/sim_pymodel_steer_vel.hpp" \ No newline at end of file diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 5d22a98ec8a28..85ce6986ee111 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -3,7 +3,7 @@ project(simple_planning_simulator) find_package(autoware_cmake REQUIRED) find_package(Python3 COMPONENTS Interpreter Development) -find_package(learned_model) +find_package(learned_model REQUIRED) autoware_package() # Component @@ -22,7 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp src/simple_planning_simulator/utils/csv_loader.cpp ) -target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS}) +target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS} ${learned_model_INCLUDE_DIRS}) # Node executable rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "simulation::simple_planning_simulator::SimplePlanningSimulator" diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index db4a96488d9d6..d7f4f3635850b 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -21,7 +21,7 @@ // #include // namespace py = pybind11; -#include "learned_model/include/sim_pymodel_steer_vel.hpp" +#include "sim_pymodel_steer_vel.hpp" SimModelPymodels::SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, From 373c72be9ba4efca3129162de57f163aa768cdc8 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Thu, 18 Jan 2024 18:29:15 +0100 Subject: [PATCH 49/97] Add C++ library that runs python models --- .../include/model_connections_helpers.hpp | 5 + .../include/pymodel_base_error.hpp | 81 +++++++ .../include/pymodel_interconnected_model.hpp | 207 ++++++++++++++++++ .../include/pymodel_interface.hpp | 39 ++++ .../include/pymodel_simple_model.hpp | 180 +++++++++++++++ .../include/sim_pymodel_steer_vel.hpp | 19 -- .../src/model_connections_helpers.cpp | 1 + .../learned_model/src/pymodel_base_error.cpp | 1 + .../src/pymodel_interconnected_model.cpp | 1 + .../learned_model/src/pymodel_interface.cpp | 1 + .../src/pymodel_simple_model.cpp | 1 + .../src/sim_pymodel_steer_vel.cpp | 1 - 12 files changed, 517 insertions(+), 20 deletions(-) create mode 100644 simulator/learned_model/include/model_connections_helpers.hpp create mode 100644 simulator/learned_model/include/pymodel_base_error.hpp create mode 100644 simulator/learned_model/include/pymodel_interconnected_model.hpp create mode 100644 simulator/learned_model/include/pymodel_interface.hpp create mode 100644 simulator/learned_model/include/pymodel_simple_model.hpp delete mode 100644 simulator/learned_model/include/sim_pymodel_steer_vel.hpp create mode 100644 simulator/learned_model/src/model_connections_helpers.cpp create mode 100644 simulator/learned_model/src/pymodel_base_error.cpp create mode 100644 simulator/learned_model/src/pymodel_interconnected_model.cpp create mode 100644 simulator/learned_model/src/pymodel_interface.cpp create mode 100644 simulator/learned_model/src/pymodel_simple_model.cpp delete mode 100644 simulator/learned_model/src/sim_pymodel_steer_vel.cpp diff --git a/simulator/learned_model/include/model_connections_helpers.hpp b/simulator/learned_model/include/model_connections_helpers.hpp new file mode 100644 index 0000000000000..0acdc60333e20 --- /dev/null +++ b/simulator/learned_model/include/model_connections_helpers.hpp @@ -0,0 +1,5 @@ +#ifndef LEARNED_MODEL__MODEL_CONNECTION_HELPERS_ +#define LEARNED_MODEL__MODEL_CONNECTION_HELPERS_ + + +#endif // LEARNED_MODEL__MODEL_CONNECTION_HELPERS_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_base_error.hpp b/simulator/learned_model/include/pymodel_base_error.hpp new file mode 100644 index 0000000000000..ef2e6116ea072 --- /dev/null +++ b/simulator/learned_model/include/pymodel_base_error.hpp @@ -0,0 +1,81 @@ +#ifndef LEARNED_MODEL__SIM_PYMODEL_BASE_ERROR_HPP_ +#define LEARNED_MODEL__SIM_PYMODEL_BASE_ERROR_HPP_ + +#include "pymodel_simple_model.hpp" +#include "pymodel_interface.hpp" + + +class SimPymodelBaseError : public PymodelInterface +{ +private: + PymodelSimpleModel base_model; + PymodelSimpleModel error_model; + +public: + SimPymodelBaseError(std::tuple base_desc, std::tuple error_desc) + : base_model(std::get<0>(base_desc), std::get<1>(base_desc), std::get<2>(base_desc)), + error_model(std::get<0>(error_desc), std::get<1>(error_desc), std::get<2>(error_desc)) + { + } + + /** + * @brief create a map from model signal vector to python model inputs + * @param [in] signal_vec_names names of signals in model signal vector + */ + void mapInputs(std::vector signals_vec_names) override + { + base_model.mapInputs(signals_vec_names); + error_model.mapInputs(signals_vec_names); + } + + /** + * @brief create a map from python outputs to model signal vector + * @param [in] signal_vec_names names of signals in model signal vector + */ + void mapOutputs(std::vector signals_vec_names) override + { + base_model.mapOutputs(signals_vec_names); + error_model.mapOutputs(signals_vec_names); + } + + /** + * @brief get names of inputs of python model + */ + std::vector getInputNames() override + { + std::vector base_input_name = base_model.getInputNames(); + std::vector error_input_name = error_model.getInputNames(); + base_input_name.insert(base_input_name.end(), error_input_name.begin(), error_input_name.end()); + return base_input_name; + } + + /** + * @brief get names of states of python model + */ + std::vector getStateNames() override + { + std::vector base_state_name = base_model.getStateNames(); + std::vector error_state_name = error_model.getStateNames(); + base_state_name.insert(base_state_name.end(), error_state_name.begin(), error_state_name.end()); + return base_state_name; + } + + /** + * @brief calculate the next state of this submodule + * @param [in] model_signals_vec values of signals in model signal vector + * @param [in] model_signals_vec_next values of signals in model signal vector to update + */ + std::vector getNextState(std::vector model_signals_vec, std::vector model_signals_vec_next) override + { + std::vector base_next_state = base_model.getNextState(model_signals_vec, model_signals_vec_next); + std::vector base_next_state_delayed = base_model.signals_w_delayed_input; + std::vector next_state_error(model_signals_vec.size()); + std::fill(next_state_error.begin(), next_state_error.end(), 0.0); + std::vector error_correction = error_model.getNextState(base_next_state_delayed, next_state_error); // need to change input in all_variable input to delayed one from base model + for (size_t i = 0; i < base_next_state.size(); i++) base_next_state[i] += error_correction[i]; + return base_next_state; + } +}; + + +#endif // LEARNED_MODEL__SIM_PYMODEL_BASE_ERROR_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_interconnected_model.hpp b/simulator/learned_model/include/pymodel_interconnected_model.hpp new file mode 100644 index 0000000000000..0dfbc23df70e7 --- /dev/null +++ b/simulator/learned_model/include/pymodel_interconnected_model.hpp @@ -0,0 +1,207 @@ +#ifndef LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ +#define LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ + +#include "pymodel_base_error.hpp" +#include "pymodel_interface.hpp" +#include "pymodel_simple_model.hpp" +#include "model_connections_helpers.hpp" + +#include +#include +#include + +#include + +namespace py = pybind11; +// sim_pymodel_steer_vel +class InterconnectedModel +{ + int num_signals; + + std::vector model_signals_vec; + std::vector signals_vec_names; + + std::vector> submodels; + + std::vector + map_in_to_sig_vec; // index in "map_in_to_pyin" is index in "py_inputs" and value in + // "map_in_to_pyin" is index in "all_variables_names" + std::vector + map_sig_vec_to_out; // index in "map_pyout_to_out" is index in "pymodel_outputs" and value in + // "map_pyout_to_out" is index in "all_variables_names" + +public: + py::scoped_interpreter guard{}; // start the interpreter and keep it alive + + /** + * @brief constructor + */ + InterconnectedModel() + { + // Initialize python library + dlopen( + "libpython3.10.so", + RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. + /* + More about the line above here: + https://stackoverflow.com/questions/60719987/embedding-python-which-uses-numpy-in-c-doesnt-work-in-library-dynamically-loa + https://mail.python.org/pipermail/new-bugs-announce/2008-November/003322.html + https://stackoverflow.com/questions/67891197/ctypes-cpython-39-x86-64-linux-gnu-so-undefined-symbol-pyfloat-type-in-embedd + https://man7.org/linux/man-pages/man3/dlopen.3.html + */ + } + +private: + std::vector createConnectionsMap(std::vector connection_names_1, std::vector connection_names_2) + { + std::vector connection_map; + // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is + // index in "connection_names_1" + for (auto name_2 : connection_names_2) { + int mapped_idx = -1; // -1 means that we cannot create a connection between some signals + for (size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { + if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same + // and we can create connection + mapped_idx = NAME_1_IDX; + break; + } + } + connection_map.push_back(mapped_idx); + } + return connection_map; + } + + void mapInputs(std::vector in_names) + { + // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index in "signals_vec_names" + map_in_to_sig_vec = createConnectionsMap(signals_vec_names, in_names); + } + + void mapOutputs(std::vector out_names) + { + // index in "map_sig_vec_to_out" is index in "out_names" and value in "map_sig_vec_to_out" is index in "signals_vec_names" + map_sig_vec_to_out = createConnectionsMap(signals_vec_names, out_names); + } + + void addNamesToSigVec(const std::vector & names) + { + for (char * name : names) { + if (std::find(signals_vec_names.begin(), signals_vec_names.end(), name) == signals_vec_names.end()) { + signals_vec_names.push_back(name); + } + } + } + + void getSignalNames() + { + for (auto & submodel : submodels) { + addNamesToSigVec(submodel->getInputNames()); + addNamesToSigVec(submodel->getStateNames()); + } + } + +public: + /** + * @brief automatically create connections between PSIM and all of the sub-models + * @param [in] in_names string names of inputs available from PSIM + * @param [in] out_names string names of outputs required by PSIM + */ + void generateConnections(std::vector in_names, std::vector out_names) + { + getSignalNames(); + num_signals = signals_vec_names.size(); + for (int i = 0; i < num_signals; i++) model_signals_vec.push_back(0); + + for (auto & submodel : submodels) { + submodel->mapInputs(signals_vec_names); + submodel->mapOutputs(signals_vec_names); + } + mapInputs(in_names); + mapOutputs(out_names); + } + + /** + * @brief add a sub-model consisting of base + error model + * @param [in] submodel_desc descriptor of the sub-model + */ + void addSubmodel(std::tuple submodel_desc) + { + const auto [lib_path, param_path, class_name] = submodel_desc; + submodels.push_back(std::unique_ptr( + new PymodelSimpleModel(lib_path, param_path, class_name))); + } + + /** + * @brief add a sub-model consisting of base + error sub-model + * @param [in] base_desc descriptor of base sub-model + * @param [in] error_desc descriptor of error sub-model + */ + void addSubmodelBaseError( + std::tuple base_desc, std::tuple error_desc) + { + // SimPymodelBaseError pymodel(base_desc, error_desc); + // submodels.push_back(pymodel); + std::cout << "HAHA111" << std::endl; + submodels.push_back( + std::unique_ptr(new SimPymodelBaseError(base_desc, error_desc))); + } + + /** + * @brief set a new model state if it was changed using PSIM interface (mainly position and orientation) + * @param [in] new_state new state set by PSIM + */ + void initState(std::vector new_state) + { + bool state_changed_externally = false; + + for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { + if ( + abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > + 1e-6) { + state_changed_externally = true; + break; + } + } + + if (state_changed_externally) { + std::cout << "Reseting model... ----***----" << std::endl; + + std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); + + for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { + model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] = new_state[PSIM_STATE_IDX]; + } + } + } + + /** + * @brief compute next step of the PSIM model using python sub-models + * @param [in] psim_input vector of input values provided by PSIM + */ + std::vector updatePymodel(std::vector psim_input) + { + // map input to vector of all variables + for (size_t PSIM_INPUT_IDX = 0; PSIM_INPUT_IDX < psim_input.size(); PSIM_INPUT_IDX++) { + model_signals_vec[map_in_to_sig_vec[PSIM_INPUT_IDX]] = psim_input[PSIM_INPUT_IDX]; + } + + // Compute forward pass through all models (order should not matter) + std::vector model_signals_vec_next(num_signals); + for (auto & submodel : submodels) { + model_signals_vec_next = submodel->getNextState(model_signals_vec, model_signals_vec_next); + } + + // Map vector of all variables to + std::vector psim_next_state; + for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < map_sig_vec_to_out.size(); PSIM_STATE_IDX++) { + psim_next_state.push_back(model_signals_vec_next[map_sig_vec_to_out[PSIM_STATE_IDX]]); + } + + // Update vector of all variables + model_signals_vec = model_signals_vec_next; + + return psim_next_state; + } +}; + +#endif // LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_interface.hpp b/simulator/learned_model/include/pymodel_interface.hpp new file mode 100644 index 0000000000000..6e57384aa4c9d --- /dev/null +++ b/simulator/learned_model/include/pymodel_interface.hpp @@ -0,0 +1,39 @@ +#ifndef LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ +#define LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ + +class PymodelInterface + { + public: + // virtual ~PymodelInterface() = 0; // <-- build fails after uncommenting this + + /** + * @brief get names of inputs of python model + */ + virtual std::vector getInputNames() = 0; + + /** + * @brief get names of states of python model + */ + virtual std::vector getStateNames() = 0; + + /** + * @brief create a map from model signal vector to python model inputs + * @param [in] signal_vec_names names of signals in model signal vector + */ + virtual void mapInputs(std::vector signals_vec_names) = 0; + + /** + * @brief create a map from python outputs to model signal vector + * @param [in] signal_vec_names names of signals in model signal vector + */ + virtual void mapOutputs(std::vector signals_vec_names) = 0; + + /** + * @brief calculate the next state of this submodule + * @param [in] model_signals_vec values of signals in model signal vector + * @param [in] model_signals_vec_next values of signals in model signal vector to update + */ + virtual std::vector getNextState(std::vector model_signals_vec, std::vector model_signals_vec_next) = 0; +}; + +#endif // LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_simple_model.hpp b/simulator/learned_model/include/pymodel_simple_model.hpp new file mode 100644 index 0000000000000..693cd12329292 --- /dev/null +++ b/simulator/learned_model/include/pymodel_simple_model.hpp @@ -0,0 +1,180 @@ +#ifndef LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ +#define LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ + +#include +#include + +#include "pymodel_interface.hpp" +#include "model_connections_helpers.hpp" + +namespace py = pybind11; + +/** + * @class SimModelPymodels + * @brief This class is an interface between C++ and python models. + */ +class PymodelSimpleModel : public PymodelInterface +{ +private: + char* pymodel_import_name = nullptr; + + int num_inputs_py; + int num_outputs_py; + + py::object py_model_class; + + std::vector map_sig_vec_to_pyin; // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is index in "signals_vec_names" + std::vector map_pyout_to_sig_vec; // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in "map_pyout_to_sig_vec" is index in "signals_vec_names" + + std::vector pymodel_input_names; + std::vector pymodel_state_names; + +public: + std::vector signals_w_delayed_input; + + /** + * @brief constructor + * @param [in] pymodel_import_name_ path to python model + * @param [in] param_file_path path to saved parameter file of the python sub-model + * @param [in] py_class_name name of the python class + */ + PymodelSimpleModel(char* pymodel_import_name_, char* param_file_path, char* py_class_name) + { + // Import model class + pymodel_import_name = pymodel_import_name_; + if (pymodel_import_name != nullptr) + { + // Import python module + py::module_ imported_module = py::module_::import(pymodel_import_name); + // Initialize model class from imported module + py_model_class = imported_module.attr(py_class_name)(nullptr); + } + else + { + // TODO throw exception/error + return; + } + + // Load model parameters and reset the model + if (param_file_path != nullptr) + { + py::object load_params_succ = py_model_class.attr("load_params")(param_file_path); + py_model_class.attr("reset")(); + std::cout << "Params loaded" << std::endl; + } + else + { + // TODO warning that using default model params + } + + py::list pymodel_state_names_ = py_model_class.attr("get_sig_state_names")(); + num_outputs_py = pymodel_state_names_.size(); + for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++){ + pymodel_state_names.push_back(PyBytes_AS_STRING(PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); + } + + py::list pymodel_input_names_ = py_model_class.attr("get_sig_action_names")(); + num_inputs_py = pymodel_input_names_.size(); + for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++){ + pymodel_input_names.push_back(PyBytes_AS_STRING(PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); + } + + std::cout << "PymodelInterface import name: " << pymodel_import_name << std::endl; + } + + /** + * @brief calculate the next state of a python model + * @param [in] model_signals_vec all available inputs from PSIM + */ + std::vector getNextState(std::vector model_signals_vec, std::vector model_signals_vec_next) override + { + + // get inputs and states of the python model from the vector of all signals + std::vector py_inputs(num_inputs_py); + std::vector py_state(num_outputs_py); + py_inputs = fillVectorUsingMap(py_inputs, model_signals_vec, map_sig_vec_to_pyin, true); + py_state = fillVectorUsingMap(py_state, model_signals_vec, map_pyout_to_sig_vec, true); + + // forward pass through the base model + py::tuple res = py_model_class.attr("forward")(py_inputs, py_state); + std::vector py_state_next = res[0].cast>>()[0]; + + // for (auto state : model_state) std::cout << "STATE AFTER : " << state << std::endl; + + std::vector action_delayed = res[1].cast>>()[0]; + + // map outputs from python model to required outputs + std::vector next_state = fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); + + signals_w_delayed_input = fillVectorUsingMap(action_delayed, model_signals_vec_next, map_sig_vec_to_pyin, false); + + return next_state; + } + + /** + * @brief get names of inputs of python model + */ + std::vector getInputNames() override + { + return pymodel_input_names; + } + + /** + * @brief get names of states of python model + */ + std::vector getStateNames() override + { + return pymodel_state_names; + } + + /** + * @brief create a map from model signal vector to python model inputs + * @param [in] signal_vec_names names of signals in model signal vector + */ + void mapInputs(std::vector signals_vec_names) override + { + // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is index in "signals_vec_names" + map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); + } + + /** + * @brief create a map from python outputs to model signal vector + * @param [in] signal_vec_names names of signals in model signal vector + */ + void mapOutputs(std::vector signals_vec_names) override + { + // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in "map_pyout_to_sig_vec" is index in "signals_vec_names" + map_pyout_to_sig_vec = createConnectionsMap(signals_vec_names, pymodel_state_names); + } + +private: + std::vector createConnectionsMap(std::vector connection_names_1, std::vector connection_names_2){ + std::vector connection_map; + // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is index in "connection_names_1" + for (auto name_2 : connection_names_2){ + int mapped_idx = -1; // -1 means that we cannot create a connection between some signals + for (size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++){ + if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0){ // 0 means strings are the same and we can create connection + mapped_idx = NAME_1_IDX; + break; + } + } + connection_map.push_back(mapped_idx); + } + return connection_map; + } + + std::vector fillVectorUsingMap(std::vector vector1, std::vector vector2, std::vector map, bool inverse){ + // index in "map" is index in "vector1" and value in "map" is index in "vector2" + // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 + for(size_t idx = 0; idx < map.size(); idx++) + { + if (map[idx] == -1) continue; + inverse ? vector1[idx] = vector2[map[idx]] : vector2[map[idx]] = vector1[idx]; + } + return inverse ? vector1 : vector2; + } + +}; + +#endif // LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/sim_pymodel_steer_vel.hpp b/simulator/learned_model/include/sim_pymodel_steer_vel.hpp deleted file mode 100644 index 5237d27acc833..0000000000000 --- a/simulator/learned_model/include/sim_pymodel_steer_vel.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef LEARNED_MODEL__SIM_PYMODEL_STEER_VEL_HPP_ -#define LEARNED_MODEL__SIM_PYMODEL_STEER_VEL_HPP_ - -#include -#include -namespace py = pybind11; - -class SimPymodelSteerVel -{ -public: - int test_variable = 123; - SimPymodelSteerVel() - { - py::scoped_interpreter guard{}; - py::print("Testing py::print LIB"); // use the Python API - } -}; - -#endif // LEARNED_MODEL__SIM_PYMODEL_STEER_VEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/src/model_connections_helpers.cpp b/simulator/learned_model/src/model_connections_helpers.cpp new file mode 100644 index 0000000000000..70349b80c9796 --- /dev/null +++ b/simulator/learned_model/src/model_connections_helpers.cpp @@ -0,0 +1 @@ +#include "../include/model_connections_helpers.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_base_error.cpp b/simulator/learned_model/src/pymodel_base_error.cpp new file mode 100644 index 0000000000000..f829bd20bbe64 --- /dev/null +++ b/simulator/learned_model/src/pymodel_base_error.cpp @@ -0,0 +1 @@ +#include "../include/pymodel_base_error.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_interconnected_model.cpp b/simulator/learned_model/src/pymodel_interconnected_model.cpp new file mode 100644 index 0000000000000..fac2a02ca5d68 --- /dev/null +++ b/simulator/learned_model/src/pymodel_interconnected_model.cpp @@ -0,0 +1 @@ +#include "../include/pymodel_interconnected_model.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_interface.cpp b/simulator/learned_model/src/pymodel_interface.cpp new file mode 100644 index 0000000000000..302ef331fd00a --- /dev/null +++ b/simulator/learned_model/src/pymodel_interface.cpp @@ -0,0 +1 @@ +#include "../include/pymodel_interface.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_simple_model.cpp b/simulator/learned_model/src/pymodel_simple_model.cpp new file mode 100644 index 0000000000000..f3a11c3a8aa7d --- /dev/null +++ b/simulator/learned_model/src/pymodel_simple_model.cpp @@ -0,0 +1 @@ +#include "../include/pymodel_simple_model.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/sim_pymodel_steer_vel.cpp b/simulator/learned_model/src/sim_pymodel_steer_vel.cpp deleted file mode 100644 index 98222cac22134..0000000000000 --- a/simulator/learned_model/src/sim_pymodel_steer_vel.cpp +++ /dev/null @@ -1 +0,0 @@ -#include "../include/sim_pymodel_steer_vel.hpp" \ No newline at end of file From f6cedfe6adb0d298f38770f5f11604ec1e1d6e55 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Thu, 18 Jan 2024 18:29:45 +0100 Subject: [PATCH 50/97] Update CMakeList --- simulator/learned_model/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/simulator/learned_model/CMakeLists.txt b/simulator/learned_model/CMakeLists.txt index be3e67d0cd114..ecee90775790a 100644 --- a/simulator/learned_model/CMakeLists.txt +++ b/simulator/learned_model/CMakeLists.txt @@ -8,11 +8,13 @@ find_package(Python3 COMPONENTS Interpreter Development) find_package(pybind11 CONFIG) ament_auto_add_library(${PROJECT_NAME} SHARED - src/sim_pymodel_steer_vel.cpp + src/pymodel_interconnected_model.cpp ) target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) target_include_directories(${PROJECT_NAME} PRIVATE ${Python3_INCLUDE_DIRS}) +target_compile_options(${PROJECT_NAME} PRIVATE -fvisibility=hidden) + install( DIRECTORY include/ DESTINATION include/${PROJECT_NAME} From e697eea25284db4f4f1855e99b3545bb034a8bec Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Thu, 18 Jan 2024 18:30:18 +0100 Subject: [PATCH 51/97] Add python model to simulation models --- .../vehicle_model/sim_model_pymodels.hpp | 47 ++--- .../vehicle_model/sim_model_pymodels.cpp | 161 ++++++------------ 2 files changed, 67 insertions(+), 141 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index 3267221ac88d2..affdad9bf24cf 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -16,6 +16,7 @@ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "pymodel_interconnected_model.hpp" #include #include @@ -60,25 +61,23 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ ~SimModelPymodels() = default; private: - const double MIN_TIME_CONSTANT; //!< @brief minimum time constant --> TODO what is this? + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant /* Specify string names for states and inputs. So we can automatically map states and - inputs of this model to states and inputs of the python submodels. - In order not to compare strings each iteration, we compute mappings between the states - and inputs of this model and python submodels. + inputs of this model to states and inputs of the python model. */ - // const char* STATE_NAMES[5] = {"POS_X", "POS_Y", "YAW", "VX", "STEER"}; - // const char* INPUT_NAMES[2] = {"VX_DES", "STEER_DES"}; - /* - Index means state or input in this model. First 5 indexes are states, last 2 indexes are inputs. - ---------------------------- { STATES }{INPUTS} */ - // int MAP_TO_PYSTEER_INPUT[7] = {-1, -1, -1, -1, -1, -1, -1}; // <<- Value means index of the input in python steer model. -1 means not used. - /* - Index means state in this model. - ------------------------------- { STATES }*/ - // int MAP_FROM_PYSTEER_OUTPUT[5] = {-1, -1, -1, -1, -1}; // <<- Value means index of the output in python steer model. -1 means not used. + std::vector state_names = {(char*)"POS_X", + (char*)"POS_Y", + (char*)"YAW", + (char*)"VX", + (char*)"STEER" + }; + + std::vector input_names = {(char*)"VX_DES", + (char*)"STEER_DES" + }; enum IDX { X = 0, @@ -92,6 +91,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ STEER_DES, }; + InterconnectedModel vehicle; // py::scoped_interpreter guard{}; @@ -112,26 +112,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ const double steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics const double steer_dead_band_; //!< @brief dead band for steering angle [rad] - - - //PyObject *steer_base_model; - - // int num_states_steering; - // int num_inputs_steering; - - // Create vectors to store system input and state (C++) - // Eigen::VectorXd state_steering; - // Eigen::VectorXd input_steering; - - // Create vectors to store system input and state (python) - // PyObject *py_state_steering, *py_input_steering; - - // std::vector state_steering; - // std::vector input_steering; - - // py::object steering_module_class; - /** * @brief set queue buffer for input command * @param [in] dt delta time diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index d7f4f3635850b..860f2863a3b85 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -15,13 +15,8 @@ #include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" #include -// #include -// #include -// #include -// #include -// namespace py = pybind11; -#include "sim_pymodel_steer_vel.hpp" +#include "pymodel_interconnected_model.hpp" SimModelPymodels::SimModelPymodels( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, @@ -41,77 +36,52 @@ SimModelPymodels::SimModelPymodels( steer_dead_band_(steer_dead_band) { - // Initialize python library - // dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. - /* - More about the line above here: - https://stackoverflow.com/questions/60719987/embedding-python-which-uses-numpy-in-c-doesnt-work-in-library-dynamically-loa - https://mail.python.org/pipermail/new-bugs-announce/2008-November/003322.html - https://stackoverflow.com/questions/67891197/ctypes-cpython-39-x86-64-linux-gnu-so-undefined-symbol-pyfloat-type-in-embedd - https://man7.org/linux/man-pages/man3/dlopen.3.html - */ - // start the interpreter and keep it alive + // TODO this should be in config file not hardcoded here + // Think of a way how to differentiate between "simple" model and "base + error" model + std::vector> model_desc = { + { + (char*)"control_analysis_pipeline.model.base_model.kinematic_bicycle_steer_vel", + (char*)nullptr, + (char*)"KinematicBicycleSteerVel" + }, + // { + // (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", + // (char*)"$HOME/f1tenth/ws_testing/base_model_save", + // (char*)"SimpleSteeringHyst" + // }, + { + (char*)"control_analysis_pipeline.model.base_model.base_model_simple_velocity", + (char*)nullptr, + (char*)"SimpleVelocity" + } + }; - // char *model_save_path = (char*)"/home/tomas/f1tenth/ws_testing/base_model_save"; - - // // Import python module - // py::module_ steering_model_module = py::module_::import("control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis"); - - // steering_module_class = steering_model_module.attr("SimpleSteeringHyst")(nullptr); - - - // // TODO call function to get model signal names - // // From the names create a mapping table for correctly pass arguments to the python models - // py::object sig_state_names = steering_module_class.attr("get_sig_state_names")(); - - // char *my_result; - // PyObject *temp_bytes = PyUnicode_AsEncodedString(PyList_GetItem(sig_state_names.ptr(), 0), "UTF-8", "strict"); - // my_result = PyBytes_AS_STRING(temp_bytes); - // my_result = strdup(my_result); - // Py_DECREF(temp_bytes); - - // // wcstombs(buffer, PyList_GetItem(py_sig_state_names, 0), sizeof(buffer)); - - // // int len_ = PyList_Size(py_sig_state_names); - // std::cout << "STATE NAMES: -" << my_result << "-" << std::endl; - // // std::cout << "STEER: " << len_ << std::endl; - // // std::cout << "STEER: " << strcmp(my_result, "STEER") << std::endl; - // // std::cout << "STEER: " << strcmp(my_result, STATE_NAMES[4]) << std::endl; - // // std::cout << "STEERR: " << strcmp(my_result, "STEERR") << std::endl; - - - // // Load model params - // py::object load_params_succ = steering_module_class.attr("load_params")(model_save_path); - - - // // Reset the python model after loading the parameter file - // steering_module_class.attr("reset")(); + vehicle.addSubmodel(model_desc[0]); + vehicle.addSubmodel(model_desc[1]); - // // Set number model states and inputs - // num_states_steering = 1; - // num_inputs_steering = 1; - // // Initialize state and input vectors - // for (int i = 0; i < num_inputs_steering; i++){ - // input_steering.push_back(0.0); - // } - // for (int i = 0; i < num_states_steering; i++){ - // state_steering.push_back(0.0); - // } + std::vector> error_model_desc = { + { + (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", + (char*)"$HOME/f1tenth/ws_testing/base_model_save", + (char*)"SimpleSteeringHyst" + }, + { + (char*)"control_analysis_pipeline.model.error_model.error_debug_model", + (char*)nullptr, + (char*)"ErrorDebugModel" + }, + }; + + // vehicle.AddSubmodel(model_desc[1]); + vehicle.addSubmodelBaseError(error_model_desc[0], error_model_desc[1]); - // std::vector state_steering(num_states_steering, 0); - // std::vector input_steering(num_inputs_steering, 0); - - // std::cout << action_delayed[0]<< std::endl; - - SimPymodelSteerVel test_; - std::cout << "-----> " << test_.test_variable << std::endl; - // py::print("Testing py::print"); // use the Python API + vehicle.generateConnections(input_names, state_names); std::cout << dt << std::endl; std::cout << "OKAAAAAYYYYYYY" << std::endl; @@ -151,45 +121,19 @@ double SimModelPymodels::getSteer() } void SimModelPymodels::update(const double & dt) { - Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); - //std::cout << dt << std::endl; - - // Model the delay of speed - vx_input_queue_.push_back(input_(IDX_U::VX_DES)); - delayed_input(IDX_U::VX_DES) = vx_input_queue_.front(); - vx_input_queue_.pop_front(); - - // Simulate the system (car kinematics and dynamics) - // do not use deadzone_delta_steer (Steer IF does not exist in this model) - updateRungeKutta(dt, delayed_input); + // Eigen::VectorXd to std::vector for model input + std::vector vehicle_input_(input_.data(), input_.data() + input_.size()); - // Simulate the steering system using python model + // Eigen::VectorXd to std::vector for model state + std::vector new_state(state_.data(), state_.data() + state_.size()); + // set model state + vehicle.initState(new_state); - // TODO: define inputs to the model in a more general way - /* - Following inputs to the steering system can be used: + // model forward + std::vector vehicle_state_ = vehicle.updatePymodel(vehicle_input_); - - IDX_U::STEER_DES - - IDX::VX - */ - // input_steering[0] = input_(IDX_U::STEER_DES); - - - // // Call forward() of the python model - // py::tuple res = steering_module_class.attr("forward")(state_steering, input_steering); - - // std::vector next_state = res[0].cast>>()[0]; - // state_steering[0] = next_state[0]; - // std::vector action_delayed = res[1].cast>>()[0]; - // std::cout << next_state[0]<< std::endl; - - // TODO: define outputs of the model in a more general way - /* - Following outputs from the steering system can be used: - - IDX::STEER - */ - // state_(IDX::STEER) = next_state[0]; - state_(IDX::STEER) = 0; + // std::vector to Eigen::VectorXd + for (size_t i = 0; i < vehicle_state_.size(); i++) state_[i] = vehicle_state_[i]; // Calculate current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; @@ -199,6 +143,7 @@ void SimModelPymodels::update(const double & dt) Eigen::VectorXd SimModelPymodels::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { + // Not used for this model -> we can get rid of this later auto sat = [](double val, double u, double l) { return std::max(std::min(val, u), l); }; const double vx = sat(state(IDX::VX), vx_lim_, -vx_lim_); @@ -210,10 +155,10 @@ Eigen::VectorXd SimModelPymodels::calcModel( Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); - d_state(IDX::X) = vx * cos(yaw); - d_state(IDX::Y) = vx * sin(yaw); - d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; - d_state(IDX::VX) = vx_rate; + d_state(IDX::X) = 0 * vx * cos(yaw); + d_state(IDX::Y) = 0 * vx * sin(yaw); + d_state(IDX::YAW) = 0 * vx * std::tan(steer) / wheelbase_; + d_state(IDX::VX) = 0 * vx_rate; d_state(IDX::STEER) = 0.0; //Use python models to update steer return d_state; From 204f0da70e6caedf5b388649f4fc6b91315aad0b Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Thu, 18 Jan 2024 18:51:14 +0100 Subject: [PATCH 52/97] Fix path to the python model parameters --- .../vehicle_model/sim_model_pymodels.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 860f2863a3b85..2fa8eb5766102 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -65,7 +65,7 @@ SimModelPymodels::SimModelPymodels( std::vector> error_model_desc = { { (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", - (char*)"$HOME/f1tenth/ws_testing/base_model_save", + (char*)"$HOME/autoware_model_params/base_model_save", (char*)"SimpleSteeringHyst" }, { From 1d26ec3ebc726438d4f0a3e7298373edb4815aa9 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 23 Jan 2024 11:29:16 +0100 Subject: [PATCH 53/97] Fix minor formatting --- .../include/pymodel_interconnected_model.hpp | 32 +++++++------------ 1 file changed, 12 insertions(+), 20 deletions(-) diff --git a/simulator/learned_model/include/pymodel_interconnected_model.hpp b/simulator/learned_model/include/pymodel_interconnected_model.hpp index 0dfbc23df70e7..ac24181787802 100644 --- a/simulator/learned_model/include/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/pymodel_interconnected_model.hpp @@ -9,11 +9,10 @@ #include #include #include - #include namespace py = pybind11; -// sim_pymodel_steer_vel + class InterconnectedModel { int num_signals; @@ -23,12 +22,11 @@ class InterconnectedModel std::vector> submodels; - std::vector - map_in_to_sig_vec; // index in "map_in_to_pyin" is index in "py_inputs" and value in - // "map_in_to_pyin" is index in "all_variables_names" - std::vector - map_sig_vec_to_out; // index in "map_pyout_to_out" is index in "pymodel_outputs" and value in - // "map_pyout_to_out" is index in "all_variables_names" + // index in "map_in_to_sig_vec" is index in "py_inputs" and value in "map_in_to_sig_vec" is index in "all_variables_names" + std::vector map_in_to_sig_vec; + + // index in "map_sig_vec_to_out" is index in "pymodel_outputs" and value in "map_sig_vec_to_out" is index in "all_variables_names" + std::vector map_sig_vec_to_out; public: py::scoped_interpreter guard{}; // start the interpreter and keep it alive @@ -39,9 +37,8 @@ class InterconnectedModel InterconnectedModel() { // Initialize python library - dlopen( - "libpython3.10.so", - RTLD_GLOBAL | RTLD_NOW); // Manually load libpython3.10.so as we need it for python.h. + // Manually load libpython3.10.so as we need it for python.h. + dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); /* More about the line above here: https://stackoverflow.com/questions/60719987/embedding-python-which-uses-numpy-in-c-doesnt-work-in-library-dynamically-loa @@ -127,8 +124,8 @@ class InterconnectedModel void addSubmodel(std::tuple submodel_desc) { const auto [lib_path, param_path, class_name] = submodel_desc; - submodels.push_back(std::unique_ptr( - new PymodelSimpleModel(lib_path, param_path, class_name))); + auto new_model = new PymodelSimpleModel(lib_path, param_path, class_name); + submodels.push_back(std::unique_ptr(new_model)); } /** @@ -139,9 +136,6 @@ class InterconnectedModel void addSubmodelBaseError( std::tuple base_desc, std::tuple error_desc) { - // SimPymodelBaseError pymodel(base_desc, error_desc); - // submodels.push_back(pymodel); - std::cout << "HAHA111" << std::endl; submodels.push_back( std::unique_ptr(new SimPymodelBaseError(base_desc, error_desc))); } @@ -155,16 +149,14 @@ class InterconnectedModel bool state_changed_externally = false; for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { - if ( - abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > - 1e-6) { + if (abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > 1e-6) { state_changed_externally = true; break; } } if (state_changed_externally) { - std::cout << "Reseting model... ----***----" << std::endl; + std::cout << "Reseting model" << std::endl; std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); From 7fbfe64f1d6f9c09796c25d5b1e2fd621e2473c1 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 23 Jan 2024 15:40:08 +0100 Subject: [PATCH 54/97] Add README (WIP) --- simulator/learned_model/README.md | 84 +++++++++++++++++- .../image/python_model_interface.png | Bin 0 -> 88467 bytes 2 files changed, 83 insertions(+), 1 deletion(-) create mode 100644 simulator/learned_model/image/python_model_interface.png diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md index d490d175585e6..16c2336a5f05c 100644 --- a/simulator/learned_model/README.md +++ b/simulator/learned_model/README.md @@ -1 +1,83 @@ -# learned_model +# Learned Model + +This is the design document for the Python learned model used in the `simple_planning_simulator` package. + +## Purpose / Use cases + + + + +This library creates an interface between models in Python and PSIM (C++). It is used to quickly deploy learned Python models in PSIM without a need for complex C++ implementation. + +## Design + + + + +Using this Python model interface a PSIM model consisting of sub-models implemented in Python can be created. Each sub-model has string names for all of its inputs/outputs which are used to automatically create model interconnections (see image below). + +![pymodel_interface](./image/python_model_interface.png "PyModel interface") + +## Assumptions / Known limits + + + +To use this package `python3` and `pybind11` need to be installed. The only assumption on Python sub-models is their interface (see below). + +```python +class CustomPythonSubmodel: + + def forward(self, action, state): # Required + """ + Calculate forward pass through the model. + Currently implemented in the way that the next_state + needs to be 2d array with size (1 x NUM_OF_STATES). + """ + return list(list()) # next state + + def get_sig_state_names(self): # Required + """ + Return list of string names of the model states (outputs). + """ + return list() + + def get_sig_action_names(self): # Required + """ + Return list of string names of the model actions (inputs). + """ + return list() + + def reset(self): # Required + """ + Reset model. This function is called after load_params(). + """ + pass + + def load_params(self, path): # Optional + """ + Load parameters of the model. + Inputs: + - path: Path to a parameter file to load by the model. + """ + pass +``` + +## API + + + + +### Parameter description + + + +## References / External links + + + +## Related issues + + diff --git a/simulator/learned_model/image/python_model_interface.png b/simulator/learned_model/image/python_model_interface.png new file mode 100644 index 0000000000000000000000000000000000000000..28b217801d57b9c3a020d7bf5444b4e42e641c22 GIT binary patch literal 88467 zcmeEu2Rzp8{y(~jq>Q47ZaZb~kr^Qwk(Is4-YaEiq_jwgic(}Gd)F<>NU}#(C@T^v zqyOh?l*Z|K&N^girI3k97gbWKy-P1)z%f;5i+{(rbi;?HR%5RL^oR;>^E{r_VjNIHt4i4<5 zRz?<1Mz+rEc4jVc2tK#9H?=aiGBaH{hntg|myMH)jhp8v7Y`$kB%Hz{$;ZRa&&zde z<$NOxGrQFrD%yKl+1MB{av$Vl=Y(7B(cH)_*1o;j4 zc~_1(xxsC@xVhLlS>O{{GZSljxC=KY7bpC1!J&gzPG;uzaM{7-vsNB$=3->AGE7NM zf#Yn}(ua*)?M!SXxp;VvxE@_O$IZ;i*~;E-^=-J>x!Aeke#ncucsiJ^9Ja8xx3Doo zj`GMba!SHy$Pdqf)yaWB?2K$z&sBlA}+}>vO^;XYu zfH(iVhi{;8uLB7@DA!o~(Sy@=FJsH+^Wt6r?YbUQBaJDou zwRc~+e)-V)Wm%cXwdr)Sw}<_v<9fn%Uc$xj1>kZPzYeTjEulX5->SPDdW>{?$adI0aS@TCTGXUjEgC zMyqyd@#Xf&^}lEB;GET^|Me>%(eXPA`T4S}y69hhS$H^CUu11rgam(F6@HA?KW>`YgGl{T?4a(1>d`PO5;b+3QuEM}(P1jS$5 z*-u?%og=J;%JpurcFf7l#>mCW?b}%UvsL&#L-;xn6?-el-d`M+o9{cz5ai@z7g)_h zYfp2wcXcu`TfOr4`Qpk0|KVeQFvy=h*2T!l!p!Agcr4F>KW1=Z5f?HQd$L{3f*W+W_8@ zGPQDJiMwcM{rncjPH?m35&xr`BB5*@zyc72jA|WJ9@N3Dlere@|w zuFDf-WMgH46dfi263qa0Ng=4p3c#}D>Ib$~rlts#`zdn!oyq5qL~1jP?60*V)C$1q!Es!*W%W)eN-$EvrTDp#5*%L0MH^M(L>J zQF&#hAMf+G@qfi0UvR(|)bo>w{+G7?HR9mc=C9(p7P{q;7-f#sj^G0TmY_D2`iBAh z-;a{31?XQbMDnc_L|>|=wX*fM;v?^B%v{4H>%kCWm%q(IECbj-5R!5W{(wo5to`l! z|Gm;{D-9gv>Jt9eQuF-jmYN5a5n0J~!9dN$$;!?Gjv=%rlK9Ml7~YgZf?eJ zf|TWdwMfCm{T-MRSVK+g&1cP2)*~qa&M%IXVSEv6H73z++B! zUMQvzHU`Nf4RzW96UUxboe2UOPC%^D%J-fY&6{dY(r zHo>*Tw-z4-IKgiJn>9F_;F?E$83NxghwzuJ!FhfxW&T0+OylyMesJ^Os)^V8*FW;ezgjdGT!U|4Ol{5f)_En@UniRX2>q7v zznu}HkgetMRgn^Nl{MxR90b?WQWl*qeH>VI_wAKvi*bu9}#U@tUH!3Y~w*$p1pVT-6_WzIR!)f8$x- z-*nb!)uSR&|4ID5%ImMM`~E9b|9>f|^RBwVs*Q39v9I?LAzq%9 z7_u$_t%3ISDSr(btmT0Jdo6(LXGs3HjZ#_t=pSvBUybx1+4bKRxxT%U`@69DS2W3= zIxp8M!}_cI{^u($1=iB&x)AWw%98*4(6JIZ{zM_+XY>Cd$@3 zeE$La`@ZPo=4AgOCL+J^t!Bk_MaZAdAb(!;{SM&ye_r(Quc?BoIg(9~7Zk-GTVK}A z{!dH5wcD(r!2f$1Xf2R^Q*-|i4>ZgytxQ0${a>j1*6i)?st8(Ir#gk zzF!*7f538otonXAg1@fmu1ZATnC=fL_6NxQhgN;dn=F4DbpLErs{-QBF#CUFf7$I$;vc9c{=FX)HX-~^`3v`VUs;JyY;`tfv@^8VHb(?AU`Qh7ozS&H}FSvH#A8e*s->vcc_RTCm@B8gHE12wu)?N6P zt>tg+_{E+WuGIa|!&G+x^0czH@}1en;1DdKLFFzW5hd z?sWlmtygw^`_kI6|8W28S0`fUTs7J?o95@|Wfu_oYSiD^^nbvs)*N?j1OgyS{bUQr zU&*U}x~-At57$%IrKUgOKF{wQ?XT`Eg5Q5XfdJPpw?zK&o+2ppZB@f2Ao!hk{)sZq z^7nlGgRhTV`^K=Z-_*I@IF>(M`)<&c4^}ULECOHYgM1rhiq_^gSXgK*d1=X`9{OYP z1Pj|=eE!@Mw(SC=>htIuA@~M5ff;;ZW3-*oa=ByoFR^*I+@HxlC9i#dTl;+u0|wL3 zYJIu;44ayJrswL1PZf3BPQ=HEI1jthY(3W$FXUW5T^lDf%=gxegeUfM>#4nTXq=z? z5G&w>qxBcmjoStJ4xywae)vImqsX;QNiKSvpYWC_p}-~~{_e+5i!9QEWVG+dz)!B0 zKr!gQruqJoUGM8bm!A|?K9CEf}ZITBC$5t)lNE72}1QJ94ZohU49@xsG?_(8+mE6x@#3!S#?_&*gd8 z$nz&(%(#yPERRN~nRV5&Lm7QNrrJ!-wWl1psk@s=`P!It$aYQL=fPWd+l6epY93kN^f446!BR$l(QGSBDH>5TwuMs^~9$)x9^!$oi)oos1+k%e{D1&3_rwo z`A1?ufn0IOaDIyV>Tp%maoYQFBMJu2ii|xZ5A+|7kR1{Nr#!#%Swor@eg*TV5Pn7KT))G91QUK`eMI&(2m^bmr2` zNOO(Yr`Pm4Hd&P;BNF?q+A^<`Nj_!MakTc2$an=>?VJkO?|Rxot6{+)47vqtvK zb7CE#+wDs~-pa9h@%SaoY7mByl`Z7@RG0I!g3%9S?Jq)i>fY44K90$F-CguS>h#b& zkvn;p9`Ub-RXxamA>+uYg{jk`bG-p>3sY4hd8DW_y8V^pIyQZn(-zk}9~Zign%|Cd z3L|583Z=jlqHp0E6AZq8p`2%MYSERphOkR6vkU$DmgZ9uj`aNgP4vEdrg(oj7ke1= zd!_DA6pnU$=s8s;5xj-lFA_Hp>q-G8oJ@KD^U4g5{!nI{PTRbkXJYfUcw1@W)QWeA zdOdiKr>nMSfm-vj{6(f_?SRR*_)Ml^1>?D$aWz%%-@F(tYYE^f2QuCs}EZE;Me#z&Dc zD?JdMZF`i=-ffZK;}k}bZ;CcNLnTn(k!Szb-;wkYUGyMsyJEr>2C-|%DF5rXf_FPGk`4VYPk)U7VveTL}5M&z` z_@Sk`2)BQk3J+3&g>@cS_8IF1sy{xr+n&lMQ&Our_3I^AFe-ek8+U)2P4ST_nrypk zzV?LmS;Fss^U@(ort*NBbl<=lc?Ql?OA8GAI<`tefG=3eu$va&;ZOEjHgcScuV0NX z%X5=LgMH5=wMt(z{R~Xq)44Zemq;6u5*qz9^JKojAaozlR({-l^AUs$!fj4j~+dBgZregNHRR|wo7lb?bnr+ z6NI6YiTSeFm14rTZ7nU-7sFQkG?@J&=Ii6oIL-8lt~N7mqfakGnmn>oJU(6(9;hZf zIg-1Uqvwyz6GzzjowxRJB6F52UKe}!gi4RfM=*$FpcwXctpPV|Dc zxkuxzSHO)!%CUFgcDC9fc;KOp`rYJTtluw?tU;K+Z^M=TA%$YOsovDlL9&J<12K_! zmamVaQxS!=(KuI+~qfhBe9B=%v)X-54dkICkl zcd*JF3GwxiNbwWOb6WMSt7yE_nllV~^wN&pJlIm;Y(XXLl9FT7&TYQ|;y1fUukQw% zwoIeko|E%z4qYAA8Mb9hpHh7QMwAL?E`1v9jCX%&6_=1PRysf7R!w_a>%&Ahh2X2} zr)lmx*9%p>JTYD8-&#CBQDpnF*s~?uLfOD}AWqM%y>ezW#^TzPYx)-C)%}u4;id1; z-*!_dZV*M|m=VJ(;F zOxTJ{<75DO9j-0GI7GRS3zT6)>_1xRi`TE%oYYxDR!#0=@lKOZ4pWgyF@tGgb%N>l; z>6mFXSjJJ8jXkGcdcWQI@TvIH)K>42JGXUrzkU`cc@+U0 zJ1=YMTh%r{=<5&XaQpns)$OIH#fMIq>o=b`&7N-SJUKr;-gkO;u626Bz!=K4Nf?U^aYaX?q>OzzWoz=blLb+m|&V}P`6*)Rf4$D zd|HA?jt=HYeivVkHmdOeTBmq!VfxuU%?$mPd`A=JW4qkD9CI5F?Q7p@5oh$`mSQ$v zi`FB8O#OB8a5^Xap@LDnq#F-t?`k8kuE`El05vee~9q)FZ{;c4` z*_Yn-%zL|BUMS{{rWbX14#!&zC!8uR0jrwuO3f%O%*Jbn#IEbJ`{dkpGNMDc*qCBZ3Hj=4NBpHG6ER2wcQ3H1cY0opWl@2)HLuJ z>^LVEP<{UGu{Q;sO=^fOL+s(VqQ5okh18Pz#D3iP;B_0WDv@B5gq*?Z$E8}pBcjZ= zHfG6!dPC&7(PecF$JmpBK@h9Mjzto=iwK$p$b;LL@CX1`g6kPg(|mB zTY(Sm{nGDWKYygHs{&R-qq!9aFA5)ERNB0I8Xd8ezR!&MhuyBuY@5ip-aD3afA^_{ z=LL4#!ae;4ap^YsW0TY7pJ>2Kljg1@9FYo?xN@C%>i|?0d5!~*g{F*C@8vo@L@}xr zmq!HV_8#LgdAymzdc!D5{pZ%Qr9sTrPl}%4`L@mKnyyu^iZo0E?h%F};YJyv(I?cc zMTci=`q}jeXJjV`ZP0>ZJlNu;>E{ z;=`BPY%7iSBk9MD+QF#}52dj5zHXiTHeKx;q_hiCkPed0T9_!ORpd|LubRK4{+q{R2!(m)-vV22)B`zJV9olAshmB*l+r6i7 zjK*fn@t#C+`MZZTU4D!Q!1~9>^LpGSs0XUfx$jio(e!3(hF&Gi!x2ie z{KU&zM~L11VP+=nqxIZk6cfa{3GncVuX*^H;nG!JR2H*>Ld`kWt4oY{a|oj&p!EVR zG73GH^D}RIdbDL(OWrrKn-IKFcKnPce+&@(l`iL@2+t#O#dCwv23<3map=h5D_kaN zQ2PX5@vEuz#on+fj6y5Okk&s4?~a<4I6DLWcFYHWskT+!L02f=$Fs73Out-Oq$CO{ zL2eTgkhxWlTku1ID`b{V+>C$^-&<<%j&#Ud`0+`Fle29m+0aL;Q+!3#JGZm=qI$p; z(wKf+E?#2BkZ?)K2Zm&2uWy8R0Or;!CSs+5GKQF`g-S~djTPrLa%UF8s|f9e@GY!6 z_HO40$&#c(sz-`g|6D_YcToQq@b`X_S~w>aMIpZUUM-$#lN}wZKS``!rdn|Dl3E<+ z!-Z#`y|G1q@7&WxhGRLhhKx_PTO$~9tc6-)N|ZNn_s0*B zxIjRhMAqk@cnMRi0zn7KZ(CtHU+}c#w5WwrS4Q&lqXu6}Uum=`f-s+HwH{Mu)NkqtxhsAivh{8Pb3JL4bRr)9 zE}%&cQZ_OvUfr?taJ8z`)8}{8ZXYXh8y_p4uFp=_D~jq?8@Ya#$}a!!Ku-NoC_iErnhO6Zef>qIM$dGH~TJ^61zrg9*{7{9t9Icpl=LDL6mF1Pb;F&#n zo8p;~qY39gu;iZp@5gEiWGrvV8kL zJ((!NgqxmAIA*{I6O@Y5kKPVzyBV96tzP`|2?R#y#(K|=y}_ITE^PbwBgh`MTBgKg z^hK}cBMR+{#`!xTe}%VXY(Ul%U58A|^lRp#x1<~wL{A>~xr{6~sfV5he8`0gag{tA z4t~&`^YI%tfa&lRLwJ^jUJoz=Zr!e$qc3is8wUs!YLpa=lo=9y2xcZ4DKsaB6o`^M z;+k|8=A?>-izh0pWhq@ESbY)@;IO8<2+Z@E|ZH)icXRuB~D=QoxpclLP?%G z4i+LKJEWmo*2UI$ z@<=(3Tk$qWSHt_9+nmD=Wq70t&P8VBTmW&;#g^s<3TY-kBu5>43Ypln?kb-%iyA@y z$6B6eZI7w!b=`G^q9{VBcb}N_BNJU^I^F>+*Kn6rg)&%fr20;`I^gZHyK}CN;HH7Z zmOZ5<=h<_>KGG{uV;CRPrj#QGC6tYi#lb|i|@k+G_miq5MA^N;+VQKL zA`)${BK2n-k=1cwhABF}1NYA2$-_yJyz0@&>#zPu90jL|2z!uD#tVr_l1$y_4jL-2 z?T^xwzE;>0bx8d=6wJ#+jyew|SjXBhgu0$N4I(+v7a<|)n3(NGU=y>M$}JoOi(oh)e?Seq2N zQDI5MNO_MmM{fd)QRAu|Nm`rEzs+jJvi>$J#JK()v#JBp_xea=D`DTs4*lvKdC@+p11f!oDm$T}N(8c}#s5 zH^ea74H`2!5b1!L>nP4{-bbPp={qFcO@agt6>oT})|XdqCCid>G;%kNSR^jr6#sp& zsu>-O|LQ0*{wUJ99KuE;=L#J_TO1PS8k*v)h*n2H=MiB(3pkXHWI*m z=S42188aaKXMvnG4^4RvKnl#!G#>!|{-DHT^#GXTN+auD#=v#0q#>KEH%>xKA zVC-JHryOI(sUU99rPwUnyv(XP(j^v%D&4%EI|sGT!>w%8`*1$f11jzhqQN7P+kjo? zYPG{`<_P3ETX?VAwixT%WdIc{Seq~6udG1d_$`G{iDp=VR7x*BEhJh?(Zfx7AKVo* zh1HV6OB=Di=*#2-bL?r6s!5lLhPxKhBPlR(7T?k?ql8OQ;T*(Qv!v!{5-T@-O_cmp z8phAtS_$K4UAA;z-=Wz}y>FWC0vUfqeWB1pb@|-$P2QU0l4x&*hq5#l0o7)}j&Q=Xzdg;WLo zGR1FIa+Fa1w21}mJI>yfLEfuh8YVXG-~rnysM>>e9~0IcbOfvMuv0Ob)bB1ygRUmu`@&^24?GAD zu(VWntcH~f+^p5y$^;Gs^F3X!ks)m;2~Tj5MbfIoIM}1Ux5?Eg8pv@qa;XV^D>rSR zI&?V{-o-LK9C?@JH^j!S8%Kyab%0Gb0Dw&acbK$0!egr8z}9x5;W!gSL_^fqErPvR zR04K!z`A+4=G7B0#~C8nn&;X67waYCi4VXmY7f`P+rDnN-g4he0nsJLKsL@jwJ@;T zb+`v8O#Tyd4x>F~Wwu4*xf4CZfYPUjpJk*D!y|L3?2DhXm(Cd%y}1=@^Z3lp+?He3 z+HYT$j3)+c*|f-ia<1?6IB0k|%3>ce8v34Qqa8V^dLDg28X5YcK<<1PJ1a7gslZ;q zkm*)#`|16g+*cPA@H8wUx8DP%)eS`D4nWHBc^weZy0-|uA|8EN;;kKB3QX7-B5gul z)Y>hgSLl*8{Y<><>=R(}-cD2nY!%B-UwU$px$~-Zx)mb-6}9f^l64-g&jHc6X|V)m zVzy%A?zT<)(hS8HCIh&lRd%$#P!{h#E7*@;)ay^!a@Kuu-B{kT#UcvLoq93}5Bbmptwulz#wo;2a{gsJNc+n4^D z-zNwY4$RU038nnS+0O1E!Ra1iA_ zQ6Pt-Y@sn!7Jp}m1ra50rg48z>)!bG?lK3)W&dbthA1&_1N+Zowl|ZFP z7fWzzna7xo-LGL!Eiysqz*v6UtJKc5TxMM|$?^WU zU8+pZUai2>r9Sf=c|9jbQTY75h+L0!H4um*Rq4G4jfdP2VziFi3@TneOw7%*!cSkG zoVU$uUxMj9lQE|ijlQ&hrb*3CZF9}3#ZiNaQL!i@j?%dswpo?r2%9(Bnqd$>6V}>N z^5JEt_G9#(EO}YWk0YrWag24)#Be;X+buSgVqYNev26%L<4sIF+jHuaCeg#qEHz-rO6lLw{=StY$q+?$)*l}5#7=O0QS-FZ`Zt7uCa23V#dy#`Ov{S}U zFR=s%JysT9G>xDVa8swZM3~ZU!ZSe$Hwx;P(*cB#CuoT0I|q_9*jFXuWu9*&gyMve z*xTaiuE!+qp+Hq`e^A_;>JD{O7n9wq>hs$}DWK&d0cpdqBda^;04nkwo@R~#^1UCo z@qe;T%L*PZdRwZ*ib%{|(Wf#@kLjQIy!b)MLt;;_AO^)M?I0pFaE52|^9|uR{Q3A) zCnBnnv9b(X_1s<4aWnhHm5UP-LYPlH*)8ahC_059n}wDOL2|{byjkE^O`%K=i2NQo zmps|Vk*VteTE;l=Lhc%S^YhVIy>5Q7xKg+pIG2D^sbErydp&a99-qx-#If~y#2N7k zDlN%L9hkv=`i9y8d-%+2uLL~~>6s|`pzr8-W5@4sPuyr!mJBQa zDIlJTPzo#7gB6{rTfL3Lu3(USm?ofHLNdv+`R?WH3+0FWdW2m^UY&e&1Lr(WG!B2E zmVlw71Wbp+3(0NQ&xP!^$#k^sy#h=BU_wERi>fzHp}4mv^U_DK{Chw_ri-P^dy_~J zSlB zW5%xuN5q{1$KLb<2_c>Sf*PztGSC>&u>j3Ng61I_nDBOwfr#!!|DZEHU}+p&)KHG2 zai*BT(w0n;$cLbml5!8mzl2nsu9bxR7N}a&=^D++THp8>E{^-Bb5*D4vP+{4PPQuv z4Q|UTsNa&N^_Wfl9Sc657ENYMRVA%jF`9it3z-&!DLE?*F;1q_q;b%shz4;}P$Z{S zBLju>1)y?!nJUcV{kLzlqsk9+U^NcZY(pEkJeLuwB&?Twk9J_}52E;tofUAh!5l-# zKc(L*pT?6Ze7z16Q*FB0IqhI(@RMc(43%`S69hPxer3bONIeJ1hwpH(+3=?xhI^j( zi9b{sUi-;mcg1nseR0rIRZXCOGHgg#{DDi_asUf}0A_mT;;{lwB!KN(4Pg89IZbW! zNw#u^TbN9kQL!-S6X#!y*pI!t#J_XHtxqQ^J{(H-jYl2C#QW|b)29{N&`uk2gjk~@ z{)W%|YES-8xbx=b7 zzFTI+>>>6ke~wnvX@*S3SXqBr8lU&i*)I~r)@5#rV$8qYz|?;fEk~Soac(zevHYF? zgDV-u@4}o}J`Rh2e#2jq?`^1F@%CELBVZRt{P{t9Xikn~T&mR2u^rR_|5fHxl%`K( zBR&~o;HRTd(2vU^n|0>8PY46V+??#Ey_7Z$1Y_->nYoKLb~NJgPk;QRlkQO==)^%1 zfWw6!PU=AtyF}eXVWd55__6dZ;y@-$)FBLEAj7Ai%v>OB^G6+@(M6HS zHDxRHQ9_H^Ljv1!ubURnt|^&5B*iU7`CJ~9(NexU0^H{ETnY0FnIuOY0@@3WvmLPi z?yXZesT=r@Cx=m(Vhjhub+EJ*@KH}RT{yH%p{HvHg}%d~H1g`+3cQ#)UMdVHp}}*4 zs8C!CD54X_6b(`QwjKGb)HazAd-Yl~5E<{z?Q6%O*(s+JYk=Dq7kg@Zb=X4+A8DMk zv|{*`8H2bFv3Dwk?2|leN*hmtdc=rKS>lHYozOpf} zmKDWh+mv!X`;%cUi8HrCu1AG^2y4~uQ^jP%4gwpE9o*gw}Dj=pjhX~6NDL*UeB zE>%KjvdF=piOrx@N!(NZH21{)O=G1kO%{8#`tf+lop~~Mm1s-x9Ctg&SNNbPHt~Pb z_o_cyk!J{3c2gWW*kivx3`DE3m%y>nLABC&B%(Co9Png zkzRg9CfDIJ9G%}Y&zm+JSDup3*HUY^nuw{+?2Tf0QzUqZC# zH)$?O_)R1sFOrn8DnK0;yY!qkkzSIv#6P5B=yY!=hHJ8jWK;6_yGNn;bO^0wev0jXOa zl9_U<{G`>yJocd!Mk<=|bi}*imdx=ks(Xm&JZx@xR4hpO9>KXjJJBt2$@;D;Qt=V{ zS^B4kXX6J-!f&(2R7)#kM zA-C_)F;!_|Rj&)_=eI@2+T0H%+R2SsEQ;1AGeSuw0ntA}d78#6rG`7JUU)R$<$O%g zX3C^XuNI`rl&|e?2go{8IDO~T$H8d#o!sfApGFF;2M6Ss&Vme4xJ3Hdq-3VWnM*=$ zP`&_K5yqhzRWF6cpX7oDpFx?`h8W?t;`1|s+oBP^U+YnUO36kVkHi6G^Q~n;W9&(X z#zEN_kc)~)>`)Q~QAEqKcrlkNWxzRt1e>`aTHd5)5hE&jtRsTIquT5}I+;D_L2g&g zwS7;aoG?eaCw3a&?^er?F1W1;?Hc}N7i|L%bHALHt;KJAc|rY_=k!q2XnSg)jFvOC z4_c?*zHD)5h(bD?r(W=pS@V$<79Bqtv9SDm8sl`6AC^qcmq{$-EL|wewZkXbb6J9t z$%j8#OTVG@@biZzM?vy=BN(5-+4js76 zwY?!DHtI-I)FwAQ-Z!Y@K8?&FGzQKjock5R$GG-kddIS#xoJ>g2GG_7pP$_d-h<1%sb9-VhUb6(`GJ z`s$xz)m()7Lgao%y~tQ*fb*iHmL!A3VM-yVq(x}0$bF$CQqVIe@|o7~NSi}~TZfg> znL?<3ZII@k&kMuiN>~**CeXDToU=uDBUF2=%$a+QrVnpwhE|n=tb>!14*fk@yovf9 zeP;V&Kmn$rV0FoKx#K38^pxf%(rPjjduFG;WN)W=tnCP^)7*zW5RaX~6N+jjLQb|n zqj{*s$atn;ivklS8ahGg?|YY$;t6Xa@#T1g#13cgdt9ogzST;Cx(|xY^={AsglaR= zbf=zoSA{6UMp3} zDSt1*87hS>BM7qffto_;6+scwnw&!bE&#GNLHZW_!}y0DY`Bi&PSqj9gs*F&amrSOM@qKU#9?McHKsf4UxNvv|pjfBC>*E}AE z>RmzvuTXP*%Vze1$FHxLoOLuN%0d%c_eP~k)9OcV$EQg4nQgC%IFfFWK^lB@bno@s z&da3JH6Wd;oOPS-_5*pd4KyQZLs}s37dLtvlT}lEgjU?PO0PBq^|rd{N)=yFF0S7G z0!Pi0zx$)HzDJoi7Y8N3&1?36m`~8*(Z^p~nZ_yQC^3aK zFB&hNeMj=B(Qj_EzAhdB1*Q&#oluR;(J0!FA)^7e-z7j3B5t&; z&ecdoJszokRP>)}Xk$SkGZMZ8@4stQ6zzh;!q9vEid9 ztv>y>hCwPRuShvU{QUSNS)wSC--|qz4b`H^-%)-Q)_Pyn5n1ZNrFtgHXkwf_bnHkI zMkvX~T@JpAae~jOFS6-RHt(jO52Gk2%ffdLu01T6pY$Hu=PM7;ax!b+Y{Q^c-+J|f zyXYE0gPv`CA}QnUvQ{blM>#{l%(xd5a~6{Z4dQ-4v(CSNqb3l1a$e=NN1f}M&_=u! zDdxDg91l7L8GnK2#ZEdE&KC!dP0o2vWgTG>?4dum|2`H?7Y>j@A%%%CM?xs{H)PJ0 zoB2h!U%#MGT)yb1Xp(h>B6(KisW+bH&NLYZ??kkI02}ZJ=-#0kZ}+vRZIB`4sH*;fly}!n`t#XjOB7Y{8EBk@!FaDHIT-B9ca#pZ6Z)bGn{farp(`c@)JZ!Kga^&f3bc^2J-)+2%@t*YD-GnT8o1 z(b6y9sIXl(3qO(4<+&~eNMp;r1kF$|mV7cvB)^1gF!1bh(Rd21@CRmnha$r}zE+|Y z+>Y|-nb*K_4)n8lB&*aCa+u2u-ILYD>!NIv8=8;5Le_pq^-h4trV~@LXZ9Q==BR!s zc?NrFAax#SzoXIWIQ#He1@Vu^Xoqa@+kSv$@$4r@DLFd5y?#eYI5N?_Z?$wHKbis$ zTK~?^Uj#>kGIRDR^g9gk*M4~EouTJ5(_H@$r^4?7i@9NeYyNzj4oWk+jm`8==r$r(|%f}Ksm6A>&lW^e#QJt$ijcAS@63v|eE~kso z-+@rZ;QFd*;x16e22}l(#h%k+&`(wG&5`A3|NMrG`r8~%cBL}MQM8T%P`I$229uB8 zhZjX^+Jc^z+lK2-k|uPO-PGW#h3ZeKP+g9A2Viku zs)Pk}ax?gsLt}Mph?`eLQ`fbWK2;MQaE6BQl@{r5G@w+Q>fY9yvge-eecleaDvOEJ z7=s@xe(Gde=ap{3y75p-c0P^eO^PXJmi4R4-IBsOs0)%3&U$783@4E$a=J(qj{J%G zi)}*;U4+L?=1``dBw{9d*YTFMsUrb-IfV>3gowt|u1NdyD?sUarlpwz;tp7BQ?Knk z_E1QbzHhNh_Tk{&9g#){qb|(FF=y3r?egNgvk9?U>_YY!WNQJVKEr2rjjhs&f?&0k zJGToS9Vo=P^A}Y;AqiiLI~oNH|s0nQ^!n zA~D1JLv7FYHKfX`qd_Oy)ZqH$)rxHR(LKuxjZ+Dl(!P4RD2mHmjqutoIP!7K@Y+c| z%*Ri0!k|!(j^re*%o9t|U~@NYwSbo{4>v$IOU&RS+k}w)e+#g4kG>Nq-?0k{>{$Tl zoB2kdh{%CHpO%rABFvUgQN- zxXy>S(?fO6RtHPGwuru0fsXb8WT#RVBG*tqvbWAC$wlICF6`TCL$=o;Ac6YCWQD6M zQsp5%%JZOEb>*-$iMUU66?(xQBNL>7Nfe44H;@K8KtC}CCFDMAQEX-}o|H<3HaG3^ zi_2?aMs!x`t`J~=<92e**Y`l{2#C>@KDVqxBywP)Qf*L}4A|OHB#gRt;+;ISge}@M zBuXxKV~@3GBi-Zfr%nxC)=abQ%vCD(7|W`%fd;7b^C24nC+6SF?+;T-giT>8Wtu5S zgZzWqQ;70qgD5RX8TzR7^O83e5Z{Pwck6JBF;oi&B&Qyx&V#0v2fcFL1eE%c#ciO( zb@P#^emmh*FOjkeFdFg58PjRR%x?CyX`IykZS}9@lgI~ z=#pLnEE88g3B977P?T%sdN=}a)DBB+20WUVUeC)@pM8pku3C=;Q`zkyJVBZ;&5&D= zR`FaOI+OZk>WJn0;yLK7i?4YMdQ-ocs%cbh z4{R4K#DyJYPOqC&qxn{LH||XFm;!CC9hXAa5X8(>pXoY*mSlx!kjtQwe2Q$!%nM!y zAR>o>*VyYHzdSAJmXR(_b)dH$;W=>UtTfWhbm@UB+j1^o0)M<9WM^eJ1A3^5ERAQ9 z52$T~M(C3$^6hH3m2&3ami1KLn&e-?q?c1~5QqdBFxhwt2Rj)d(}_+keY{P!i}-LZ z>{PtL#ZeqQ>_pWku(SwG(eCMc4NiVcc8;hp#)J>|xi74k*vTrHD$sv{0uo9eQLhHy zJq?O5vROrfh!_?I50A@C4;?ZHn&zmnB$PV6g?=f%jMBM-GW@p^yAN`Ya*$_cU-3Av zP0_-DwEv6Fk7cv_S_m?^|ML{Vx&l(vy$!s_VNLO{zqrbd2N~WPJanpp_{!wu2h((H-@#Dl<3}H{Ti$DGz zt>o41;C}s3WCy6k`JR=d#Ae-t@8k(v4LV`pL$<_pXaLvAg}Gub4(_c?4#VKTb|9pl z>=rlhdXqe{a9HyJ!Y}hA>E(OJ!A50Q@e-hD>iu!@NHc)Qp8HVt*>=1$FHcQLD0w1Z z$a$zuRe@~~>d%7*fo2NYlsJeS@#F|cbSJ%Bs<9V zw%HYpckSwHwN>bsA?|TOajf+MxciRCQTrWBc&{dqII_|NAR+NKj5zV`(*}Ywus_#_ zWUyuo=!1TK9{pSrX@8A-#|AgI#k6yBdt_SeN94EQ*RZWJ6VVLK-nT*68`;7pTa13q zRB?cS@~P$k#GV$U;g|W)W}5x^i5^pc-wNAy>AHh}>B7!}8hsTmY>-#OvYf`u#Z3Y{ zBu8`~&m~<{a{54kOXBN&VSWy17^nV9sJ(|LZX*=#Kvt7w$AddH_XDh(K*2Rc8jIuI zI-U^dYU%gdVMObFO2E$BArzZZ**-Wwm|a{xxG8+AH1?qcF&|wK;;!np02wmZlDYoy zWzlp@6lp1)P)yo*YXYiCZP*hPG5@(>_*yn7u7%zS{J=HOAWgpo)8~Xo_B^@~vSe~E z3;G7w0?+Q?ejnO|xr9(E`L3fa_3>f36q}r4+4L_xjUSVNK7k~YzT1F@4=7ZHm4C)> z`jnL6x3YUICC<0U9oPKHiSEa*t~`iD5(KEA^=xNL9=4j3x4axe$h~{>ciSLkjdm4e z6HyB{9m}=R=A4lDytJryS8kI*=SR9XKJliK5Lh$b3Us?%)M?7bn&>Wa*ETP`eDNsJ z5i*aNW=-32U+f9}S7Jjo`>1H_=`Zc;lryKYsJm*`Wn$BEOF6;LCa~?2(Z@(_Xb%=l zsH2p^#UU_gB`Eg<-LkdvN$-^tTpXOwnP)~Yp=7q;4DPW9(xf0?sgawPJ@eG2Aec*H z$Zg+>qg>oeBbidJ82C`MTJ!`$bWGt)Z8E|l#2V5bQcEKx#=jDtGMHvIP5}=Ey@o?YdF@HRX-kP-6t&B(Ra9pFpb<}BGao} zDs=wRm}pECMbehX!eOJveoe3uiQ*<+>lSI^PmiY^I;?KDLyyI9V8NKb%(j_}+9Akh z65MyN@#@$)8qv7qZEs_xB<-9-DJt{nDbKs@v?jeW#{$(f(&{uRj*}zy#ySyUR3uP) znR`Fz!M5+o8Selheg{I$!|p5~f7~i%HD)<+Z@?NID@16CpYyMwcc`Uo-AB-M7TM{2R5Je&DGqHz>@y(&wc^BX zL`T!ujejbHK51k9iU=!ECJtKkL`U3#*ia$ABN`gm0(?9xKGopSqqeV+965C%@uDI8oyy`^X zfVCcDKQ0}vYx?pL=w0fy$2plWE$E`=LX!FTZbi)v&Vq0oq z7YA|~tuxRILd+^h%uozg#<2VC7&;F&xf^$h-a@)zoPDOG6ICB;^~Fgc5g`7-f`f&{yLg~w+1!_-o z$=2749TB2!CKkZ4+HJxgb27YVxXXUgXjcM;f4(mgEK(+To@|=Vk1kZSFO;Iz8NcN7 zhf|-2?^aP_)ocVfA^7Os!_I|-6YrkbJyLd~G*%~`e6g?T+`f%mT!<-DOGHXI^m@U* z!vtg}J^Pk3%+Pd#4k4b$X(g|(2&3zK*AMu{VPaSQM%Bzq<$UHB75N>x1TtG5Ic;ba zKl|kDpuf)M7EdjGk7wSrQ#Z6CbxwMySl zEJm+J2m5H{2*p(9ipHjJ!CJNs8F=28}k#`b>iNW7+90%!ltX=(FCW|lTq{W#v3oQQHNmz>b% zQ%MF+iterhpE&3eNZ)TNV?b?v*LNe+$n!jhfxTD_A>C6_Gx8e3h07`r^O?jBO7}gM z4GY2!Ct?-g#O>cvdKK-_z;w2<>9qeyAeLVNvlJHYi^B_tKZ>xVT5_~@L(NJ~*}5_x z=JKes6@~s%ok!lkCZof%mF5fFXnrV1pZ87cC;Ze?%Ut-f*Gl!{-m7>Fejv7MS0+#!g!vlF|w(_ui(BhyC@vz#skNK!RQCf^8 zmByZ!pdHk61{OLsURAyGbytIOOx;*2Z$xF}q`lFiQcra_M@$&Rk@oRWX-1jkgBU(u z7Fv0qr@~h_s=L%MS`~*V6zsUSJDkc29;)FE%1G-Hh^vVnVb?3n=i`*z#$tM?ob?)4 z@|N0nA4S#n*EFB>r@AkWp1UzDHuLJDa^Vb5tMTHQWKwUe36pC9i4a)#zaEWNGa&8M zRT6hqI*B7o?Rv>skwrkpfzgJfA?6W#;cL2{W`;w>5}OUBx=E=b_Z+RW;ZGV61` znf!lry>~p-@%uN>LTRvAjM{I81H7eI3SKg1-)JqTqBUWrY$1jyIX35|$JI z{UYGc%%6*qAB|xWx+KCTU#?-cGj>O*ETBcKZ@xd~^)n>#)1d1pn@PF7=bmqlsH{-* z(|cw!@WoQ(Spg1}$n^J}rkRTnj?dK_EUwQo4XL+^yEy?A`$nesAsZ~#sL{d@g=*7? z7Ge(3L6C2_F^6wI>^!wk{QEiNwVqP2BAuqAD=L`r&SVnoZ*;>J7oPFKVzC!stp`kelQ)~n!GS9wWZnD%&wtE zD7|&BA0eRDqh{>tJRNK2+os_dObsS^OPt`#n>0^o^<Lu5m6l!ElGcii>T??00FRUA_P zGO&QsL<$r@8W;(NweyRDXEnruB+z)sXHr-Hlj-$Pw9Ks3aDe@Vv$$1vkR88Hc|MsQ z)%L?_fL#e6>pcT_TSe(5M&5j((Mz85gv=jc$33-0?@iy`#gopL5`xbYjx3Bf*;_P5 z&enZQeQqY`d;9Xb*zgxU#zH<0NnR8AlfJ-FtSQ<=pDLy-dJn{6T!f%~!f{;lEkU;CC$3gxApXk^;v=<;ugA&c5H>TiaAL%f*{ z>ytBk6YehJy7u7lRSi!+K9Kz8{jhp3o^RnU8U!NY76S$7+245ygV3aWxu~`dF6dBU zAKZmd)OQZme`G#j*lGA$jq2bgL{h3=^GL#N5!f&bv zwo~4}ON(C;;mKtP-5s2^RXg<=&;F}#03I?bb-h)77H=NpzRPk1iJ&9FIVk#x!wH9# z_4rYc?XCQ;)uk{UYI_6yJu{mym~WI!m&kBfQ{8qGT^wq0+I8Ev$Emq0)OrjN$?0eeX!zvZ|pSJvi07T_lU}zEPgEGHP91Esu0LkZRZ&CpZK zt7>cav7G@K2liP?s$2YbG3b&;uo&65xtH!d@Vsh2^y2DMO!3&oOS7f*Zor8pWNCr% zdtpR_<-r`mVHnvn`cE<9`U8C9z3{Thp#rcK7MdP7;J_;&uu%mS8mBEpK0Siwn6BB1 z-i=hW-af;*-}K5)n&FUqLnlU=2WgfqxY6dZq__q{Cm93th8?V{(eqfZrtq#vm7(5N z-r-#8;iq2cM#%s_8tw*|E6a;n&N&(>1D38n@_AWzpH=(KqQyFGmed~tt#^4b?tu21 zv_pY-pJ~v=2OVFOOI(?sJHG&wH!(q|9mDXi_jXUyt84&;+RbEt?w$_QaGq8isA~PN zbvb5*0WThR*2%)*6sZJ0CEWARNr6sh{CDJ$aRLuNB zT^E{DTH_SliK7U7fh%1JU&@E<*-m))gIlrmZ1r~p zC5(gOn68qf!<~VqTQReWV;uQj-@$=J6o}92R zcugy&_Gj_9^*xcZ@GtStRm6?(*w2{T$G-T@Sv#W}B8R`(cvt#N))}N3Wv#9JQq_-$i55SwIucZ1EX-tUzyD(2k2|=d z!|kQS^8k^Ia2bUP>*SXfz1=B}4DdwWc$mvvQ#rg4Y^XO@m zY}b8Ct|jAW?gPv8lIRMS*}KYSgQ`|Hd-JcVXo*$a)$sv7~v6hf(XD=B0O!LCN=dikCTq=~5^SH{rVSq%bU_eL3NfO>A31MEIdC z2r&;JJ3K%6X}{U$!lqyU09Uq?alT{)CvyXW67yn}11RhDfj11S`t=fomOyWUG7WH$ zv4S?_3lSWUkt}|rlo@ak4^CGEBe&}Y0uC)ULJ37I{22L*gqg6Iz+8soZG$ae{H8_JnUZO|!^{8jzH1Ju?36^XJ6nLuj`JVJ^`yZUR&5FQILX|yq7nLRTZO4@+FFVO_*!k-su8rbtV>%y(p1*d- za^NwdNo8Tz*Y5Qz8-7rdTwU>*|K3dutJ(GpRFg89C-DtA;HKwCK#T8ZEmQJ4skHb} z<3j=5K~b~Jc4leCNDr*|@{sAog7`&j6UAu{cKr5Wwjo*g)#an+@W)p=^os7L^H=DY zy8hDNn0ExHrWnZdD_b~6mU(`Q z-X*mL^)G9W-|u+wmd>}S;QB2-{JAGf zXQl`gJVyEdJces==1!uYd2a9+2WLhlZV2=m9)18dM|o5hw6MT_@MDDV#Y` z1DY{b-_atph)k3+YZmu;%gS)@<``OG0#`E)Up%`Ow6SeJ! z-6K!Nqqwd0`ff3WU89xZ+0EeLX@b!a25FPp*qDWpXSIs%Rf7{btHih!M=b&FkV<%N z!~=S-w46S<0}9@H?xB1*P}KE&7I*K})h5k~2yfW+OAD=NG3|&b`~nJ1K;}6d@auX5 zSKu;oJ~h_RT6nJ&0D~Aunn&+iufBTFnAMWVeT~mppA5c}X)b7Hi@TGo@3UaGY0Qqd z=k5k<0^qLmQJnSEw^AxrB{^mG4Hr9pj%sEv3rtsu^w--qP6L z6?L_4ThmkJ?lRT++JkrRvS9l7wS!{j-?`!#;&pydJu1qdH^a59eR9~Vm>sPCX!4+J z>AVeGx^H^xhT+wDoXo>d3IrOcLPL-uS@GbIi;$q1C#) zZ-2D^)gCWnz^iTJaZ4L)#u3&vA)8{&8PYf$@bOxKP=|THD9T;%QAe!bIVbV0obtju z^^Y=Q>*9Q#C;{k-mR0py78B4l@=4PB*kGdFqhtH;|Etj4fp~eL2BBLpjgksr6@oJ( z+ss(1;Z{~-q>pn4mLa-g>s?&hwL!NGD(#lJ1vX`XH=d`HSw_NoD@CvA1S9Yn2RU-JJ=pJfV^*7DC%BK4w`%KrdR(`Is%VlQK*5 z#jKZu#m4I;5&9by@wz}l_6We?uJP^pd-@|U0nXvLS2g#FF@F3`fJ;8Q);N_eM~v+CVWoAf*_!Pfm?+XUP_=Y9DF*wo`ScxbM?~JGY=+aB#tFG^n=%4Ur5xpO z*bVFHT;TVCI~?vnjH{lPeoFNjXSiu=nUoRe*8D%G1L~u@PA0~1)beXP`?5m%%-8!_ zPo-q?BY{A)yk}eLG(hp@w`Bbzobw-CZ}hbC6lVgJuYJt*=H06w8j_W-0rR22Fg0eg z6s+_(9pvaqrS@+tAAKuhm0bONoiVxviRM(jy!2z5G3awv7fybztMoScFsE_HZUHsVA14fKr9CO`I*qx3=_B5x5)(FK z^?A6~%EbBj$G;Lbf)Brj+M=v`x$SF7TaCG1O0tt@{QL2Cf#GTomz9oC*BgSB!EQi% zM`vi06=xKxCmK~TX|CT_WT8kLRLUxdZhI!9$~)-~8&bYYyGbItHl%W{_iWa-YX_J$ zyRM6g?g2c$HV-Zs;!@h$5!1=l*rtjL|@atEuhgG<_m&B5hnM1ItalTF+xC?wxxb zF|+7uh16_(CoyN!C3&JIQqgh1V*UJkJk`ddzLi%heCH8eCq#5Gzd!iP-Ab@^?B|<8 zdL)r~WIPZAKkU-$Ss5c*p9MK8*%m--nO{@f4_0Y^lm)QTIzl(=;U9WyQR;LuTvet& zUI5g899S%`ne#h!-MjW~v8Nf5J>-VbkkWv##d=0AqvNi#F|uVe5?R0uK3iDl_ERExyS@}i$G*Qxg! z!t|p_&c60aeO(>=RP`YH#HV$V@AktRuBMzp+TnCs*|Vau`!*2g9cGH+kdNW*EWqzh z&&iV54P1Lg49ctJGbwGoJNKyh5#HNaj*f3fkJv-_Z%8j>{-;24<64@V!qTP2T*Y`TGUp0b4?@ zU}P{&FdHBh_(eX*|9w12CDkymb?-aC8@o=&>{Kl4jvkJ`M;d-)x|m4E{==46W8dA-8v-ph!TLvYMS-nVTIOOwg18Gx~?fdbMI zOAyn105h#P=H7f*ZVAPJ*1tGhh61)=9DV*MvVyeQFoZEmf7EI3$NNCx6bT6DJe&HQ z%7ua5=SP|TN1~n3`||mZ*^=V@)`^@soL08YF(`E-;Lph?cZIg7!U-i|DuXsX0Ft6? zzK3k{gst}#pO-~<)mnVU+L4Zl9&0uee**HNVehUR?3lv2R#`eg!%hG=#RdQ{IVz$N zDCQ$WDY{*mh>9l_DY3~{LBU7BO?IFOZj|TFryu$Fu{P-|C4tgK(!Xz5A9enx6z61R zO?Z&SGDXI3dUalvB@JT9ktzWc0!S+M+XJrYq)MJaw|fABQr2f?#-pUN8GiYnv6K=N zD=g zO^4wm=B9 zFd4vwbX~K^pK9+3cG9)oneS#PYx(}45s-;Y`j=;9Kw<7Qt}GchP4wKg;Jt*Z z;I9N|GqXxm{u5ZU2chY#@qp;=}yz8C8md5tQi*_^@ z&v>k=nSNDD#5N%x@(|zsm!2_jB3i0gM-n7Op&I*C|8rnAG{^Lmu}#|_efH`T715#@ahp{ zjA-mAdtKQRJ(1?2Qr*TH`e#0hMegkhhaja=bECiiz-yBax&qwnJ`kyo$EowTo&%ER zkMq|yoHbccmmAc2L?$X;+TSfe!;6(O)=ECY)O%+QFsoR7y~J&EHfC}DhY(jYeP3md zi<6bA1iR-Z$Hh^@+V;gwK?gXu5@3)Q7m#i6#^+wdpv=FM0jQCdsvvu|Lgt8aY_pJ! zq;Mzb-LV0DOPp!x^PSt=(?R;BDURhz-Rm&??M4O)fJF!H>FzEv2a;TD-_RyTR#~SuMA3`YUzFN3}$j0DdE19J#rC zx}Uqui(y>r1p$>srn3jY33;8CHW&#UU;?RFpV9r~f2v5&`3Y%HI_-c(zin z08Lo*m0<&Lz`%Gb*tXM&f<* zQURQf0-HP=lSsh|+ul9u;%QflaPlpbnF}MDZ%KJlV>97c^?8q*w|M~g1Z*eEjtfIC z`6*JwRYtkXjDH0ULpr`a-!K??gNvmDN#KdUfXJGFBdf;i4Tm|ENiJ}p5i>E)8P43w ziv~ubxXd6e7n780nXH@>bS*QJ~^;-ysUBc#-uDT z&|c-a0qA0zUMdvG!nQUerl`wyAsq2e9(gMI2q2n*smsrDu^L!715??$@0LI#r_RN) zK|PF2;;7=!smODACgfRs2D0!;U9S8`W#hKoGkUo!@dX1mdxpkKtX|^` zOmgnJFkiG~PeaU`{Z#6FvXBjVQ;mR2apN3Oy@83o!8p@U!}SZm#~lVy`E6hq6CUY8 z%@bYJwKEw{QQt;cuWKn^cnTuosoS_32Zac2nAwHo*KNVeox=c-syMj8yrg%{lQ5dh z&7t@w>Z8`h*`9`OVS2ytFeDiMR| z6SW5QJ7xBc1P@O6e5T+?Cr~ZFT^`*f>%_z93X$+QsS10T3Vbl$0+)lTS|!&l%HzXmmDXF<5b-|KRs~4I#P$P{~7Fk%M&8l0FMuOn)E3Cx~RPP%P+&3COQ_b zG5@4i4`DxzwP=2QvnIkCZT8s@P;l#U7vH<~h_7f$j}hm`$v45UQgm?!__b@ca?}$z zm29{`ex&4FCl!AolMm1}KiyTVHa!nFjZb<705zqf^*A7Y?cS{3Q|~-02O-)N2^^4G z-G-arThG1Q+j{wDB80bzd|TP-xALlRYvF4vhhsn?uP7D6!BT82f;)v)IH#hwp6-Bp zZCfhtBH459t0`aSMFaG|VBt~T`wpn$>NXrT3V?_CP6?a*i-tc)!lumWrrCfU$T<7Q zxl4h74^L3IkbSn_QXqt$$=(*PM=_r<-QSe%DeX+kAmOgU%18Fc!Y z9H)hoNpYzsi`ZbxkW~EOy$W<;C`BgcfUgC2Gt&S#tGB+ph4Xa!b8%EV4EUNMNU5uL zo=Cn{h?;SCk>dLxXP4;P1ZVbFc}~&Q0_gYBjT?AbD>($2 ztiE#LBp2+kAl-I?JSa0TQ6B+?`=I2W zzp|!hfH2Q$yl`b?+NuiirfBoM=ce=O+*f$*6kzZ7Tww+9p^8aOl-v4nw?5IEA1z

$BaM$tc(3kxS)535m$7Qj~P-8zhx0>+CmLY{&iC|p^+Omd7QPLYAFf6izA(Cd2% zHHR5(j{ZX|hZk`4_IIE?OwB@f^TCOBRR(bRY9QXiDZX(AP1ooweJT}YIQhk-+%fN6 zIVC5RfFVzipDr+s*&b;;g3tNs(PmjPvT$AFN1k0Mtx7o0%Oibb$WC!a^B>$wy}4+G zVJhD}k1l6^0M%Loo9Y|?P_Wg#|@?DYoZ`eP|KtkteYb`T+=L_D6v`KeAEOd#Vf{K7k6b{-k)rJ*EZR1C}}6FcQ-zQr|wwc_XsH@%|x10{9=`j`t3zpOwn%?W+{$g|uqqzmR;>!?!N=@x~ zg-SQW>fZb4i2;If^ZFj*=aXCaUd+-Q*4>E=POgNE6L}N=*E1H2LT2XRef;aGiY6t2 zo)jF<#y58_pF7$eCa22AtUZl_;@r@VcEajXk_c7I&A*w&VViBA*QvkeAh=pEpm!9% zAPVmHRnSkt#q(|*uzlH1CogLJ`ktc^vN;Ady>>wa!eTAu)Umb~@yH<(0#O?vktvqk z>k{UhP%5BYbh)}HhRo#-ak%Us0U5sl3~b6iTHw4c2Hj|W(bDgkv98O00?Qw{d3oFsQ`AI6){vb|20UoXl&-YRT$6ssLu4J zl;P8*2ELq91+DKS$#~BFP9vxv*M@7LiH|LM{!lf&Nqbva9MN~V>pWxwx6U7e#!AUJ zqBy-Dz9T)rFO?QTHD!#%g|O~~f)JHU{A9L*I~lQ3U$LFk{1vogx0=IWVI0*Kx>Rs8r^!SiQ9ts4*pjk{nyYIG2|ygQ#kb3@KxzL7Bx*X zh>>jeaeo=yoqBsh%c$(Ue>i_IDwgtLxj$>2)dlc=cA?)zAe#ahbC0FEH#z(U$O1L@X zvmHZ>RI6tv&Aod#Fo<>k+04Xd@D6L6VY-r})}C2$lzs6$@|gmh1@kz&yZn#A<%$29 z3~TVYzox@j6fIalS^pMLD9MAm`;eQB9&j0p{tpDkPi9Exu(Hjvn*PI5p5@4bB8Bp}QYGO5aR(uR7ST zxIoJV$Yw?+qjKeNp)*1Zt2|AUI$8Vr{G!ow*t%I==VeITuilxOuQQLU&vmVeYyD`R zGuW$*U!`>Se3%ZN^d%(j&dS9X+AjKg_AUaF{1}z%^~QsAeTP8VlugB-);6O7?5995 zxe<58%EHC}00igP>V|G{;ux%**%oWbuv*1&0TFK?WbJ4E6jU4VUoWb#PQE zo9x#aTNPyH?N0}fYDSU0C{CWI*1V=>o)UI&J$vRqZ)?d2I&$0@hHm2;%4~qp3&_rf z$g*F^qkuu^eq&m$1P(w?D}zA~3XY#Gz8Ln(yPZF`yDkn4xuogM)W(158Ws+vB(n)@ z^_py?B)l%BHJs9M@$USksoCoh8x=FU%ymKy|2wjPmko$z4WPLfaI5S{xj#aQeG2yH zEq^RPH_#hYi9jm3;U<)Mm1w6RQTj=Nx4RvI;a*`SOR4scP@pld@mRI~Z8O08lH^JG z#K7kxk`-3L?3=+Wch(%)i@LRh2M&N!gd^Jy6SDNiGMJ}7ycp~IRC|mMyQ#3GBtON! zTrx$R`J@~sd;jhbEVDf#ysw}$EcA{SGDmrAF--k?@qO9LtmE?^3?epbRjPwecypIS&?uloe)vid<|AGG@_YA*ncwGhk z?5CN9*9$6|lnwRs9DDB$`C0Q*HMv}M2rGRRmRw$dWCD*}U({tdsKHGBwK+N8Q6<07 z8LB(Tk%5W=8F`;k6AHLYYe44}(Y+pzEFiT&5~=&(-h}?AF(ldV(O;)|Iu9b%X|dU` z5&%WrOg>tIB&67pISj-gC$941TpFz2Un#3oVNOmaPjQ@$azwUwWcjiw3)Y3;> zr)Tr*9@5htFL10EBRxPcdUm)&41FB(H9q&#pEr~|n(Ky@*IH2Q!Ql~NOh9IlecdhF-BiU5D+nI2JVOl!Z6MKuqkgqF4+r|%pUPtNMnYq!VUT4R z^r6JwtZn|o@{_si#+@cD7`_n_h)!T#rwhULYU2l?DW7W}9;isL^*R+3$?`*FF^M*t zR~PrNEXHTYrl_YW2xV#mwfypR z@{*T_D<;IX?4kL)$GjsXJ3O!eJpA7Kpt8q8(%e3D6RNYQT_9xY3Ml+zzxoc6s%NG0hZLI%4e+YeRy)zFUTiD0c z=_-2%4q-vpE$A^8M<#@sXW7v}xS!^UgQ=Th$_`cIMId%%31Vk3Datd|%-VilZ*FdL z@|B>`qTVe0C3=tNHZ))bvrA-M zTU#Dl&HUurr+IFX|9#Z0h~)DJg(|{y2U(q7Iv70VD}Tq@tmI(OXwKUNR{vFVShH{7 z-zl7xzN4qHwe=;diDSfeR?W!z|6Leem+nl)zM-T|(l~VfYWR_0{vkpDAL6DfalhF1 zaT{Om8p=qWv}iYZTsaaoOEaHm3ob%{Fx4Dy9{SYUb#_+#rl!UT?c zjauL0Yk3!ps!1W7va^Q+VzIf?-lWXe<3o5t>F=|Eugr;}zHPvUd+lIP=`k71t>q|} zF^B*CD@#(ramg3eS~R>CaUj0&E=*CTE&x`+8?L&V{DLYl(pompyn`CI`$?&rUvd0> z{~>9qDY+*FYUwXKHU{caD*7P!WBY0<82ho}z*m>AC?okXnK$i*HGvK4D(Iyad(%!Q zb@kWqXU66*WF!d2(ZY{=GJ!}@D*#4uyvL=Lp*hzOXaic~#fmwbhtdhMPJQ=Gh~ZRQ zbNS;(2)70um_$Is(Hr{3?0?RHo$mhcqbJO_0bTr4rOV0y=&oc9EbI4|yBUEHRT4Cg zeZ_FgF~500=%)qnrh;A)u-d0z$TzAK0^M%0zcx0`zmr!~zc>iZPzbZ?ulc=l2lR?5 z@j7|hktq0~zUJW<%sXIiqWmyV=STG)Zk_xzfW7sD-X(PFPnSCSfkD1kA&>DlkuQUw zx6kqZ2JoSd+P}LRMo_e7$&3W!=wevTy-7eePuB^g;obnWef?&yRQfM*@0!8iUB&J3GT(9R9lZ zxc7Y)ouIiU7$6&9txbK)aT2w_Ru1VP(-4ft*XGsERHCNyNN3ywo*rBeb04T$|GX82 z)N2Gf@60bIwYyBBwnM=MnD5HK^O-cNnoF-REiz}Z{mmZ3@cE?xqfqMW?hYb$!{Hfs zBRZf+`}g85Ei1n*e;I(=R4-H2?Okh;s3kK$vRQegbez` z1imcbo3vxN5fz05^3&!}1<>;BNM9NFLvmhZ_alItu0!_dXR- z3~qXTB?Vnm@?_>cpGi$ukX3bvMm!I_apluMr#=NaY8lYkTPzt`;W%BLM9HbD1XRf@ z1SK;%RnHw>zrfcI@IM5#`c*I|OdJFgVMxH}5I!9&FupC}G>dmc$c_fa2&0q0h(pX^ zd8squL7omTr;d{}EC2QuPzv>dUr4maw?SM}XDXIoY(D>elvDyBJIcs=wOCZP{#VWg zOXXeJ6@rVxS28Vx)E*QFkDXs*!m2cFZO@hGX&fPb55_@Wuck;%T-3EQ)EPH z@~w)3ao2t5-OGb?)ZBi)goxvvG5@QMT{1aFlf1LWMntg|~qYRC=ld}{&WRFk); zUg7W8ZI&$l=hX6OhgaXbD&-}{i~tj_PD05Ji>j`h*ofyf+%D(>}RN81*H2p;_T5o<`*O^W+n~v%@E1aa zY?g9d1~~7CJXbp~Nc*3D-XDTpi5Mj+`$G}O7jsgQ)B3Nphrin?e2*#k38D_;VOeVh zbQ_$^YO}NR>!Nc88%N#?ga3vHx1>F=jXW8UZBd`4j<}c6NLZNSu@u8J&w_&ptLXNy zID0g*Fo@vRvzwQ%-f2rFMBPnOoSmIbYu?=*31ZpJ01k4o+`4>g{U3;#^(f|i+weuM zc;Ks1$bUqs;S;;e1`s@rVZSHV{Sl3YcuAH>I*|`t+GlKoBPfFXP7XI1?d+?4&Xdo1 zjVhvEUzO~z5$)5(qlcUmiOBn!N)2S*Ys?WtN%tYX6eP)z#NOxgKO|MvZ&{%kveF{4FmJ@8~5Lr&=cZHd=(O$PH*gb5ws8;dR>Zi62KLw;5Xf>n-f{Le7&Gv8|& zcMz+7o)p;h8MKuzS6^~tM0SG93N2O9_#9WB4WocI3AqG zPRSbob}0Aa$cw3D4U;>)nhgzmvW}`eFbJ*u?AMj+yLW!Pi_sA`tH%V}$$G!TL5dBORm~wg-_`f2!eBQ@e_+T%xx~`sDi}aKFyWRe3Umd@K+<@2B=+fxo zN1achB&<3>mo1DSs%vU*4+$dtPAvcRC+oFm7B^o3zyd!dSQ+hlSwx-!r9iky$cm4k z@q3j`iqPs?LJtw0`YH=rqoh$hA=U^LYs{=KIfWYR@tSU)%uFbfzlR1B#Rq`UE9x#X z&!P&_XOzP5?`7Jxz@DEP-_L&PcMuj#Z!GtWqKzdAU3Y)vjP$v=n#K7khlyqcvE8V& z+8f{Dl3%XlJ`0ksOCNm>(_1yLTi2^5fpFfPee+v}Lq}RUf+03zn2fAC1pYTCyS)q93n5^UJ&gWlcF6<3UyX?&{j;zC&y&3s z65?&p$HE1QEjUrc%#z)Nva@)F$gx5>Z$JB47pLl!UGiX01!L-kB1SyQ$P@Jwxh%Ix z*u?NjHL2kREciKE&a&~7Myg(0|J2mG&TiR1kW;$TZoOT|Be{O&QGZ8;BJ*ak8w8nD zm&Xu?`44g^foEs3%CH}*4$EZ2FP8nG;ZlZFRGI%t8Po&oY!M;j4;jCrREa0rw~x0< zP)ta{&6SmF7{;L-JG;+m9ckGXg$-dahF|u;ibFY3lc;>smwKJ3eiVCdm_3%fc9Afc zY9GI!k&lnh$N{RwlgJpW9@hp}@%E3{`|af7$T2LgNI@qaQ6*DA3C*X*-)fy`>vWJ3 zPdH&FvCA?hdovZYGAbaxAu4E%+mS6>TU!GQQ#4$U-c9_^eNnd93@+Rgv=xuX&d!3O z6w`ucWG)9HtZCxuyEIGmBB2PMlXcS^bu{c-E>g^C0cD*-vV%^{I|~8~UZhZ(0d?hRZhc zL;6B-DOJ~`+m_mPgma z$4v6!gbpwE4h}e;N)Nn^(XCC3klSH}B~oDB%4r9#^-{%VZ+i*9pct%IsAKZ z>t2?rGxDtRc)_hhi2cKY%X{+XRvv47s}+Q2JwHheP9bt^4y31m!BZ|k=S{Ii<{Uhw z1azpMREXJ)OZ$6A-C~b8g(FHRZOX#X)`pQX??TcFOzwO4o*xPqo^xa`6bG(LzAk6` zEjGrbb1GUUz(g-cr}*FMNNB3(13JY9iJ&J1-=`!LxxW{4fZ|S{spv}|Z@Ht9kLg1Z z7@L1d;!THBMC|_lzDQ@Gj2`+(X5<-r3%Jhpj`_2RS-w8g4kv~9`Hms#FvL@2E+xK) z$l5qe*WApwJqV!`LtJ~b&EHk#f3_Q7mO-__s_)ziZkQOmKWR_k=k;aM)CZcsDn5Mn zfrh;7Z_?bg{AG*z=ce*w<-x^w|7KWk@M9z?-EyuUHYOg#GJAyAVV8o100MgtRM2Qs za22#*yh90?Px1%Cl3G4$6s#Dh!I zHQp-1R$XZToF82KkyqJ42=-YxTZh)#IJm7VBw#l*L_mn|opBmakbZ}w!#d-w6_65; zso%Qs(0=OMkCo&Sn?a#fNUb%<48d_H%8{_0i@*TaxkFskFbYpS4|P!4GHGX#2Z zU#RLncU>LGvIi+tg#piZ)Ct;Vh)2H|q3l6aiq9PG0NioHX_^eR#)qiMvJA8`9tLGy zw>Joj0KczQ?Y;w}d8%pa25@7e7j6L5t@##QoXbTOrpyOy$d)OyL5tUAyR@7B2W23g zRF37MP?VazjTOrNrYvX>aVvXfFKwSY?VN+_x&7SumnVF0s_gEA5F>;4l41)A22yZQ zCIxD4V{Vy7QVM~UX~)3EIKD9iZaf8(YHQw1m=i0HoaNC@Hw6*lcF#-DD}5ix;I&Jv z`@N!RCEWc(>j9*DEa3KQRFD>*P#m9?AT#*Fa2S{`^}oJyyTa>QaClde@E{oK9K9lQ zuV8M?G`x3`p7H3r&>=L&*K)Nloq$T9f;eaiBm}QRUNEPBjM1);FO%x|a*_DTRfrY&Tee%~(MBk(l+VgG}=*=*$y)+szS| z88PR|n9rI<-0A~fM)OLSOXi!yT$%`j$x49tx?rXA&b$Jy`0yk5VHn79D1NH0zd?Qr z+o5HG5^0-(l|+H^=+Aw~4J3KA5zNxlts{UJrf&l6X9UT`k{rk7PhcqL8$aGG1<2m6 zq33My&isZCfVD|vL&U@g2Ep&`GM)pMDj^^dko^S6;ka_gRXgzaa$f$qcmTo^T8HL2 z`sZfM6jx9$og|lO7>1OuyJA*eEVwXI*(;tGzEc|cL)7QBtGg%q|@<%|SR)M&>HC59>R z8fwfslo;T~=E_?be*mb-^wL|O~?9P;~zj>C8>g9WRB>&rJ8|g&&4!5c4jqW>F z^hUAx%gFot*MqmtR*GiL{LT0zEt=M^QJD1xxaSn^#jwvcie7H8Mi4gRYw8|AgBpAX z6rNo7&k+3CFi8_iLRHPQ3!4R6j znQTF-J4rYQQ#=qlTA|(FiaGm9%gMun!@==W>>YKrY-n%cF+YeX7Pgr51@;Ar#Db z1n)VTOuQg0tEIDOj4z-lQa4OiqlY8hi}QGz+XvocaPaZ^>ajqU$0t5JWe;#L%7L6M zf^&aZ)>C#E@lxq5iR}qvfD(%+s}qqU*U4Kp2|T4wH9BsrrDIdMlbSgJwrF znvNAeGOGEJxC}Dh--w%q-oKx73Z4yCXKSWIm__=XV6F93b7ypRz=ku&q}FF~cGgrC z$@9D$BqbG*%+fxKazbrzYY#L^Ys5`n3%FPMzkU1G8l1PP@+bv-V*BdVqZCny5EM$e zS^IQ}w6db2kcj9RqZ5Kq+;6zX+fzLUUmeA-ldmTOG)r^6OjoWX&H75Ufvr|)-W^2F z%=7Av&TAP`te5-g{`GY;Q@+BF6d{>2QLKS5j!dpqsz(L~gVNF;Pv09py_8;GUOfvV z`^xTpi4+o9M#I^9$AU}Q&rhaOt>6a?RGV)?3?>tqKu)H5Ip+`2lk44KB4m=+-x@!N zxD(3gf`d<1+u+sMR>sFa4Z+j12i8P=;LDUUsqylvYr)b#`AT5N5>dTv#~e)n+8vmm zLw=g-x%k&Wwq+$QGqk~@$WJ+3cMh2sWx-u3{t%zTIYq>#VG+Tqb{2zFV#FR2CaV(= ztO}v{!Lc&uGJ4mN`0{G-fUkEKlJ$1qv-vj?&i6qKM{cK5DcCv*)=PYQ1~~e57)JPI zF1=KW^qYpaab_l>=35y+sbo2okYZg#x@BI*6MEIYLq=VJR(rAHr8Z@jJ`z$~ZaCtl z^dwvnER2OB(*C&egqDvF4z6k+D*@5AqGQ=*Q0~74QcC*3O~{RCbT;B4L)7{gyWlSL z3xeF6zLRw_?jYg9@bMuJI)4BJ_D))KN_i* z_pL1qgt*QI5Qgm>kPdumnbnL4rb5u8qS2QL*++BuXG83wZ(#SCMVP+HXrstL;#I1p7p3M|j$%;3(9o-Hit3 zo3xoOKSlV_>S+d~`x0m5@-Bo}2F(WDEI18{c_^;%y#E3*F^~@jtN*i|ROYP~ zT$O)EFYsr_ZEi}fP=*99*;wf(sH{Lo+=Afsz<&xkx#&~udzgzJ!t;6M|JDa4IY6O_ z4-GOohmqBClY(jE-R5K@mJhy`QB2qd7h$S%HVrrooFS6Yh@u8nwFN1bCgvdZ(I&>d zY^~4I=Wq(vpZo#LG8mLGel#yCB+U^2lGbhXQbZ7h zbghZ&g@fnJ5_F(A`ltc2hqYytFplwUgU^!ALu@$BvREWZY2?82ToATInAPE}YkgAm zYfAz?nZS5ZIjKM~Ma_J)GI{;9GUv-qJ>~z3h$+EjpbI6_Z zf0|;%4b!^`afGwo|MqcOevZw+Rl3o3#qtSxo-p%iEAL|9T^IK!YNX2Ab0`2RseGD*!PhKcx1>KIp$qJUh$Dop0 z^NhJ`c31y6MUO72t|Q;vGI3ux4l&pcl)GXiNM?C3k?ow6yY zXS)Hl5ziK7?FQrYy{G#Bu=SQS^!l1FNxVrJmvrel}1El6@&BK8hWjOzs-n zOuT)Sija3xhd-Py{a?fm%H%Uug$v6NZbrpAK|qF(UVoLj84Yl(Dev<*0FG5WWdWBu zUHUKY%<>7o-$z#(;1L%t4zlO-e?}x!P05imxKw!HjaIbjQ6zPZvD7t8dbJWV*N;&v zRJj%2kXT)w7W=I^d)zdm75Idy$eQXUm; zyQ#kN5%^62q0GK>i?bXYNQqeB^V5Go8vB*gY{o?VGzQLQzC`_WD-XJL!+a)^Ht+3* zlq~H=ZjhOkwUY?FnI2Z%Gx#3~tw|ZnzOJl|{qR5fOE_S(DYI35@*Dbsjnu0sS! z5BQ`N4HW(paBQ1l0`nOQ1c>kw>!I|}KIZx_$Gg++ndC0Tn1Pg;`)cyQR} zn&%3El*ZeL^ zavFO7_gy7>&R3pYTDqZ^lzWWMm(;Q)6!OiNCZv2z9av-%hoU?Cj@*E-cE z-Fqd~iqTg6Vzc@GJgNr|r4!)vVY#q3 zxKV4+r0W?gx;;t~9$)Xs(Uv5$C|E#7`RL^-)^=|e&1}NDLRXRG8Nv_nQ79Gbx z#`xaa4$gIiFABoH&2{^_T@4HPuHA>ceHG7_@~Lz0<|n4M*ymLjm%py0E{_EAP_wOU zM>G;mm{U+nQWy@eQF56ec@38WM--n$axa*|gd0BpZT$2(wXznXiD7iUUGe1gppg}~ zmUzd6gQ>Z~{7g3UH#uBHca(v~hAJd7_-%jfbb#g22Us4>Gb+P=w?cgSz1Ao!=jEiB zkuJ*Y@=Ca@r2khRr4-jw8ACmIux($`Tql)w%t}-aUOa#gV;CTe)U^WWWG=qI&blxw zi|d#WhOGDg(V1-$A2WqCUuwlzSpez|f%j&Lim=$maX6LMw;#~&0m`la2oydd1f7uQ zLmQyVfV=@WLwkXSJzgp6j%CdVna{w z%2T;aD<=~RR>`B3encaMd&9fg@7G#K zv96~?{t=UCObPqtl*ZC#;#hSr5%b7xU?Qq5hFH1Dl)L@ZLyyvYYM>q$E^RTJ)EZP#J;K^(Y(+3>MKa+le@Pg*C(i)P*JVhgnzd2MV(L~)dFkryOI^NX3-vh z(+Ak5PBY?L(MbXIK5j5$DZH#$42HmOqtJUaSPDhCVZuQV4!orNz2D1z*3p%iqlG6k zBh{b!T897jLFhs!vVowu`*WAdG4g@>S(Z+9iQ*I{vqTT@B6l@Bj4m}%c#D*o_=P0i z9gzOUhQp&mD`7cRKUghUg`$i-VJ10eCiXuXH(D6Gh@An7D}VVKxQ2nX?3MrEWyR>{ zDq61LPLMqT{n&tycrh5m!9_5ApO(EBH}9j6FDTG#qZ>jzUo#+S*bK%5Fr63!+teq! z-S<|kg*r~1*7pUh1R-0;L1?K>Jyg-JXoK|bey^W$LFYQ@L9B)!ACIPgRDHgP)3Kik z^JK{$5{0gIV9r+Rhudnu==4shCz}DzPTu>tT#WXyCE)t*U9#-^c30FNUWF9;zds)P zgO^V|6-QP@$dyKPW=9h3Co`_k#yN*dNoK``3q(RoBOlEvN+rc)jaYP>!%Eo zO6@bJrmB4s%Jmyxb6bkhL$zyO#pQGa0VIDVeAMJTKd~uN{%q>{dl|5m zsFAL<6j(~YAwr>Zc7697coC`(ZF;{9CDwI+RmtTe!c9{Jp0t0mdk<*dq@+z*Xg^tx zm3m-(s}f$)Pb588E?|h)2&0Tu?o$6cn?zXc{is-PCUcE1T<>`xa^`y(u0i`0k>yS_ zbam@tJ5*sfZrlm6YduzP2*_b*zeRvYNFt0x8%P%6d86ML_u2PhsPDVG$3y}aCA?u5 zI3vrGOkw=3k*m~qGmp`8#ZNa}4+Nr`jQeZtM9NvM$yBn@a;y401usYUCoB5y0ZG;F9=hsz&||@sG;tT7NPDkD&dQBiXg? zrvlbD2h)M9qc#Jf?QgZ4!kL&X%-62o<}K{SOb17Wl>2qpyMvz;6tF~V65g1QfiA(n zY9ufA&Iy@}JD-gbEhl_Wr}bmnhklFUbv|Udt-AHu`GUJt`YngiP(iTUN1)xER4#M6 z1Y#JWGJ$(dNQwj>dvthc{cwfAEw*~SeI&$`%!WKlHlM%IA0^F2SY5)5_7pSkI9XIS z&S)I3#O%JHLM+;B|OLtYy8usmquuQs6pZ%CWfNm2U zNMUk^B6hwp?WRM=0Z0gyPa80s9>lY4LR$%zG_y)i7!Cf&S)Nf?N^-#qp20`_AP^1* zU}ujC$23k~wYjIK#)fIyeQa#a z)Dyo-D-A^aK{cqO&;D!;_H3ng`jY2~T!y1h%E4`W)6Q?kcw(YTh)hSTOJR8KbIlH| zLF@8p;yqDsy45zne1{MzMvbC0;_rxba5Bp8}g> zoKW|+1{Ky`!Z-3y!pJ6IOsp6dKufW~*N$P9dU@$MoEUV8i6|>LmfVkOD=`p%3jSG&vMfR35 zDptu0mAO3V$)B~E)=8o)VXSbY!uQ~ats&F@D%$P##8JYi^_2n~gy5A4x%s32eWYY> z`=IHa)QBbCnn6aEt3mXMg9Bs+&#G5V zY9)Q7p!e^%!6>wYpd{_e5dWYRy+zOLA4PcWSZPdYOiH627)2%1;p*a-nT6b#5B1uP zQ*qzt>MW63!sPIa*ADe~LIz_b54N4r;@6&^QN{1iF9hY@6rt;^mdnH#xnHi^D&!Qw z%NBk3@VQiCR)3-q{Zkv8|6mL^W3p>1GV!rTg?jvJ5wHs1$=iY(rVRuPbuM{V9mx1~ z!ZZs{Ei>;?DQ@&{v9C`qNAA)!1vaNuENnj?_0i5k)Peyvv(Ox3Uy=yi3BA6A}X$^_eQQ;3CS2{ni83W~Q{w)aRrhUFu{dJEWRY!(Jp4 z@()l~BfL>>X>dPUC%&zq641XUmfx8*6$Nox5uhHGPszJ)O245s(bGZIqe4R53{#yz*M zEdF9<+5Cds&(3lBSEPk` zjY>W?HQ=nbgh*z+e~?w7iYpE0;E;wL75256S011fAOx!snt6vradd2Ay)+~G>8yu(Dk`YI8msRT@-&Jgu?41R%%p_qlva*K zif@mQ)t*#FeNo`a82qW()Gj4;f-JtqBShmZ4A_uUupPf%lX6M>5ehrFj)xyv!lOO) zN*1*=)JawG2H73H*s9pI`yYs4sMAPQP=Rl<0uhBg0Iq;-;`)j1UcS+%dVs8qbW7mO z>?--PN*zKi8QPhMA&$lVk5FErh8qsZ79XEUaZmz_797}2@%yeUy4PAlRHXY4^)3-l zQuD>g{_UytJ4)9PE|rpHyt~uT6KsDnv#~&ZvRVjM}vZ*0A@T{M>f# zK!)lR^)H=NXoR<+7N=z^a7)s*;S$qr@Qs3JQHhd!A(=(^m_+Ds8w-iG(VVKwbJ$H6 zEGDd43a-o-D?>p3(*tOOjG9jP!iz*KD>>SkCntMn-!(k;FDmj`*G>)TzO`VGu1`?1 z*!S8*iKO#jy2)bjJ2Oj)o)QZquy&?sF3M4)x79D>jPZLeV`RVVl^XAOn}9>_TI|gv zNSmopDUO7jD0sun_ns4Jk2+F!GKp~F;hD{^RxRl96PChg*|Bh)C4pV~vtRQHZEmAE zlT~2K%`gw&onpd5Ot_d7!g>7j1-Cz1&FY3_uO`VDYRw!d%ZuL2UfMCKxAMH-I&^K%ogf#NcKv>E4(iFjq)n@fI9YZFb!=%| z>S0c2=>sSKq5XZ_)rw=wEMw$Y!@|Fr(+IzYyk-6nZd~&R>Cf*G-m!{{W#Gsk9}fBf zH!@Jhv%%-DU^+9&AHE=CbZikVoSpKecD7T;Q6MOTnLzpHtUK$z=Cfn0UTBL^ z43bDG^uF0p(k|c(KZrV?Dfs$qf3mbtP)!zv6RfUT;-1s~Auh{{T%5d9R3o7UqoP$o z@#bv3&-kIYu_ypguUNf?PJN@e{j=8?jWyV_gom3NNY^Ls^CH_GfP;*pTs%qH1&h@9fn2VFyE%2{) zlB#<}#pW)G3`Vph?p4%$d?#(?_2#l23G6>`9a50EHh`YG0Pli0*DY1J;)|JDAEBD3 zYNdrYTH?nHAbF=5pb=CcrBPrJWBs{$d?~C`L6h{$yf8FuJ2ad%yDQ=3gq8lJ%txSE zCX2!Z1+E&1U*yYS%>-QhPLl7oL5zp*z!{#pEGRo@krfdTPJ$O5jNY_(@odQF{cH~c{VrDDhe zc8zSc1N*{(7b(p=hMRy>ZLV1xrqjpoeibu+FmHmq=QE?{x7Yp12GeAWODgPa6sL{P6lt}c)r^oHAWhSO)YIW zP4qG>-6%&Z4acll%`7)yh(iGv@m^9)@l3*U9TQ&eU4)5Mbn)0{iZ5K8My!xC)S(&M z#eXmw{3Wb9elXkvq7<$Ds!u2@^dj=Df*z3|dSo)Jw%MJxnWJZWF{}8E2UEpq>#G29 z6LETZClA08KG~X5M_^!6sYyJZZ84YWUl4PEP_GY7&)}P(>P5uWVrkn-QUxQpcqxi= zHsha3yk3u}<39-T?Hp)3{PS4K3c2Kk?U94)fE+XksgiTv$e6UY1ENGpQR)m%6C5f%9-7r5IK&ApD8gS3tIT6Lkpl70iNlP}iQ4HZ zRii&TY&UBnM~wd48U~em^JiMt5=FB?E23w;-TDgvZDjJ+avLYQ9Kt+Rs-ysPN>`L@ z7qh`hgnTkljQCo8=#kH`a4FcL=QRQFdgBe&_GnO{^hOX=T00JgEL?$3Crm>bK`ZT!13=j}H1Lv6?DUb!VdzZ3^0r zC=H!4&3gGnAb%K2;31?YOsX9aLDD0?wfX*fxiydzUnIEAx+Zs??gTkt!jSI1i30+d zj~JWfcB>&+!if8TH& zOQyxh476pp!06V$;ZvuK_;&n)5>SltqkR>5B=4em=LBeN0XP_gb`Jb?E`sS7Iweq$ zg<^@3v>OhflVr*fg#+XQ1Ni69n`lnFN=a%^XsBYGtAT_0$medTY%r*6uKeJ6eD3HZari)t&^0+p z70FTSoEMN5a*^XThx4ed2WL#RXq-H6PtWONJXqK2Vg~V^09M*4Fmll7TNtFgT~Yya z$_@Y`hrK|5RfH5KxG3xF2@EgpKD0BKe#zwJC=Gmm`gn{+JJrhe#|WO!h6 z3gHKW(9g@mc6SJ*40#CTc|Wc);tf;rkoM9#2q!yA9q~y#ZVoKUEByX6Td}+rKZ-~s z6qi=2!Bj-iP3?R((Rv~bsgb0cx?y&_IJ)y1`bdX_aWt8wP%%Vo4Qz_K0}TVvceA|( z;U|8}jc|4sI~pq4?OPmXVUKE%yTzE5xsT}-TawEV$8DQyX`~VtWcG{9jG0$=0fA`R z73?_Z91zoFVNiivhC}f$LnG8yft{C}&?Li8NXLP{~k2+^HlcU8-kp5Pi3yP{R-~ zuPME3^v`M^j3Z%Z*bLK_iy;?Z4?xk)>14k)f050fpcqS_mh@6J4aLHr78tMuvN{VE!gB<90#6fCcbpiucT6|9Lp)YX8Jrx+W^(21-Ur`n9x@C|xzZT{>fr zz{vxKEA-4D-nu~|&GR`8ezD|Qju$~_QE3@x{^ion(W@d3YU}mu%5TmyyV|JELWgnG z8o0~lad&DcgrxbK91K$EMvS&`!9ng@gXh{BZO(mc&%VFdEq)}6H#WNg!aL@p|DV3x zrHj}7C3@H7!bMDeG^Of(Lce3gQ(2{HuD%&Y?522gXn+0ym&MgWE>M19j)iCZI_Ec~ zDAwT5YDi#Z>#p>2>hFM3cgDT@l<0X2f9Vp;uW<&%;{)Vr8HyMS?q_7iwpcU$K8+P}x4_ z<*~;;XsX!bJ}?;(H{!0+3!=j~k`azjEtHQA9oyAw5$U7b_|mT=+b-U6 zCEr&08`^wG2#W?{l==^*Qhy4Uy8(K^TAav;uA=N6uKxm6Wq$Jx%LzO?bT5u=6FbFd zB3OYhkjw)cV_MzO$CTEdgOl#0Uxw4{T5)W5T=0p{;*m*(HlY9|N5f-Tv(_`N+^D%h z=`Th%n2Wnls@|X?B%MrJlCo1v-1jvV9mY+7<9W7Xqu#v1t^=P*D6;qK{4eq0Kfr+p z&iF(WHC}Fm*6%OwXuPs$Owxm%J$oVyP&kVt$>sXbJQ- zr_)vdT=cfgYMueW>1SgUpNJuxfZb@c51k3I!~In`8j@B4Z=5YBxZZ5LTdUMspxyqXZHV+NE0a5>eJlp>uVq0E5I?FZ$4wv zFSH1OvbY;pXqnnVs0b%kgpD-jG%hnc8H_41gg+eCOsPX|S2BQ$|Hw#SmEjCtozzj5 z?{`-#08HpB_^8MlnUANYFY#56=7YQL#Nzc7rdwsp9;w(4QwR$Zyb@fAqdsrSckU49AB+Klv<43#-BqTcJIlGR{z;Ikh0(5j#mJloj+GMhTV@p>(p z;hkPbX=GXp(^MbU>rw+20EWO(uD7s*UH*1lwp+g@0j38Y-n6P&Ob>uZg2i-Y){icU zF3wh-(eAc~0`xR`ni{|imgn%scr4+-Ko2z}OdG0>^mZx3ETI}jls8BtWyu}pN+Z;?Yq8wcWu4bK_>Fk?MuA{BkBD?B>{=6GdtS8 zaKj!lK1aI|0PP|M6;S^^xI?DhT@2m5R;^LM5z!baAgNy`OK2ga{BBUBg(pu5qpu?f z5Lv{H+GZGJ_o8PYa7+(HHmgHQx)6&0ue@tJ zTm82*Ft2_2M8So||NRT&B?jw~?I!acjlgD4_8h`IV1|@ypgy>g$b4_wqS&_uwGw3!WrEuqJ zfy9gB{>T~?^cGOQRcinCZ;l=NxG;qStA+=-fZ>lqT*al1)S$(2Ko5G${;jmdX&{er z5Pg&IB`II_%L-%CFAm$iKLPq{L>sxp2B1?*M89-t^FFLo(F2wbSU2d72srJ3L1|9i zUSNP|Wr5;T#)h~X;*1SJ^qX*%{V3T3oFxs3nw7S%jM%Nm!W$V*6EOpsw8FnIlg0<3 z8z|7Yi@k}^-&3bw_NrlSZ{iZf(;Z+QRX%9zWu(W(XE~@~1fbi4kM68%jOn2n#0AmT z6+_`UoeTo-g@5+2or~$dJ^#I#N~om__VpB!HPX`2C*7%8DfUtLgs_D7RJ|Z2>i<~W zxyUuyD<3i_ALX^*8pCARSMrM@SVrd9p6KI;%mQv96~}i{0%@-!0ydE@SYP}!9qNa| z$eiz4uHX97(_t5VU>j!d2o~71oEux=t%C+_3Jk>ddi}PPWl!^52C)|b`J8PZOBkRI zzF}lTufMByqVnun5CM)RbUCPCSIG8xB})e3Zo0{e0#4^uGUb{ z@A46vnwj7ILE0og<>8kUK(5e9mNeYI8-QWzzCq^yy!8Cv*L zmkj94Z6KFIFV!Ub<7=Lrs)lNr6^cL~iT61rCl|25aGa8&8wI}tU6{~*AuE&pNWqGq zbJ(RB%b#|lpv4mO^4!5u$bF0ILB^Ssxh23Pg|R7}@dbA;_Le1tX;G`lg3YqzXPNFO zZz{8$H|tmL-+%OgFFrHxOx!4wi04r(<#0UgZs#IvX{1>Qezt(yLA{kV3LKe8=zJFS z-iB1nh+@ZSI`~wJ#`xuu*Jjff4LzV?D9n`#XZj1_%AolAxn*j~Lj>{55O3)0HhFd( zf2_Te|B>L8ta-X6-mQ0xk^&@IaRUdbff$3-nk}bBe`KIKXYMpxiADMMdr+bu|N1YFm-jZA0`x7B@byMr|nc6o9`rsZkX@edibTh)T0L)Dy&>k1qBwss?RJ6 z(#wY!!AP3%dSa#GHgB$sYPT?$;O z0IsF@>)F}H)x&oY=qOy;5YAaI%bFg?Kif<3`w8X#cDr94(6B74Lm-L_zEa4QU$!T& zAb=0bn08KY!Sjxf@dJ+K zo8c$pE{g;W>_-LayOy1CfK?xwd;!p3B&{c!8YSAK{UV%X089(zIT3f1|aGzweFyb zpMY)s8lePGO2@E4*ysLjN&qP8We_GN-{kpMWd{G_NjxD*Bm@<%zLKI=GrHNihR>9K9DH8u(0UtRvXy3|sCtYYJI~Qv&Hu z3-ZYGqd&D04lNA$Ari7i9|G=`itfC$Cb10}Q<$Q-%ik$#vyB;k=AaxxfTVQrN@CMw zdwy~S)24!Yh2E$`+eOZ)a(}Yjk2q^AdSzWxbGxU6V7M<6k)EDP zfh!%?iOrk_d?TO^Vks~)Z{a&xKDGO{`U?9H6@v{BA$U?#+Ena8w+w75PYUSz|2Ye; z#`eI-puZK7)OvrE%EjV2jjX9ueD0(f7Lf3O23lL)EZ33XAcX&=ABwfIk=8#N6=M5> zn#v6C@@Jf$COam}?3ZyTKYTQy@Uy#H%@1%{-xf{R7Z#gr=j7%l)tST!{-Xtm!RHdx z(d3Xux0w|OG*BGqW?!|jQFu1{8!m+y0Uk>be2iwNU9co42tq;(mChC^+8wBfF~F&S zKnH?Q&=U!3=*KieULussW`}FF=!)3RIq+u$MOX8d`{!o43`GDAk%*3Q9gv{h{^k=q z>XhVcgv=hjS3<_~L39*9iCEl{(gsTnykEO4Fw?v0|{8)3lNqx$(4{5@l>hjVQynnNm2Z5de;MrqyfgX^Mtf; z|Bd$~FTP9SzmgFP7NdxLEmma!*4hm5jVNul5z8Q`>e@Dz?i@v@S35>i9yp&Rf$Fy~ zyvgN_6}%uI|QU3&Ht*UNU=2QRnFH{?(&f88n)r@Pztfmi2-`J)iC z=y?%j-L%9mLLq?7=<_z3SL5t(AHTxLv{UvO5Lf(ONXuVi6A& z>4yL6X<}#p=qU<#`1HpH=;^2ug0cUvisv_&$fjKML;ZZ`opZ;BBZ4iBG?ZU`(c)oe z=646QrS%8;|1_4Kyqkh{FpH91cG9z3>dmlArF8q-IWEE}+`p~W3(DW;2X;WM{IPdg z8mIz)Qb|oBR()TNQSUVP%4pk5mGuXgz9mS^c`%07u65enBXF(JRWJqbe>5H;n_=+_ z6VgV-th%{4-x3s;)0Im3j84j>jy$;~OJu0^0T8{nw)`HTcCBZc!rV=c?7gqQWH+Qk zRjs^OA1ZsvsnU`p;x6`O_b2itOMKQuo29YwoXQKL!^N~*b);W)>hNk*h!l$6xLIzC zP;LFo-nLS_!>_^+&^ie6dJ^|!1j14;s;DCGcI#D@<@iZRcxc%jzm#;+? z0VHi}Kir?XF3zw7aInOr8V~|Af$=wyra1GuKlFR#e|2p{A_YW*&AQaUjS|!V7;<6Y z5I2hNhq}DVWSRj!K+EuS=TK+Uc|Sf>-9MS8TNZ(H5Nl~WZ8jc`jlv36ZPqf1 zsv_}(7riGwsn5C!0XTmEowRK@gJFP|u0k%HneRlc^7(8mKmk()eiTD|4xtBz{+ygB zb$S1ieyLvM?2bHD9mBZV#wC>Qyw$VOygbD$Ex7(84Ei+Dy8OwTZW(;Fl9IexBW2_m z!O-#b>9Y=2cf`P>8h%$$xLlI$s1GMNiAG0SU)To^U+qC$vQO4MqMX~*dH&fTkm{aF zIHk`|h2t|7r$@gY@C$zJeq0f4&z!=qR(7f(FJMqVyvdsakd9{8e&Bxnc>;Yg>WUJI zcGm0qJJL!MCQ_`x#`pP)U!cH$qgr?A6!rsqqeNuXb}=+6=m~gU zaptPcUWplT@3JQXW2U!;Bb0N#L+$}x+igd{2{iLF6VjaHY6MJCqr9)RaZsQhQH}}p zfCI<*omB~hu;Df&6!--(gld~eh!07Y!RdcncxNf&`-Aa|03R-!^YsgDa`I}QxZ)}z z4RkQ2v5k1o3s^t@T&=&@6evg%XaA8IP?*Rge(robTtPFLqFwR_{Ow~W^T|oIwps4G zG~FGAlx2dDmN*~=(WQ)L08OO36uSPxjQZZz#?-rn`QT?mg>@+5}flB+!>V z(6W_2EyoPuv-R+*86K}}WzV#zW9(N;Z6z=V!#UJX&5CJ77@uuKDZm9`lHRT!i)dAl zB9wRIe6KZZPpy?#eAh;Id9aW=_?%QiW4l&C2-J_8NQfbXVb+V}FgAse^T^?J|LpMz zFd^;&(kKyiJi#IW{4Q%#Kt|{wvR=L>v)}}bX{e3qCFnlsDVn{1?iU|qRT(w(H{TNyhh zfuf5K4oYN6W!2|%vJZ6Dg>+({Aq(+VN&<{3r#H0cxzh?blE^q!vVU8@QbT_1yixPU zpMhf*P$j^llTmjdY>nn6ougwvhKD!Qnsiczh8jqPP8Q}s{ol1ZhysGG3<6c6{jY)~ zt6xkEB1PWl^i~jo7WE{nJJ9Cspu7$j%TBmwRIdt#-~4-&>41>}X)YJr`!udhP(mAq6_D%R%%+%62U&REW4*zf)i8jk|%g4XFX&_j$9xNV-Uz`Aavwb4ZIfoy;aG z72DD^iDbS=fN7zsSDL?3o44XzgIHJ+;PJhuW;6M&77z%^vnR6A>VE02R8pNi;Lw%nfoBnMa(vf)Cn(_>tCs^0(?nkHb8+fo8msz_`@S^-z zn|XC1us9;UM~=&uRGf3Lsw387E9;|PK96ZAIq+mRJstU)5?vvAM|9bHc)EB*3J%Ip z{tHIX^SU)B-Odh84Jm)x`&BkRQ>v~^FfMgaM8yWssFDnlOR#Fo`P&&;i7|fust{ejFD4D3RS(phZw~l7{qYu^mCnGYqh{I|1Y6Lm3sz z#UT1h$j&>1yOnc%ozst^E{WY+N`>QaUo^Bx_NEI_<8s?Wr}<4t7@2$wR9&>>POjo^ z+DYD&Zz3775btAh?_nLmVI))$O|vg1E2SCYpHUt9dRY`NQNko2w6TJiSp_c-BlrWV zJJCR&p8gQY8hz~O#D29m2t7=SRx8rSSDg9?m&ndf0s3jvGun+t) z;EQSsjLA!4$Q};zPYjfWUabaxp%K4_%N&>exqXyG>EwZ`eBs4(snWitH`=Qb7PYtl z8w!I7woe#Ob&bjnkL8)db$009P{f2e1~_2yKmRFP7M^_WO@^vazY00Oceq*QnF@u@ z?-3`LN9{--mR)yRYTemgD^T^79p?DZ{XqRvPh8nF2vjC%Xl>=xZ6P+NaM&d%wfMcwFHKAlsEV zO=3VbOZk479;;2)|5qc&GK zG0eVis%9FsjMGS!VIV<_1fnUgt$SUqLzpSG7A9fWPU3y#^;IQAuMb5EX~C>XAey@! zyaSh}#WXnJAT?Np$3*Dz7#gHiqJ&K>0v3r=3&2u|7=Mvg2Xw}eH5(V!zrY7fsQxy- zxe%C-<)Swm$?-_IWnI<&Kht-<{h$ISx{3f75@ie=?C$~4MaBk71L5(ba0;F3nZxD` zlKbAR109=^-jDL-33*?&6M!hV6W?9#6BurP-O?EC&tBS@WDKzEoC<=~Xk;sqzJdB& z65CiHYyk}#Xf=dzyL?45w6@Tan@XbPSXhD(q#w;{T%(p}Z{N!X4sumxIBzoFSBv+x zp0YqmVIQ75;g&P{SNJLq7$4~wl)AM`aWxc0DGU*%l+JuXyI1vbn-B#>BA{c7Rx(V}I0LlwbZqXl z_32fsW8f$(6eKX*DILpm?oBii5H<;gZz4Ksu;ly{9#lM=AhzsM!DCSkl{t}|YJq4W zc6jIC>k06M9Viy<9mQn=x|G?9Vs%6OdX*xR#`-F|4dH9)sfM*g5U>rLH?6J)4=T`auoPHNPBN|lgN%b3KpV{Q+o?%oUwQ0N2N zU$NI!t1Sth8S1tIZsjs59Uyq_=+fS~tn@)sQKWL`5rWB0wJzkO-2*aToZ z9n39aKZMX}f#lP%!qCYJu(1zb2!3<@{sgmwY9qk@&IM7Jyn>5KCj6SZjda1l%a&7s z7bFVBk*Q|TnY-|%-~)mg;rrNs__9ohSh?W%&CFu`n9gG_H}#uK^${6pVbeB~lnnLv3HhpKg>l zDDmt@QgMkHeQfAd&spHG0h%jruSJ>LDh<{GTn@!q-*`MmDp~JWnm-0+HjxN=jsSF51-D56isr(oCtdi)>t;?UrODF^Au}B-4Y#@w--DoKkM(`=%ny{@zR5n!2 zv6@s%{bsA%2!9QSFQQywU$aozxlRMk|4_c~Y^B)(@L_$SumfLP}Tv_>47JjEXbp0o}mB-=qJ5!r(keb5Phmc2%N3aqRF zbS-q2WHuq`NG!^}0X+n;x;|)$WT81smFZKV_@1$seAJQTp@>nAF6B3|wLrk6c1CJ9 z#fbZ!7-nJ(H;G|(IQ!GoqsQy_MvVX%Fb?c0pgyf-ji+F1y-}EOW0Y3bN(DxSn>obt zXZq+mm6cd&$BBt;O792HR;X-EaXa0})acF^+BgtTlpyh(h~zYsTL;xt6aQWSG$#G( z-$oBWg@O*Z_1h%SL-FAk6+^UWTia+KB%aZ(-xv=3l9$?gbF_;;Y z))_gYym(XqLBHqSEdd++D59cUsZcNb63SquH5Ot-42eWe$d^pW2RoZfbgPV7_kS~F z8A?Ej5yE_3DN>_pEA2^<;Nu%-mF3wpV+8B+Xa`HSQ?sY=ldot}vu0LzzS-(p6@L z0}AJVbDO4sKUU-~l0#Rn5OzBsr+(e!pp^n3#pa@7rU}-Te{zIsL{o^vefOE}Z(1pU zHekQD!hNh4);s+~MX@_>2Kw$EEnpDVe^;FkUQ|Q`K#JlRA(9%fiQ^kvuc{(r&E{5Z z_Ht*buwH?a*!>NbTFg1SPQ7JmZCiguyT}RUHM%1l849O-PlJ)9McgB1{jn;5fY8F! z7nnao_aJ+hkW>2p9RZfqFZ!p}q$Ty`cdv#g^T45&QPMDz^wIROjI;>*K(4-6w>cZh=$%3qY`x}rfSK|#|iS? zkv-!wMMh-Plur`xzj4`xbI;hUe>i*m6QwH&RBP_P` zU&#H>`3`-F_+wR1!Ko&;%zSoCqYdS%uxNTpu~L$P{mg7BGEeKXl=$&=aXYgX-WM`D zdYqkdv2nH|I?VBTt#W;IZ%>Fv zBoI_BN%&}wngEumW6tPEF}D$eEXUVIvi{8bz|0cg`dVo$h9nf?pe0SI#%J&DboTdyso&-`RqkTr}L+~#G{dX{>UTm&)#1}MAt;e zL|)u*H$7|qE0U?ayH$*cg_%XEgOZ1pLIMd$8f?6Y>+lyZ{$=(9+p#{hbn{N0tZj3$ zc_P70Qxv@X8l_mx6<4ihLJO?&FeEBfJnB;sV8a`7y1cH=l(yLSSiguB!pxgkjZkg# zCY@qQ1V;@UiI2``F`z)10=0hPwz3a+v_EdZa34MT0nj=F5p!~e z?fWtnscV3*^p&yXW>}?yW>gIdl@`Yg4@#DZaBD~uo}>uI8~MNL0A4(Mlqu-|*g$83 z@KTs1*p7qZf7hyqa**-6wOc0Pr{F+&Ea&ctgsJ8iLNxD8fJwkCFbrgYYIXy_K$`4t zo#Urxsv zn9H=Rh0XGlXuxA}sNV3|Q|pQ7&?XY}rMAsdt|R&KZlgbIu$bH<=7H5KNuxu-55cVE zKHG8RJ-k>yUA8wom=cAUQ-tU7DiB&yfVPKD1_&%%1g7ng;K1s)T(8hoOg5$c zXkWE!*zo$^Ut|_Nd#vp`53GSM{P=Udi?Z+xfSTdp_PXW^0!^%gp1~CpcLZZ)xufu0 zu)dk{!O$qaW&(+$Xh;c8Y5k_Ww;I2rXa2I{EPpI~z2f${aqeBhW*%4LwbMA+j_+cl zYy*qAFqKX`r#}N!W&9&gNA06P%LuxFL-BmwAc|q9%-V8+r3GT;w2ah+v(dq(SGEqK(`>?Da8>c{S_>E$O5A=(=9^tdyYXG z!#wWS5BftU5Jx}M?WZp}qlh`pNH!^el+VeaM6S+!5a=WlqP9m zDE;X3vB)hF@HFd0@OW%*zvgy*eg#)qMU-=S3w-mbqy5(-2$KqO^#8PXp3!iAZKEfV zgot1W5(I;2VWNvfZ=*yPo#+w-(I$G2UL$G{qIY75j2bO^uZc2cCL3pK9Sl6{d@YFv0Y@mqOT*sEhQYMjnJv>`|cPOHv=_pY?OE$uz)T~4#xOjuMjgUibJh0&|=-j*jix(qn6)}V! z*Nt>eG45MurJ**Yyep+y_raNwuwvKL(OvGXuTj;9RF#!(9qaG z%pOs=FWyfXUzq$BwAfRy$Ld2in^`NYe8;BDI=g1nP~~r&pRJ^ll2wwv zb{Z1&=Q*Xvg*Z_9SsBOe`q20`7<)yn^*=7ie3cC&5%7rk))O|iW%Iae;jCd4v9$F5 z_2%|g{f$obtn07Sjz!13BPu%*`eY-ucaId&xx3tLia!h|gY>5>vHgaV!_t^7Hcum{ zjtn~VJZ|YuJ_)0I9#u-<#cL`l&|;^2BCBdi$0UAb3reDmfjYKl9dEaJDW@K+49>z+ z_(F$|mdeFt>It3vO_bU-{~9-FHp~In!x?YZ8vjs zL+lMCC;WHES!lq1I3`pB9L?hL$ouRMO{Il5W&CwFsf(YJF4NhK$?sy=oLn+W?v1yH z$XB`iZu|Z&dZ1z(-brwKbp{@woFORA*jQLG#dvYum8iXcnJX(~_cC=}B$mNT_}zKN zQ`)VVj=Xg0+3P1~b~mWsG{DHbS-b)t>Fvn*T$h-sm^ zOr0CcRBN=3tT757hi_;{myRb>DvsGo_J1OCJU#lRFu>}|_T8PncfAaXykPP9Mjp`K zr|Na)_oj`|G#!1WzH2@hoWHX^V1xI(z!7SeXSxba5G+AcI+txEoqfUF7--L1-}_l{ zT~?>w@y65vO)yz1bUg zrl9?xLbp|ZIe`d4?8t1BY#39A5m#ACmQWa~K2o`T3;z|79LndL>(!-mJ(KkzHpKa8 zqFVi`_D<1SK$#wY1Vx#qZnrfCy>_6_dM0mHoFRDot=JjPX5;;+8$M^DU&*e&!a#E` zrUm!paUcMTj{LI>i`<3uuLtG{`~g+7h`*TZ$4_XP!u^Djp16j`5_1 z`1k4q2dAVs_ZBTPrgWFIn1^k#ka~jXqJi!c(w!lS1l(A1d`F#SVJ-IHq-^bt1&1Lu zLw%$4^#o?s24xOIV+kI5rr}KSoZwoQH)QI$2>I}<7wjB!wGiRgGYtXn*~#d^X{thp z3wPh|5w>2GtD1N38Wp31@aZN;MWFt8Y|V3>?Qf@o+iBgu=V_9Dq=kB={i=uZT&H8_ zmb%y#v;QOf`MK_O^#!R?!_(+Y*{VK0RHsfpJB89sS%L(9Xrg)0XD~~$tc|7+7;^J* zpt6_TYNGpBrg)R0laJ=fLTi^xnrt+?B$yYamG(!4ckAjyt0)%ocpEdbyKo#^^?8i9 z3T#7SL}?FrTaNmEold>WH`NG4Fx({{`pwrSS-LK0&uBLP>UjMp*DqtGbK=#>`>TYS z&C&_=>O~4~nG`?pwO4TJ-UX+yjJhaDw-Q*NR(Bcv!SjoJW#g?+p(5L0FiA3fTyUti z+KG%rgz^q2YBnSqaHO(9(<|CSs(HpM(nC2tBE@jOw}yKxQ?$?OAzu(&fi-S#A9yWTxBj7=fvJ7?_0ftKOV>4vbnayo^x%H= z*0@XYmSe2rV1YW8RZ5LBj0wJz(Yb6s9bqoo%4F0zmXKqTTA^g2CtcstFuPjr^KzDy zm=qROw8<6!?r@;@S#rImofyIR87<~RapKR%l`F*`tjdyF!y6bhViiK+^;(unt;}st zeMG2oCtF7@g%UU`Y&R|2gf)GP3T(OS6X3`W>3V7WKL$uhfzpSEuWUnvMoVA*f@9Fj zAG>1&4s1f~&W>f-?%4=p{B11sRk{b$+>Z2;*L9-!?U`RBA?8bbtpy2MrG4W?JFWqE z5hO#TBN(P{pB4EO8%D^ZgmUokXCHi|hh>wF6YsX9umzkHomlYM&MJnJy$MCkk5_Vd zPc=wi(rBS#Y$LZeQ>+;zevkIF6`7R?_b$FortBw!G3{lgLk!JR;jIbZ{Kaxv)x?bk zG#U>T%iGQHri8YYhMGwPjM#u2KGD*?Pqg(5KODmz0zRf_yC8>rFKysT2Fq$*BxahEy<*T&N6MAz$^mHgbz66P ziGpu@gBGJ=BfRN89-7hUsZf=a{a%diwLeAf;n(?{O_WI=auqbryvw*8eTjiavO~XG zcadnaYUx-!vK@jQbO>+`B;!?#qQDH0suuB7CSs)eZ}$$gUTSdaIBQKSO7~279M=}U z9VF9FR2t-mt>xQspWe8v|2)J{R#?@uFf}a z4WNTs6$1l9>=@%-s&5SEPJFJ^l3t9xl3Fw3niv#+V?BkEJCp^=6^f{Qy(MIL_C=#n zEoHbz%`3~xXY*!DB>zK`qf8S!EL(>>zZF-%)7_v|=QG(}>Ah~mIRJ0&580%O+8~{ml8$x? zlk?W<2PdjntVd*Dm~~4#GTrY)5|LgtMRQ*CBsD^=cARZ9#i(eh_55IWVw8_-4Y^mA zpBMR1uUaibdH3|AM`7(tQLm{G+vSiFA2!RA-k(c|vwktw6fsK6hz;80PY_IX%torO zYyD0~hEGAfmFNgdUqsSVIZB^96;Mf3%&FPlJ9LoDgL`!*dO+n;1o-$i9s~S$2#FD= z!FL#nzRD*GlA{;*t~?!W;`@}zP(d<>B)v@xaUjMmHT(5Fp`514jUS!e9=KK zB_*Vi-ZpcKUkN6g6CQI|+!&An-KfQ9O0A#-*B*um)-&T4fmt-E4$&K?vozA-E#2?h z!>f%t@VsR$@e^=W2(_T7h0~R^eLGQ{6;T4jV_-T)_r+6_R4^?M{Bl$O&n-U+K&Vv@ zQsSqIv~OB}D_T&b9w{!4Hcrrx!G_uS`Ub?j_Y!%+1KQL1cSKIymQ$W-jn3DyPh8yR zT^Iep6ImvGL)AJ@U95}yzs0M0>a?1FsS>Tne*{i-kqkeI^Tl@6CH>AeA0_D4@30#s z7zpTUJ(DV5JRY20_KGD53}W>#?9%Ax@*%$0;_P)}pGg{vaLU#oKjfBPT66z#6FXefon%^n0*?V?x#&grSyJ# zaS*)OnIOV@yCgLrqu)^c#dz~EX4$Fp!yiMXdq0W(I9Xv)yxgUqAx-E`l#QT$wtG$` zxP70*zoV~pI8TLiD37u5`NJTv&Gsj$GE$wKDbM2-QuYsA{*z{>X03~lg{RSxd}T!$ zrzYF;WVP-KANo)v+|Y8H^0iZF*kX66eejWV@cyc%tW3`Z2zQJ92}IpvuiL|t8KxSy zYqq7vB^Dg4&(o|cqRQ?6xaseVrM^_n@gzw5hS0S#GN|*s!};x2%O^822KzqZN3|aM zahS;VT%6bayjt6tqBFrp&=Kt{G}FB!@W%XYKbs{(*fYFv z;S4vKbQ)c5r}b>|Ux zfM}xwLEhC*RF+uD;-@MxLJM*w`34d|GPAmnD?@dpYoqyr`N@=|unhn(%{OvhnV8Q; z>A|to-Pff(+Iivl2p-48J#|Erd!`(jZp;@=@d@xkg~hl; zu_m-*^NE`p(v=>hT{&&xkcQqbtc-`HGqqPYHK|zyV67amya;n=MLU-t>Xo_3GDP#w zlpm`*ggX2-pDAD`k!P~Jq-tj9;x=V&kLM_%tkyJpFg0*RlHKI}i?xMMAk``#SA_36qn0pX5V}+>JP_p@n zXe-q|7|H2($=GKWI6_^h5OyPYyPr?dme4m3na|MxdClD6Fd9+sn@aaXQ(W6tYK*&fsNf%dwzEx$_7G9jp1J$wc>eLb}r#|x%Yx(6tOmoQL-LW{l zw}wD|9-mfrj?Z+T8^~pTeQ|~it9uJi?s@2TdEQneQ3a0)Mi#I7gt_sNqh&fYWR@(@)fULX_!#$Yk?WBWzO?S)Y3Y= zKD<>H!(UbTc#S!kp@E&6(O*q0;VZXMC?sx_^kC|d&lQT2nf#gtCH$>IF$kvRu4R81 zWp#oBV>0`jQTtz4y+=LwU1{ace`5+LvsK7pLaQ;pf3RNjf@-iO36${WI_dp{NajA% z-LUoy1cfj@JOef(c&_-7>$4r*U$0`maywzxbc&QoLpxE&yq<}DT`_u)OHI_nu@{ap_E=G;McA_pN85Z0%`t{P9vjHT- zB{uqJC@0-TMzIi_c($wR;4T?9Bc%)1fe{fvA5oVT3v63yXQPeLT7}JP$({f=Bes(61 zg22p0n42((AM$vwOn$nXa_m!B!`4|3wNy&2X<@^(lv$9xvFwiUes$WuTD7cJ>F#96 z_r;6Xr`}FlI#gjPj1f95z7?-`XIOUMKe|%?6+E!Wj#XW%C00zGuw_uKpwe)$Q9(5( z+n6w4GP~a>?s0|Pwn=dk_m!iqOTQt0{VdfP8Xcjxk9MCIm+?N*EAeEp%~IAh=ypM>HnGYBAj*?Gh*3 z{Pb~JA2h}`2&dX&AzJKGoyEm7B#xa?Lit|omG^`RR6K9i3=C6@tL@IHnt?QPj&vWl@fEX9wd__0_UB-*o+80U5K&HwbU*15*9P z&k1H?A0+qzl2qS02%pd^aHdJ|}Rt_~Vw^olSHp9R#G zDg(tV9sSWkx=@KH#j?N`70HZKHmIA+h#R3{UkC+U&27bKeh3YDVU?Kzr80u@&;f6Vg`L^cl)!YXl zcPRKSwP1?<@9zP4XmQ_oZ<0Pi@dH0z|V2tenE&@$b9SJjGNyyh!2{x#59Y|u#L^r3W=#cyZ#AsBS}QvXbe zBIw10CyvNE}#G;Ljo>}lF&dsvL+E8Bx)mMp~p~QF}l=j0Vgs# z$scrHBhXJ4K@G5DhJt%a@d^`3f^SU-Wm)|R4{}R1aEGw5B~Q~tiFS;vtRe1Zs@r|> zJnw80?+a~I@}W!M{%XitLKiy2=R#uozElODZIx@yftB~lY)-<(5mMwG7MS3%y*zC< zUkuH?MgU~m*_mf_Ug5ve-8k)EGouv1zN9$!v#tBG3x%y+olc1dTGRuQCu^yb2%x9LxC|J4c?qU+Op5QT_-xOS&KU2K}CX6b8JCA%-zk_Pu4r* zMROMsm5{rz@}^E0vyDJO;>q2KFV>K)86RKNo%MyLDf?KxP;y;`vDUQ#HP%vOmNVh~ z=;3+Fa`4F2K2dTQ=eOIYOFbk%8=NRDY;I)f_!(S6V3L$2&H47fP0mq{dgb{b$Yux@ zhkn52uSTeqN=x);XEkjFbj)eg@B>cw`z#+X9p0FITFNfL>~UAj!iKfdvMU638fa}| zx;t=R4@KhP^Ro^=6v8a|0m45Ab^!y0)+qT;isjM-`~Vu8L4A0zZu*6-zVMwgR2pRT z-lfXGOPz0Fzg?`)X+EnYQY0k&dbaj_*fsS$3t@7IKPYMsOt2Vh^%|%-s{a}8?)K%bePdqVH)K6kLDxV#P0UQV zEaQ(yuwTXdyKDs75qynozkk>B@7VKx$1d3MYQUFc`f&hHj*E-^KuV(L#f$)X)J>aM zP8wv21WfB*xt*V3^iWApOz1_q^p$)LWw)YthLLUX^OK@M)2)>TO*W0WzWP%HWAA!x z2@gzK^{eB$TTcdgaa%apo#ln!Ne!sMsiwB)t9_;Elb2QaC2(vp%g(noKs~9ID;s^g zci(GIyI8Js@v+{Sym98u2Zc)UdRkqKx|N@E@4oBj4Mex?;bWX2A=Fr_Y>!lVn+~k9 zzr&XT!2qn%h>9;o30S3klYVRX+1JWX&*Furww}0+c6+5q?;HiyCw+Z!Cs+wz#xx-` zhI8*5`}BN11&m2wf(K@q-|wS+)IAnOTHn|@=g13?2M4T|(AtsPCW+y`^<0~utD8g| zeXQBH+WSy8tPfA;oY1XRFsqhXP^@2)F7GSP9JxnEn- z%nb>lTsj}QG(ICLP;Ve#H9K*(fAO)4TsIdlonb-B@oYp_Q~gSJtsgiN+)H@ahY>6% zic0ehC<4}6&+*Hi{3*}^GJQwDpkiDj$F~5<>*6E8%ILJn=hp!Sa#{k?k(M7TJVtMs za~T=h2Xw~rlW`d~gh92$hS5Q1Pe8p}4@b!5@F`ex2))0Vx+V;X`^xQtQ28|mj@YcY zd>7C92^?_?$}@i|3n>B(KAuL8Wi?d+vI5VRTyS4`PY)d*Du0&52fbRS*4UnBKB}8yy!{AmU7qOQc4JHt$GW0Ho7}gqPJ@*Sifdl9GM;B z%b-i6JKGfE8qwNapuVyxXxRwHD0M?0(xv zJfn@4uy|iO37P+d9p4<5}Wc8&r?n@43D7CLN>rjQA|$aMlCt71JdF!LBh1=Tb0{8UYlC3T`487ZLY1H|Wu|D{j>(zUtutyMAn5Be%6&`zTj}b5FSsBA5GMC@_*%_C{))II@&qXaB*S_C1>rY2OiN31 zj?ES$AZ#z&Ngkm8|DXmEAAp(Y{us^}aU~F@mTGsNgAY*rZ@x@tgUY1`x!4mHTu2BD zpaBeCX9p2LlNOSmjn-graka?+Cz_{?FdQ%LHI*VL7gwMrsX^N|SR&z6LH@pvMOaX> zi^I~})qg{8;sD-o{?synUQ_uef1HQ`7Yp2~lQGCen8b8e3D zYCo{(I}wAXw&uH@%LC|&!nMc#Aa*82uyuAhOqCOa-VXtCAyO4z&z~efaJQO!lx^YQ zV(RO^pj;ZLayb2Ja0QqX>h9F~SR8`d4`5v5yXH1q*f{|t$PTQ(n|rPVJN-6`)-`fK za7V1(uF4<=s9Pa-}*@4Hp0V z>pv~w{eLf~QS-o59Gt5z^3oET|IvuQSk7+SJjh%BVG!WW418{NIB&*Uzz&IBe-NO< zanAUcS^d+#dX$0TPcllM{_~T6y=nRlT*iba&VUy%{{Q-V^aQkO8BMYY{O@1BO(g@y zx`!)%1gd)fYVUu(Z`uH@VzDIpFJJlM*gZzJ= oNk2HIWg~d#zmqFH?kVJ0tY>_pgs5r~7YF>w%P32iO1|*_AI&f7i~s-t literal 0 HcmV?d00001 From 2f68197d37fd16e67377702eec8b2069bfe46f89 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 23 Jan 2024 16:38:05 +0100 Subject: [PATCH 55/97] Minor bugfix --- simulator/learned_model/include/pymodel_interface.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/simulator/learned_model/include/pymodel_interface.hpp b/simulator/learned_model/include/pymodel_interface.hpp index 6e57384aa4c9d..e1c7fe694d056 100644 --- a/simulator/learned_model/include/pymodel_interface.hpp +++ b/simulator/learned_model/include/pymodel_interface.hpp @@ -1,6 +1,8 @@ #ifndef LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ #define LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ +#include + class PymodelInterface { public: From cfd0b8b58716bc7166a264abda1e27666cee552b Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 23 Jan 2024 16:39:23 +0100 Subject: [PATCH 56/97] Remove explicit error-model. All sub-models should be properly enclosed. --- .../include/pymodel_base_error.hpp | 81 ------------------- .../include/pymodel_interconnected_model.hpp | 24 +++--- .../include/pymodel_simple_model.hpp | 6 +- .../vehicle_model/sim_model_pymodels.cpp | 35 ++------ 4 files changed, 20 insertions(+), 126 deletions(-) delete mode 100644 simulator/learned_model/include/pymodel_base_error.hpp diff --git a/simulator/learned_model/include/pymodel_base_error.hpp b/simulator/learned_model/include/pymodel_base_error.hpp deleted file mode 100644 index ef2e6116ea072..0000000000000 --- a/simulator/learned_model/include/pymodel_base_error.hpp +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef LEARNED_MODEL__SIM_PYMODEL_BASE_ERROR_HPP_ -#define LEARNED_MODEL__SIM_PYMODEL_BASE_ERROR_HPP_ - -#include "pymodel_simple_model.hpp" -#include "pymodel_interface.hpp" - - -class SimPymodelBaseError : public PymodelInterface -{ -private: - PymodelSimpleModel base_model; - PymodelSimpleModel error_model; - -public: - SimPymodelBaseError(std::tuple base_desc, std::tuple error_desc) - : base_model(std::get<0>(base_desc), std::get<1>(base_desc), std::get<2>(base_desc)), - error_model(std::get<0>(error_desc), std::get<1>(error_desc), std::get<2>(error_desc)) - { - } - - /** - * @brief create a map from model signal vector to python model inputs - * @param [in] signal_vec_names names of signals in model signal vector - */ - void mapInputs(std::vector signals_vec_names) override - { - base_model.mapInputs(signals_vec_names); - error_model.mapInputs(signals_vec_names); - } - - /** - * @brief create a map from python outputs to model signal vector - * @param [in] signal_vec_names names of signals in model signal vector - */ - void mapOutputs(std::vector signals_vec_names) override - { - base_model.mapOutputs(signals_vec_names); - error_model.mapOutputs(signals_vec_names); - } - - /** - * @brief get names of inputs of python model - */ - std::vector getInputNames() override - { - std::vector base_input_name = base_model.getInputNames(); - std::vector error_input_name = error_model.getInputNames(); - base_input_name.insert(base_input_name.end(), error_input_name.begin(), error_input_name.end()); - return base_input_name; - } - - /** - * @brief get names of states of python model - */ - std::vector getStateNames() override - { - std::vector base_state_name = base_model.getStateNames(); - std::vector error_state_name = error_model.getStateNames(); - base_state_name.insert(base_state_name.end(), error_state_name.begin(), error_state_name.end()); - return base_state_name; - } - - /** - * @brief calculate the next state of this submodule - * @param [in] model_signals_vec values of signals in model signal vector - * @param [in] model_signals_vec_next values of signals in model signal vector to update - */ - std::vector getNextState(std::vector model_signals_vec, std::vector model_signals_vec_next) override - { - std::vector base_next_state = base_model.getNextState(model_signals_vec, model_signals_vec_next); - std::vector base_next_state_delayed = base_model.signals_w_delayed_input; - std::vector next_state_error(model_signals_vec.size()); - std::fill(next_state_error.begin(), next_state_error.end(), 0.0); - std::vector error_correction = error_model.getNextState(base_next_state_delayed, next_state_error); // need to change input in all_variable input to delayed one from base model - for (size_t i = 0; i < base_next_state.size(); i++) base_next_state[i] += error_correction[i]; - return base_next_state; - } -}; - - -#endif // LEARNED_MODEL__SIM_PYMODEL_BASE_ERROR_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_interconnected_model.hpp b/simulator/learned_model/include/pymodel_interconnected_model.hpp index ac24181787802..14830ce36721e 100644 --- a/simulator/learned_model/include/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/pymodel_interconnected_model.hpp @@ -1,7 +1,7 @@ #ifndef LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ #define LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ -#include "pymodel_base_error.hpp" +// #include "pymodel_base_error.hpp" #include "pymodel_interface.hpp" #include "pymodel_simple_model.hpp" #include "model_connections_helpers.hpp" @@ -128,17 +128,17 @@ class InterconnectedModel submodels.push_back(std::unique_ptr(new_model)); } - /** - * @brief add a sub-model consisting of base + error sub-model - * @param [in] base_desc descriptor of base sub-model - * @param [in] error_desc descriptor of error sub-model - */ - void addSubmodelBaseError( - std::tuple base_desc, std::tuple error_desc) - { - submodels.push_back( - std::unique_ptr(new SimPymodelBaseError(base_desc, error_desc))); - } + // /** + // * @brief add a sub-model consisting of base + error sub-model + // * @param [in] base_desc descriptor of base sub-model + // * @param [in] error_desc descriptor of error sub-model + // */ + // void addSubmodelBaseError( + // std::tuple base_desc, std::tuple error_desc) + // { + // submodels.push_back( + // std::unique_ptr(new SimPymodelBaseError(base_desc, error_desc))); + // } /** * @brief set a new model state if it was changed using PSIM interface (mainly position and orientation) diff --git a/simulator/learned_model/include/pymodel_simple_model.hpp b/simulator/learned_model/include/pymodel_simple_model.hpp index 693cd12329292..1aebd6c455207 100644 --- a/simulator/learned_model/include/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/pymodel_simple_model.hpp @@ -30,8 +30,6 @@ class PymodelSimpleModel : public PymodelInterface std::vector pymodel_state_names; public: - std::vector signals_w_delayed_input; - /** * @brief constructor * @param [in] pymodel_import_name_ path to python model @@ -105,9 +103,7 @@ class PymodelSimpleModel : public PymodelInterface // map outputs from python model to required outputs std::vector next_state = fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); - - signals_w_delayed_input = fillVectorUsingMap(action_delayed, model_signals_vec_next, map_sig_vec_to_pyin, false); - + return next_state; } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 2fa8eb5766102..4fbd5bc91cf8b 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -44,47 +44,26 @@ SimModelPymodels::SimModelPymodels( (char*)nullptr, (char*)"KinematicBicycleSteerVel" }, - // { - // (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", - // (char*)"$HOME/f1tenth/ws_testing/base_model_save", - // (char*)"SimpleSteeringHyst" - // }, - { - (char*)"control_analysis_pipeline.model.base_model.base_model_simple_velocity", - (char*)nullptr, - (char*)"SimpleVelocity" - } - }; - - vehicle.addSubmodel(model_desc[0]); - vehicle.addSubmodel(model_desc[1]); - - - - - std::vector> error_model_desc = { { (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", (char*)"$HOME/autoware_model_params/base_model_save", (char*)"SimpleSteeringHyst" }, { - (char*)"control_analysis_pipeline.model.error_model.error_debug_model", + (char*)"control_analysis_pipeline.model.base_model.base_model_simple_velocity", (char*)nullptr, - (char*)"ErrorDebugModel" - }, + (char*)"SimpleVelocity" + } }; - // vehicle.AddSubmodel(model_desc[1]); - vehicle.addSubmodelBaseError(error_model_desc[0], error_model_desc[1]); - - - + vehicle.addSubmodel(model_desc[0]); + vehicle.addSubmodel(model_desc[1]); + vehicle.addSubmodel(model_desc[2]); vehicle.generateConnections(input_names, state_names); std::cout << dt << std::endl; - std::cout << "OKAAAAAYYYYYYY" << std::endl; + std::cout << "Python model loaded successfully " << std::endl; } double SimModelPymodels::getX() From 5693dbab1793cc3ae44508505350d5fd827c2c67 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 23 Jan 2024 18:16:01 +0100 Subject: [PATCH 57/97] Remove explicit error-model. All sub-models should be properly enclosed. --- .../include/pymodel_interconnected_model.hpp | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/simulator/learned_model/include/pymodel_interconnected_model.hpp b/simulator/learned_model/include/pymodel_interconnected_model.hpp index 14830ce36721e..505448729f403 100644 --- a/simulator/learned_model/include/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/pymodel_interconnected_model.hpp @@ -1,7 +1,6 @@ #ifndef LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ #define LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ -// #include "pymodel_base_error.hpp" #include "pymodel_interface.hpp" #include "pymodel_simple_model.hpp" #include "model_connections_helpers.hpp" @@ -128,18 +127,6 @@ class InterconnectedModel submodels.push_back(std::unique_ptr(new_model)); } - // /** - // * @brief add a sub-model consisting of base + error sub-model - // * @param [in] base_desc descriptor of base sub-model - // * @param [in] error_desc descriptor of error sub-model - // */ - // void addSubmodelBaseError( - // std::tuple base_desc, std::tuple error_desc) - // { - // submodels.push_back( - // std::unique_ptr(new SimPymodelBaseError(base_desc, error_desc))); - // } - /** * @brief set a new model state if it was changed using PSIM interface (mainly position and orientation) * @param [in] new_state new state set by PSIM From fe6685795338871fb2c0a39108bda9c42cd2d7a9 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 23 Jan 2024 18:16:46 +0100 Subject: [PATCH 58/97] Simplify pymodel interface --- simulator/learned_model/README.md | 10 +++---- .../include/pymodel_simple_model.hpp | 14 ++++------ .../vehicle_model/sim_model_pymodels.cpp | 27 ++++++++++++++----- 3 files changed, 30 insertions(+), 21 deletions(-) diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md index 16c2336a5f05c..57984a200468c 100644 --- a/simulator/learned_model/README.md +++ b/simulator/learned_model/README.md @@ -31,19 +31,17 @@ class CustomPythonSubmodel: def forward(self, action, state): # Required """ - Calculate forward pass through the model. - Currently implemented in the way that the next_state - needs to be 2d array with size (1 x NUM_OF_STATES). + Calculate forward pass through the model and returns next_state. """ - return list(list()) # next state + return list() - def get_sig_state_names(self): # Required + def get_state_names(self): # Required """ Return list of string names of the model states (outputs). """ return list() - def get_sig_action_names(self): # Required + def get_action_names(self): # Required """ Return list of string names of the model actions (inputs). """ diff --git a/simulator/learned_model/include/pymodel_simple_model.hpp b/simulator/learned_model/include/pymodel_simple_model.hpp index 1aebd6c455207..a0a49e0e0723e 100644 --- a/simulator/learned_model/include/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/pymodel_simple_model.hpp @@ -45,7 +45,7 @@ class PymodelSimpleModel : public PymodelInterface // Import python module py::module_ imported_module = py::module_::import(pymodel_import_name); // Initialize model class from imported module - py_model_class = imported_module.attr(py_class_name)(nullptr); + py_model_class = imported_module.attr(py_class_name)(); } else { @@ -65,13 +65,13 @@ class PymodelSimpleModel : public PymodelInterface // TODO warning that using default model params } - py::list pymodel_state_names_ = py_model_class.attr("get_sig_state_names")(); + py::list pymodel_state_names_ = py_model_class.attr("get_state_names")(); num_outputs_py = pymodel_state_names_.size(); for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++){ pymodel_state_names.push_back(PyBytes_AS_STRING(PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); } - py::list pymodel_input_names_ = py_model_class.attr("get_sig_action_names")(); + py::list pymodel_input_names_ = py_model_class.attr("get_action_names")(); num_inputs_py = pymodel_input_names_.size(); for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++){ pymodel_input_names.push_back(PyBytes_AS_STRING(PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); @@ -95,15 +95,11 @@ class PymodelSimpleModel : public PymodelInterface // forward pass through the base model py::tuple res = py_model_class.attr("forward")(py_inputs, py_state); - std::vector py_state_next = res[0].cast>>()[0]; - - // for (auto state : model_state) std::cout << "STATE AFTER : " << state << std::endl; - - std::vector action_delayed = res[1].cast>>()[0]; + std::vector py_state_next = res.cast>(); // map outputs from python model to required outputs std::vector next_state = fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); - + return next_state; } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 4fbd5bc91cf8b..5a615c61362c2 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -39,20 +39,35 @@ SimModelPymodels::SimModelPymodels( // TODO this should be in config file not hardcoded here // Think of a way how to differentiate between "simple" model and "base + error" model std::vector> model_desc = { + // { + // (char*)"control_analysis_pipeline.model.base_model.kinematic_bicycle_steer_vel", + // (char*)nullptr, + // (char*)"KinematicBicycleSteerVel" + // }, { - (char*)"control_analysis_pipeline.model.base_model.kinematic_bicycle_steer_vel", + (char*)"control_analysis_pipeline.autoware_models.vehicle.kinematic", (char*)nullptr, - (char*)"KinematicBicycleSteerVel" + (char*)"KinematicModel" }, + // { + // (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", + // (char*)"$HOME/autoware_model_params/base_model_save", + // (char*)"SimpleSteeringHyst" + // }, { - (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", + (char*)"control_analysis_pipeline.autoware_models.steering.example_base_error", (char*)"$HOME/autoware_model_params/base_model_save", - (char*)"SimpleSteeringHyst" + (char*)"BaseError" }, + // { + // (char*)"control_analysis_pipeline.model.base_model.base_model_simple_velocity", + // (char*)nullptr, + // (char*)"SimpleVelocity" + // } { - (char*)"control_analysis_pipeline.model.base_model.base_model_simple_velocity", + (char*)"control_analysis_pipeline.autoware_models.drive.drive_example", (char*)nullptr, - (char*)"SimpleVelocity" + (char*)"DriveExample" } }; From 42a241442beec6f8fb7b0a377aca024d567f47a7 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 23 Jan 2024 19:18:19 +0100 Subject: [PATCH 59/97] Fix formatting --- .../vehicle_model/sim_model_pymodels.cpp | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 5a615c61362c2..f3ce0ac444d19 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -39,31 +39,16 @@ SimModelPymodels::SimModelPymodels( // TODO this should be in config file not hardcoded here // Think of a way how to differentiate between "simple" model and "base + error" model std::vector> model_desc = { - // { - // (char*)"control_analysis_pipeline.model.base_model.kinematic_bicycle_steer_vel", - // (char*)nullptr, - // (char*)"KinematicBicycleSteerVel" - // }, { (char*)"control_analysis_pipeline.autoware_models.vehicle.kinematic", (char*)nullptr, (char*)"KinematicModel" }, - // { - // (char*)"control_analysis_pipeline.model.base_model.base_model_simple_steering_hysteresis", - // (char*)"$HOME/autoware_model_params/base_model_save", - // (char*)"SimpleSteeringHyst" - // }, { (char*)"control_analysis_pipeline.autoware_models.steering.example_base_error", (char*)"$HOME/autoware_model_params/base_model_save", (char*)"BaseError" }, - // { - // (char*)"control_analysis_pipeline.model.base_model.base_model_simple_velocity", - // (char*)nullptr, - // (char*)"SimpleVelocity" - // } { (char*)"control_analysis_pipeline.autoware_models.drive.drive_example", (char*)nullptr, From 97aacdf8327e028bdce191fc26a21ceedeaa92ff Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 24 Jan 2024 11:50:38 +0100 Subject: [PATCH 60/97] Fix formatting and add comments --- .../include/pymodel_interconnected_model.hpp | 30 +++++++++++++++++-- .../include/pymodel_simple_model.hpp | 4 ++- .../vehicle_model/sim_model_pymodels.hpp | 6 ++-- .../vehicle_model/sim_model_pymodels.cpp | 2 +- 4 files changed, 33 insertions(+), 9 deletions(-) diff --git a/simulator/learned_model/include/pymodel_interconnected_model.hpp b/simulator/learned_model/include/pymodel_interconnected_model.hpp index 505448729f403..7c1836685841a 100644 --- a/simulator/learned_model/include/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/pymodel_interconnected_model.hpp @@ -14,10 +14,10 @@ namespace py = pybind11; class InterconnectedModel { - int num_signals; - - std::vector model_signals_vec; + // Vector of unique names of inputs and outputs of sub-models std::vector signals_vec_names; + std::vector model_signals_vec; + int num_signals; std::vector> submodels; @@ -67,20 +67,33 @@ class InterconnectedModel return connection_map; } + /** + * @brief create a mapping between vector of signal input names from PSIM to vector of signals + * @param [in] in_names vector of signal input names from PSIM + */ void mapInputs(std::vector in_names) { // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index in "signals_vec_names" map_in_to_sig_vec = createConnectionsMap(signals_vec_names, in_names); } + /** + * @brief create a mapping between vector of signal output names from PSIM to vector of signals + * @param [in] out_names vector of signal output names from PSIM + */ void mapOutputs(std::vector out_names) { // index in "map_sig_vec_to_out" is index in "out_names" and value in "map_sig_vec_to_out" is index in "signals_vec_names" map_sig_vec_to_out = createConnectionsMap(signals_vec_names, out_names); } + /** + * @brief add unique names to the vector of signal names + * @param [in] names vector of signal names + */ void addNamesToSigVec(const std::vector & names) { + // Check if the name is already in the vector. If not add it. for (char * name : names) { if (std::find(signals_vec_names.begin(), signals_vec_names.end(), name) == signals_vec_names.end()) { signals_vec_names.push_back(name); @@ -88,6 +101,9 @@ class InterconnectedModel } } + /** + * @brief create of signal names from all submodels + */ void getSignalNames() { for (auto & submodel : submodels) { @@ -104,14 +120,19 @@ class InterconnectedModel */ void generateConnections(std::vector in_names, std::vector out_names) { + // Create vector of signal names getSignalNames(); num_signals = signals_vec_names.size(); + // Init vector of signal values for (int i = 0; i < num_signals; i++) model_signals_vec.push_back(0); + // For every sub-model create mapping from vector of signals to inputs and outputs for (auto & submodel : submodels) { submodel->mapInputs(signals_vec_names); submodel->mapOutputs(signals_vec_names); } + + // Create mapping from vector of signals to inputs and outputs of PSIM mapInputs(in_names); mapOutputs(out_names); } @@ -135,6 +156,7 @@ class InterconnectedModel { bool state_changed_externally = false; + // Check if some state was changed externally for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { if (abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > 1e-6) { state_changed_externally = true; @@ -143,8 +165,10 @@ class InterconnectedModel } if (state_changed_externally) { + // Reinitialize model std::cout << "Reseting model" << std::endl; + // Currently initializing model to zero -> TODO find a way how to initialize them to some other default values std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { diff --git a/simulator/learned_model/include/pymodel_simple_model.hpp b/simulator/learned_model/include/pymodel_simple_model.hpp index a0a49e0e0723e..d674876d2b2ba 100644 --- a/simulator/learned_model/include/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/pymodel_simple_model.hpp @@ -65,12 +65,14 @@ class PymodelSimpleModel : public PymodelInterface // TODO warning that using default model params } + // Get string names of states of python model, convert them to C++ string and store them in pymodel_state_names py::list pymodel_state_names_ = py_model_class.attr("get_state_names")(); num_outputs_py = pymodel_state_names_.size(); for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++){ pymodel_state_names.push_back(PyBytes_AS_STRING(PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); } + // Get string names of actions (inputs) of python model, convert them to C++ string and store them in pymodel_input_names py::list pymodel_input_names_ = py_model_class.attr("get_action_names")(); num_inputs_py = pymodel_input_names_.size(); for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++){ @@ -87,7 +89,7 @@ class PymodelSimpleModel : public PymodelInterface std::vector getNextState(std::vector model_signals_vec, std::vector model_signals_vec_next) override { - // get inputs and states of the python model from the vector of all signals + // get inputs and states of the python model from the vector of signals std::vector py_inputs(num_inputs_py); std::vector py_state(num_outputs_py); py_inputs = fillVectorUsingMap(py_inputs, model_signals_vec, map_sig_vec_to_pyin, true); diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index affdad9bf24cf..60f0ad9e1f938 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -106,11 +106,9 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ std::deque vx_input_queue_; //!< @brief buffer for velocity command std::deque steer_input_queue_; //!< @brief buffer for angular velocity command const double vx_delay_; //!< @brief time delay for velocity command [s] - const double vx_time_constant_; - //!< @brief time constant for 1D model of velocity dynamics + const double vx_time_constant_; //!< @brief time constant for 1D model of velocity dynamics const double steer_delay_; //!< @brief time delay for angular-velocity command [s] - const double - steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics + const double steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics const double steer_dead_band_; //!< @brief dead band for steering angle [rad] /** diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index f3ce0ac444d19..585a661777cf3 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -38,7 +38,7 @@ SimModelPymodels::SimModelPymodels( // TODO this should be in config file not hardcoded here // Think of a way how to differentiate between "simple" model and "base + error" model - std::vector> model_desc = { + std::vector> model_descriptors = { { (char*)"control_analysis_pipeline.autoware_models.vehicle.kinematic", (char*)nullptr, From 91ea6c1648a31c695504a882d0ec4d60d0132683 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 24 Jan 2024 12:54:42 +0100 Subject: [PATCH 61/97] Remove unnecessary model parameters --- .../vehicle_model/sim_model_pymodels.hpp | 28 +---------------- .../simple_planning_simulator_core.cpp | 4 +-- .../vehicle_model/sim_model_pymodels.cpp | 30 +++++-------------- 3 files changed, 10 insertions(+), 52 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index 60f0ad9e1f938..ded9550107381 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -37,23 +37,9 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ public: /** * @brief constructor - * @param [in] vx_lim velocity limit [m/s] - * @param [in] steer_lim steering limit [rad] - * @param [in] vx_rate_lim acceleration limit [m/ss] - * @param [in] steer_rate_lim steering angular velocity limit [rad/ss] - * @param [in] wheelbase vehicle wheelbase length [m] * @param [in] dt delta time information to set input buffer for delay - * @param [in] vx_delay time delay for velocity command [s] - * @param [in] vx_time_constant time constant for 1D model of velocity dynamics - * @param [in] steer_delay time delay for steering command [s] - * @param [in] steer_time_constant time constant for 1D model of steering dynamics - * @param [in] steer_dead_band dead band for steering angle [rad] - * @param [in] HELLO_FROM_CODING_GOD PRAISE THE MESSIAH!!!!! <3 */ - SimModelPymodels( - double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band); + SimModelPymodels(double dt); /** * @brief destructor @@ -61,7 +47,6 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ ~SimModelPymodels() = default; private: - const double MIN_TIME_CONSTANT; //!< @brief minimum time constant /* Specify string names for states and inputs. So we can automatically map states and @@ -93,23 +78,12 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ InterconnectedModel vehicle; // py::scoped_interpreter guard{}; - - const double vx_lim_; //!< @brief velocity limit - const double vx_rate_lim_; //!< @brief acceleration limit - const double steer_lim_; //!< @brief steering limit [rad] - const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] - const double wheelbase_; //!< @brief vehicle wheelbase length [m] double prev_vx_ = 0.0; double current_ax_ = 0.0; std::deque vx_input_queue_; //!< @brief buffer for velocity command std::deque steer_input_queue_; //!< @brief buffer for angular velocity command - const double vx_delay_; //!< @brief time delay for velocity command [s] - const double vx_time_constant_; //!< @brief time constant for 1D model of velocity dynamics - const double steer_delay_; //!< @brief time delay for angular-velocity command [s] - const double steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics - const double steer_dead_band_; //!< @brief dead band for steering angle [rad] /** * @brief set queue buffer for input command diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 41b60f15419e6..99823115c49bd 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -283,9 +283,7 @@ void SimplePlanningSimulator::initialize_vehicle_model() acceleration_map_path); } else if (vehicle_model_type_str == "PYMODELS"){ vehicle_model_type_ = VehicleModelType::PYMODELS; - vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, steer_dead_band); + vehicle_model_ptr_ = std::make_shared(timer_sampling_time_ms_ / 1000.0); }else{ throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 585a661777cf3..8efcdf94fec33 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -18,22 +18,8 @@ #include "pymodel_interconnected_model.hpp" -SimModelPymodels::SimModelPymodels( - double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band) -: SimModelInterface(5 /* dim x */, 2 /* dim u */), - MIN_TIME_CONSTANT(0.03), - vx_lim_(vx_lim), - vx_rate_lim_(vx_rate_lim), - steer_lim_(steer_lim), - steer_rate_lim_(steer_rate_lim), - wheelbase_(wheelbase), - vx_delay_(vx_delay), - vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), - steer_delay_(steer_delay), - steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), - steer_dead_band_(steer_dead_band) +SimModelPymodels::SimModelPymodels(double dt) +: SimModelInterface(5 /* dim x */, 2 /* dim u */) { // TODO this should be in config file not hardcoded here @@ -92,7 +78,7 @@ double SimModelPymodels::getAx() } double SimModelPymodels::getWz() { - return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / 1.5; } double SimModelPymodels::getSteer() { @@ -125,18 +111,18 @@ Eigen::VectorXd SimModelPymodels::calcModel( // Not used for this model -> we can get rid of this later auto sat = [](double val, double u, double l) { return std::max(std::min(val, u), l); }; - const double vx = sat(state(IDX::VX), vx_lim_, -vx_lim_); - const double steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_); + const double vx = sat(state(IDX::VX), 50, -50); + const double steer = sat(state(IDX::STEER), 0, -0); const double yaw = state(IDX::YAW); const double delay_input_vx = input(IDX_U::VX_DES); - const double delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); - const double vx_rate = sat(-(vx - delay_vx_des) / vx_time_constant_, vx_rate_lim_, -vx_rate_lim_); + const double delay_vx_des = sat(delay_input_vx, 50, -50); + const double vx_rate = sat(-(vx - delay_vx_des), 0, -0); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = 0 * vx * cos(yaw); d_state(IDX::Y) = 0 * vx * sin(yaw); - d_state(IDX::YAW) = 0 * vx * std::tan(steer) / wheelbase_; + d_state(IDX::YAW) = 0 * vx * std::tan(steer) / 1.5; d_state(IDX::VX) = 0 * vx_rate; d_state(IDX::STEER) = 0.0; //Use python models to update steer From 3797ce2193fb959898deb1b1988632ddea5a6152 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 24 Jan 2024 15:53:05 +0100 Subject: [PATCH 62/97] Remove unused functions --- .../vehicle_model/sim_model_pymodels.hpp | 4 ++-- .../vehicle_model/sim_model_pymodels.cpp | 24 ------------------- 2 files changed, 2 insertions(+), 26 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index ded9550107381..a0e8f5ae73c3d 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -138,11 +138,11 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ void update(const double & dt) override; /** - * @brief calculate derivative of states with delay steering model + * @brief not used for this model. calculate derivative of states with delay steering model * @param [in] state current model state * @param [in] input input vector to model */ - Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; + Eigen::VectorXd calcModel([[maybe_unused]]const Eigen::VectorXd & state, [[maybe_unused]]const Eigen::VectorXd & input) override {return Eigen::VectorXd::Zero(dim_x_);} }; #endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 8efcdf94fec33..2289730185b44 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -104,27 +104,3 @@ void SimModelPymodels::update(const double & dt) current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; prev_vx_ = input_(IDX_U::VX_DES); } - -Eigen::VectorXd SimModelPymodels::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - // Not used for this model -> we can get rid of this later - auto sat = [](double val, double u, double l) { return std::max(std::min(val, u), l); }; - - const double vx = sat(state(IDX::VX), 50, -50); - const double steer = sat(state(IDX::STEER), 0, -0); - const double yaw = state(IDX::YAW); - const double delay_input_vx = input(IDX_U::VX_DES); - const double delay_vx_des = sat(delay_input_vx, 50, -50); - const double vx_rate = sat(-(vx - delay_vx_des), 0, -0); - - - Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); - d_state(IDX::X) = 0 * vx * cos(yaw); - d_state(IDX::Y) = 0 * vx * sin(yaw); - d_state(IDX::YAW) = 0 * vx * std::tan(steer) / 1.5; - d_state(IDX::VX) = 0 * vx_rate; - d_state(IDX::STEER) = 0.0; //Use python models to update steer - - return d_state; -} From 85353cbbff7d0a6aa921b7a57405f8ef8da18e8e Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 24 Jan 2024 16:44:24 +0100 Subject: [PATCH 63/97] Minor bugfixes --- .../include/pymodel_interconnected_model.hpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/simulator/learned_model/include/pymodel_interconnected_model.hpp b/simulator/learned_model/include/pymodel_interconnected_model.hpp index 7c1836685841a..ba5959850bd0c 100644 --- a/simulator/learned_model/include/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/pymodel_interconnected_model.hpp @@ -102,10 +102,12 @@ class InterconnectedModel } /** - * @brief create of signal names from all submodels + * @brief create of signal names from all sub-models and PSIM signal names */ - void getSignalNames() + void getSignalNames(std::vector in_names, std::vector out_names) { + addNamesToSigVec(in_names); + addNamesToSigVec(out_names); for (auto & submodel : submodels) { addNamesToSigVec(submodel->getInputNames()); addNamesToSigVec(submodel->getStateNames()); @@ -121,7 +123,7 @@ class InterconnectedModel void generateConnections(std::vector in_names, std::vector out_names) { // Create vector of signal names - getSignalNames(); + getSignalNames(in_names, out_names); num_signals = signals_vec_names.size(); // Init vector of signal values for (int i = 0; i < num_signals; i++) model_signals_vec.push_back(0); @@ -189,7 +191,7 @@ class InterconnectedModel } // Compute forward pass through all models (order should not matter) - std::vector model_signals_vec_next(num_signals); + std::vector model_signals_vec_next = model_signals_vec; for (auto & submodel : submodels) { model_signals_vec_next = submodel->getNextState(model_signals_vec, model_signals_vec_next); } From 1c5d29665f15c47b0a04a1130d0ac098ed481f21 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 24 Jan 2024 17:20:16 +0100 Subject: [PATCH 64/97] Add vy and yaw rate to model states --- .../vehicle_model/sim_model_pymodels.hpp | 8 ++++++-- .../simple_planning_simulator_core.cpp | 8 ++++++-- .../vehicle_model/sim_model_pymodels.cpp | 6 +++--- 3 files changed, 15 insertions(+), 7 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index a0e8f5ae73c3d..f590f5be5e48e 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -55,8 +55,10 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ std::vector state_names = {(char*)"POS_X", (char*)"POS_Y", - (char*)"YAW", - (char*)"VX", + (char*)"YAW", + (char*)"YAW_RATE", + (char*)"VX", + (char*)"VY", (char*)"STEER" }; @@ -68,7 +70,9 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ X = 0, Y, YAW, + YAW_RATE, VX, + VY, STEER, }; enum IDX_U { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 99823115c49bd..a8a60dc2e7cf9 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -570,7 +570,9 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & const double x = pose.position.x; const double y = pose.position.y; const double yaw = tf2::getYaw(pose.orientation); + const double yaw_rate = 0.0; const double vx = twist.linear.x; + const double vy = 0.0; const double steer = 0.0; const double accx = 0.0; @@ -583,9 +585,11 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED) { state << x, y, yaw, vx; } else if ( // NOLINT - vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL || - vehicle_model_type_ == VehicleModelType::PYMODELS) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL) + { state << x, y, yaw, vx, steer; + } else if (vehicle_model_type_ == VehicleModelType::PYMODELS){ + state << x, y, yaw, yaw_rate, vx, vy, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC || vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED || diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 2289730185b44..7abb76c30b905 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -19,7 +19,7 @@ #include "pymodel_interconnected_model.hpp" SimModelPymodels::SimModelPymodels(double dt) -: SimModelInterface(5 /* dim x */, 2 /* dim u */) +: SimModelInterface(7 /* dim x */, 2 /* dim u */) { // TODO this should be in config file not hardcoded here @@ -70,7 +70,7 @@ double SimModelPymodels::getVx() } double SimModelPymodels::getVy() { - return 0.0; + return state_(IDX::VY); } double SimModelPymodels::getAx() { @@ -78,7 +78,7 @@ double SimModelPymodels::getAx() } double SimModelPymodels::getWz() { - return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / 1.5; + return state_(IDX::YAW_RATE); } double SimModelPymodels::getSteer() { From c9afcee4df00ca25dc64dd47262d0d8fd2013aab Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Wed, 24 Jan 2024 20:15:49 +0100 Subject: [PATCH 65/97] Add example to README and API doc (WIP) --- simulator/learned_model/README.md | 59 ++++++++++++++++++++++++++++++- 1 file changed, 58 insertions(+), 1 deletion(-) diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md index 57984a200468c..1605e330384a1 100644 --- a/simulator/learned_model/README.md +++ b/simulator/learned_model/README.md @@ -68,9 +68,66 @@ class CustomPythonSubmodel: -### Parameter description +To successfully create a vehicle model an InterconnectedModel class needs to be set up correctly. +### InterconnectedModel class +#### ```Constructor``` +Constructor takes no arguments. + +#### ```addSubmodel(model_desc)``` + +#### ```generateConnections(input_names, state_names)``` + +#### ```initState(current_state)``` + +#### ```updatePymodel(vehicle_input)``` + +### Example +Firstly we need to set up the model. +```C++ +InterconnectedModel vehicle; + +// Example of model descriptors +std::tuple model_desc_1 = { + (char*)"path_to_file_with_model_class_1", + (char*)nullptr, // If no param file is needed you can pass 'nullptr' + (char*)"ModelClass1" + }; + +std::tuple model_desc_2 = { + (char*)"path_to_file_with_model_class_2", + (char*)"/path_to/param_file", + (char*)"ModelClass2" // Name of the python class. Needs to use the interface from 'Assumptions' + } + +// Create sub-models based on descriptors +vehicle.addSubmodel(model_desc_1); +vehicle.addSubmodel(model_desc_2); + +// Define STATE and INPUT names of the system +std::vector state_names = {(char*)"STATE_NAME_1", (char*)"STATE_NAME_2"}; +std::vector input_names = {(char*)"INPUT_NAME_1", (char*)"INPUT_NAME_2"}; + +// Automatically connect sub-systems with model input +vehicle.generateConnections(input_names, state_names); +``` + +After the model is correctly set up, we can use it the following way. + +```C++ +// Example of an model input +std::vector vehicle_input = {0.0, 1.0}; // INPUT_NAME_1, INPUT_NAME_2 + +// Example of an model state +std::vector current_state = {0.2, 0.5}; // STATE_NAME_1, STATE_NAME_2 + +// Set model state +vehicle.initState(current_state); + +// Calculate the next state of the model +std::vector next_state = vehicle.updatePymodel(vehicle_input); +``` ## References / External links From 65954777169874d81531d5c56bf39dc0599e5a0a Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Fri, 26 Jan 2024 15:57:19 +0100 Subject: [PATCH 66/97] Fix bug in CMakeList --- simulator/learned_model/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/learned_model/CMakeLists.txt b/simulator/learned_model/CMakeLists.txt index ecee90775790a..e6461f17f56f3 100644 --- a/simulator/learned_model/CMakeLists.txt +++ b/simulator/learned_model/CMakeLists.txt @@ -8,7 +8,7 @@ find_package(Python3 COMPONENTS Interpreter Development) find_package(pybind11 CONFIG) ament_auto_add_library(${PROJECT_NAME} SHARED - src/pymodel_interconnected_model.cpp + DIRECTORY src ) target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES}) target_include_directories(${PROJECT_NAME} PRIVATE ${Python3_INCLUDE_DIRS}) From 7f9398d48ea3e1efbd4ec474094653a29d2b69d0 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Fri, 26 Jan 2024 15:58:47 +0100 Subject: [PATCH 67/97] Fix import bugs and fix formatting --- .../model_connections_helpers.hpp | 4 + .../pymodel_interconnected_model.hpp | 50 ++++--- .../learned_model/pymodel_interface.hpp | 42 ++++++ .../pymodel_simple_model.hpp | 129 +++++++++--------- .../include/model_connections_helpers.hpp | 5 - .../include/pymodel_interface.hpp | 41 ------ .../src/model_connections_helpers.cpp | 2 +- .../learned_model/src/pymodel_base_error.cpp | 1 - .../src/pymodel_interconnected_model.cpp | 2 +- .../learned_model/src/pymodel_interface.cpp | 2 +- .../src/pymodel_simple_model.cpp | 2 +- .../vehicle_model/sim_model_pymodels.hpp | 33 ++--- .../vehicle_model/sim_model_pymodels.cpp | 42 ++---- 13 files changed, 177 insertions(+), 178 deletions(-) create mode 100644 simulator/learned_model/include/learned_model/model_connections_helpers.hpp rename simulator/learned_model/include/{ => learned_model}/pymodel_interconnected_model.hpp (85%) create mode 100644 simulator/learned_model/include/learned_model/pymodel_interface.hpp rename simulator/learned_model/include/{ => learned_model}/pymodel_simple_model.hpp (55%) delete mode 100644 simulator/learned_model/include/model_connections_helpers.hpp delete mode 100644 simulator/learned_model/include/pymodel_interface.hpp delete mode 100644 simulator/learned_model/src/pymodel_base_error.cpp diff --git a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp new file mode 100644 index 0000000000000..d43a69266dc37 --- /dev/null +++ b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp @@ -0,0 +1,4 @@ +#ifndef LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ +#define LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ + +#endif // LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_interconnected_model.hpp b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp similarity index 85% rename from simulator/learned_model/include/pymodel_interconnected_model.hpp rename to simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp index ba5959850bd0c..8c153c30c7605 100644 --- a/simulator/learned_model/include/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp @@ -1,13 +1,14 @@ -#ifndef LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ -#define LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ +#ifndef LEARNED_MODEL__PYMODEL_INTERCONNECTED_MODEL_HPP_ +#define LEARNED_MODEL__PYMODEL_INTERCONNECTED_MODEL_HPP_ -#include "pymodel_interface.hpp" -#include "pymodel_simple_model.hpp" -#include "model_connections_helpers.hpp" +#include "learned_model/model_connections_helpers.hpp" +#include "learned_model/pymodel_interface.hpp" +#include "learned_model/pymodel_simple_model.hpp" #include #include #include + #include namespace py = pybind11; @@ -21,11 +22,13 @@ class InterconnectedModel std::vector> submodels; - // index in "map_in_to_sig_vec" is index in "py_inputs" and value in "map_in_to_sig_vec" is index in "all_variables_names" - std::vector map_in_to_sig_vec; + // index in "map_in_to_sig_vec" is index in "py_inputs" and value in "map_in_to_sig_vec" is index + // in "all_variables_names" + std::vector map_in_to_sig_vec; - // index in "map_sig_vec_to_out" is index in "pymodel_outputs" and value in "map_sig_vec_to_out" is index in "all_variables_names" - std::vector map_sig_vec_to_out; + // index in "map_sig_vec_to_out" is index in "pymodel_outputs" and value in "map_sig_vec_to_out" + // is index in "all_variables_names" + std::vector map_sig_vec_to_out; public: py::scoped_interpreter guard{}; // start the interpreter and keep it alive @@ -48,7 +51,8 @@ class InterconnectedModel } private: - std::vector createConnectionsMap(std::vector connection_names_1, std::vector connection_names_2) + std::vector createConnectionsMap( + std::vector connection_names_1, std::vector connection_names_2) { std::vector connection_map; // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is @@ -73,7 +77,8 @@ class InterconnectedModel */ void mapInputs(std::vector in_names) { - // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index in "signals_vec_names" + // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index + // in "signals_vec_names" map_in_to_sig_vec = createConnectionsMap(signals_vec_names, in_names); } @@ -83,7 +88,8 @@ class InterconnectedModel */ void mapOutputs(std::vector out_names) { - // index in "map_sig_vec_to_out" is index in "out_names" and value in "map_sig_vec_to_out" is index in "signals_vec_names" + // index in "map_sig_vec_to_out" is index in "out_names" and value in "map_sig_vec_to_out" is + // index in "signals_vec_names" map_sig_vec_to_out = createConnectionsMap(signals_vec_names, out_names); } @@ -95,14 +101,16 @@ class InterconnectedModel { // Check if the name is already in the vector. If not add it. for (char * name : names) { - if (std::find(signals_vec_names.begin(), signals_vec_names.end(), name) == signals_vec_names.end()) { + if ( + std::find(signals_vec_names.begin(), signals_vec_names.end(), name) == + signals_vec_names.end()) { signals_vec_names.push_back(name); } } } - + /** - * @brief create of signal names from all sub-models and PSIM signal names + * @brief create of signal names from all sub-models and PSIM signal names */ void getSignalNames(std::vector in_names, std::vector out_names) { @@ -151,7 +159,8 @@ class InterconnectedModel } /** - * @brief set a new model state if it was changed using PSIM interface (mainly position and orientation) + * @brief set a new model state if it was changed using PSIM interface (mainly position and + * orientation) * @param [in] new_state new state set by PSIM */ void initState(std::vector new_state) @@ -160,7 +169,9 @@ class InterconnectedModel // Check if some state was changed externally for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { - if (abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > 1e-6) { + if ( + abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > + 1e-6) { state_changed_externally = true; break; } @@ -170,7 +181,8 @@ class InterconnectedModel // Reinitialize model std::cout << "Reseting model" << std::endl; - // Currently initializing model to zero -> TODO find a way how to initialize them to some other default values + // Currently initializing model to zero -> TODO find a way how to initialize them to some + // other default values std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { @@ -209,4 +221,4 @@ class InterconnectedModel } }; -#endif // LEARNED_MODEL__SIM_PYMODEL_INTERCONNECTED_MODEL_ \ No newline at end of file +#endif // LEARNED_MODEL__PYMODEL_INTERCONNECTED_MODEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/learned_model/pymodel_interface.hpp b/simulator/learned_model/include/learned_model/pymodel_interface.hpp new file mode 100644 index 0000000000000..0f0df0ea5be95 --- /dev/null +++ b/simulator/learned_model/include/learned_model/pymodel_interface.hpp @@ -0,0 +1,42 @@ +#ifndef LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ +#define LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ + +#include + +class PymodelInterface +{ +public: + // virtual ~PymodelInterface() = 0; // <-- build fails after uncommenting this + + /** + * @brief get names of inputs of python model + */ + virtual std::vector getInputNames() = 0; + + /** + * @brief get names of states of python model + */ + virtual std::vector getStateNames() = 0; + + /** + * @brief create a map from model signal vector to python model inputs + * @param [in] signal_vec_names names of signals in model signal vector + */ + virtual void mapInputs(std::vector signals_vec_names) = 0; + + /** + * @brief create a map from python outputs to model signal vector + * @param [in] signal_vec_names names of signals in model signal vector + */ + virtual void mapOutputs(std::vector signals_vec_names) = 0; + + /** + * @brief calculate the next state of this submodule + * @param [in] model_signals_vec values of signals in model signal vector + * @param [in] model_signals_vec_next values of signals in model signal vector to update + */ + virtual std::vector getNextState( + std::vector model_signals_vec, std::vector model_signals_vec_next) = 0; +}; + +#endif // LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_simple_model.hpp b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp similarity index 55% rename from simulator/learned_model/include/pymodel_simple_model.hpp rename to simulator/learned_model/include/learned_model/pymodel_simple_model.hpp index d674876d2b2ba..3993cb44b066d 100644 --- a/simulator/learned_model/include/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp @@ -1,12 +1,12 @@ #ifndef LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ #define LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ +#include "learned_model/model_connections_helpers.hpp" +#include "learned_model/pymodel_interface.hpp" + #include #include -#include "pymodel_interface.hpp" -#include "model_connections_helpers.hpp" - namespace py = pybind11; /** @@ -16,18 +16,22 @@ namespace py = pybind11; class PymodelSimpleModel : public PymodelInterface { private: - char* pymodel_import_name = nullptr; - + char * pymodel_import_name = nullptr; + int num_inputs_py; int num_outputs_py; py::object py_model_class; - std::vector map_sig_vec_to_pyin; // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is index in "signals_vec_names" - std::vector map_pyout_to_sig_vec; // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in "map_pyout_to_sig_vec" is index in "signals_vec_names" + std::vector + map_sig_vec_to_pyin; // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in + // "map_sig_vec_to_pyin" is index in "signals_vec_names" + std::vector + map_pyout_to_sig_vec; // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and + // value in "map_pyout_to_sig_vec" is index in "signals_vec_names" - std::vector pymodel_input_names; - std::vector pymodel_state_names; + std::vector pymodel_input_names; + std::vector pymodel_state_names; public: /** @@ -36,47 +40,45 @@ class PymodelSimpleModel : public PymodelInterface * @param [in] param_file_path path to saved parameter file of the python sub-model * @param [in] py_class_name name of the python class */ - PymodelSimpleModel(char* pymodel_import_name_, char* param_file_path, char* py_class_name) + PymodelSimpleModel(char * pymodel_import_name_, char * param_file_path, char * py_class_name) { // Import model class pymodel_import_name = pymodel_import_name_; - if (pymodel_import_name != nullptr) - { - // Import python module - py::module_ imported_module = py::module_::import(pymodel_import_name); - // Initialize model class from imported module - py_model_class = imported_module.attr(py_class_name)(); - } - else - { - // TODO throw exception/error - return; + if (pymodel_import_name != nullptr) { + // Import python module + py::module_ imported_module = py::module_::import(pymodel_import_name); + // Initialize model class from imported module + py_model_class = imported_module.attr(py_class_name)(); + } else { + // TODO throw exception/error + return; } // Load model parameters and reset the model - if (param_file_path != nullptr) - { - py::object load_params_succ = py_model_class.attr("load_params")(param_file_path); - py_model_class.attr("reset")(); - std::cout << "Params loaded" << std::endl; - } - else - { - // TODO warning that using default model params + if (param_file_path != nullptr) { + py::object load_params_succ = py_model_class.attr("load_params")(param_file_path); + py_model_class.attr("reset")(); + std::cout << "Params loaded" << std::endl; + } else { + // TODO warning that using default model params } - - // Get string names of states of python model, convert them to C++ string and store them in pymodel_state_names + + // Get string names of states of python model, convert them to C++ string and store them in + // pymodel_state_names py::list pymodel_state_names_ = py_model_class.attr("get_state_names")(); num_outputs_py = pymodel_state_names_.size(); - for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++){ - pymodel_state_names.push_back(PyBytes_AS_STRING(PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); + for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++) { + pymodel_state_names.push_back(PyBytes_AS_STRING( + PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); } - // Get string names of actions (inputs) of python model, convert them to C++ string and store them in pymodel_input_names + // Get string names of actions (inputs) of python model, convert them to C++ string and store + // them in pymodel_input_names py::list pymodel_input_names_ = py_model_class.attr("get_action_names")(); num_inputs_py = pymodel_input_names_.size(); - for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++){ - pymodel_input_names.push_back(PyBytes_AS_STRING(PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); + for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++) { + pymodel_input_names.push_back(PyBytes_AS_STRING( + PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); } std::cout << "PymodelInterface import name: " << pymodel_import_name << std::endl; @@ -86,21 +88,22 @@ class PymodelSimpleModel : public PymodelInterface * @brief calculate the next state of a python model * @param [in] model_signals_vec all available inputs from PSIM */ - std::vector getNextState(std::vector model_signals_vec, std::vector model_signals_vec_next) override + std::vector getNextState( + std::vector model_signals_vec, std::vector model_signals_vec_next) override { - // get inputs and states of the python model from the vector of signals std::vector py_inputs(num_inputs_py); std::vector py_state(num_outputs_py); py_inputs = fillVectorUsingMap(py_inputs, model_signals_vec, map_sig_vec_to_pyin, true); py_state = fillVectorUsingMap(py_state, model_signals_vec, map_pyout_to_sig_vec, true); - + // forward pass through the base model py::tuple res = py_model_class.attr("forward")(py_inputs, py_state); std::vector py_state_next = res.cast>(); // map outputs from python model to required outputs - std::vector next_state = fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); + std::vector next_state = + fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); return next_state; } @@ -108,47 +111,47 @@ class PymodelSimpleModel : public PymodelInterface /** * @brief get names of inputs of python model */ - std::vector getInputNames() override - { - return pymodel_input_names; - } + std::vector getInputNames() override { return pymodel_input_names; } /** * @brief get names of states of python model */ - std::vector getStateNames() override - { - return pymodel_state_names; - } + std::vector getStateNames() override { return pymodel_state_names; } /** * @brief create a map from model signal vector to python model inputs * @param [in] signal_vec_names names of signals in model signal vector */ - void mapInputs(std::vector signals_vec_names) override + void mapInputs(std::vector signals_vec_names) override { - // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is index in "signals_vec_names" + // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is + // index in "signals_vec_names" map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); } - + /** * @brief create a map from python outputs to model signal vector * @param [in] signal_vec_names names of signals in model signal vector */ - void mapOutputs(std::vector signals_vec_names) override + void mapOutputs(std::vector signals_vec_names) override { - // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in "map_pyout_to_sig_vec" is index in "signals_vec_names" + // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in + // "map_pyout_to_sig_vec" is index in "signals_vec_names" map_pyout_to_sig_vec = createConnectionsMap(signals_vec_names, pymodel_state_names); } private: - std::vector createConnectionsMap(std::vector connection_names_1, std::vector connection_names_2){ + std::vector createConnectionsMap( + std::vector connection_names_1, std::vector connection_names_2) + { std::vector connection_map; - // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is index in "connection_names_1" - for (auto name_2 : connection_names_2){ + // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is + // index in "connection_names_1" + for (auto name_2 : connection_names_2) { int mapped_idx = -1; // -1 means that we cannot create a connection between some signals - for (size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++){ - if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0){ // 0 means strings are the same and we can create connection + for (size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { + if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same + // and we can create connection mapped_idx = NAME_1_IDX; break; } @@ -158,17 +161,17 @@ class PymodelSimpleModel : public PymodelInterface return connection_map; } - std::vector fillVectorUsingMap(std::vector vector1, std::vector vector2, std::vector map, bool inverse){ + std::vector fillVectorUsingMap( + std::vector vector1, std::vector vector2, std::vector map, bool inverse) + { // index in "map" is index in "vector1" and value in "map" is index in "vector2" // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 - for(size_t idx = 0; idx < map.size(); idx++) - { + for (size_t idx = 0; idx < map.size(); idx++) { if (map[idx] == -1) continue; inverse ? vector1[idx] = vector2[map[idx]] : vector2[map[idx]] = vector1[idx]; } return inverse ? vector1 : vector2; } - }; #endif // LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/model_connections_helpers.hpp b/simulator/learned_model/include/model_connections_helpers.hpp deleted file mode 100644 index 0acdc60333e20..0000000000000 --- a/simulator/learned_model/include/model_connections_helpers.hpp +++ /dev/null @@ -1,5 +0,0 @@ -#ifndef LEARNED_MODEL__MODEL_CONNECTION_HELPERS_ -#define LEARNED_MODEL__MODEL_CONNECTION_HELPERS_ - - -#endif // LEARNED_MODEL__MODEL_CONNECTION_HELPERS_ \ No newline at end of file diff --git a/simulator/learned_model/include/pymodel_interface.hpp b/simulator/learned_model/include/pymodel_interface.hpp deleted file mode 100644 index e1c7fe694d056..0000000000000 --- a/simulator/learned_model/include/pymodel_interface.hpp +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ -#define LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ - -#include - -class PymodelInterface - { - public: - // virtual ~PymodelInterface() = 0; // <-- build fails after uncommenting this - - /** - * @brief get names of inputs of python model - */ - virtual std::vector getInputNames() = 0; - - /** - * @brief get names of states of python model - */ - virtual std::vector getStateNames() = 0; - - /** - * @brief create a map from model signal vector to python model inputs - * @param [in] signal_vec_names names of signals in model signal vector - */ - virtual void mapInputs(std::vector signals_vec_names) = 0; - - /** - * @brief create a map from python outputs to model signal vector - * @param [in] signal_vec_names names of signals in model signal vector - */ - virtual void mapOutputs(std::vector signals_vec_names) = 0; - - /** - * @brief calculate the next state of this submodule - * @param [in] model_signals_vec values of signals in model signal vector - * @param [in] model_signals_vec_next values of signals in model signal vector to update - */ - virtual std::vector getNextState(std::vector model_signals_vec, std::vector model_signals_vec_next) = 0; -}; - -#endif // LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/src/model_connections_helpers.cpp b/simulator/learned_model/src/model_connections_helpers.cpp index 70349b80c9796..05b14720b8395 100644 --- a/simulator/learned_model/src/model_connections_helpers.cpp +++ b/simulator/learned_model/src/model_connections_helpers.cpp @@ -1 +1 @@ -#include "../include/model_connections_helpers.hpp" \ No newline at end of file +#include "learned_model/model_connections_helpers.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_base_error.cpp b/simulator/learned_model/src/pymodel_base_error.cpp deleted file mode 100644 index f829bd20bbe64..0000000000000 --- a/simulator/learned_model/src/pymodel_base_error.cpp +++ /dev/null @@ -1 +0,0 @@ -#include "../include/pymodel_base_error.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_interconnected_model.cpp b/simulator/learned_model/src/pymodel_interconnected_model.cpp index fac2a02ca5d68..00fdc4eae75e4 100644 --- a/simulator/learned_model/src/pymodel_interconnected_model.cpp +++ b/simulator/learned_model/src/pymodel_interconnected_model.cpp @@ -1 +1 @@ -#include "../include/pymodel_interconnected_model.hpp" \ No newline at end of file +#include "learned_model/pymodel_interconnected_model.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_interface.cpp b/simulator/learned_model/src/pymodel_interface.cpp index 302ef331fd00a..edd0c0d2483d6 100644 --- a/simulator/learned_model/src/pymodel_interface.cpp +++ b/simulator/learned_model/src/pymodel_interface.cpp @@ -1 +1 @@ -#include "../include/pymodel_interface.hpp" \ No newline at end of file +#include "learned_model/pymodel_interface.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_simple_model.cpp b/simulator/learned_model/src/pymodel_simple_model.cpp index f3a11c3a8aa7d..26f1b0963fa89 100644 --- a/simulator/learned_model/src/pymodel_simple_model.cpp +++ b/simulator/learned_model/src/pymodel_simple_model.cpp @@ -1 +1 @@ -#include "../include/pymodel_simple_model.hpp" \ No newline at end of file +#include "learned_model/pymodel_simple_model.hpp" \ No newline at end of file diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index f590f5be5e48e..d49f7f727c555 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -15,8 +15,8 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ +#include "learned_model/pymodel_interconnected_model.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include "pymodel_interconnected_model.hpp" #include #include @@ -47,24 +47,16 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ ~SimModelPymodels() = default; private: - - /* - Specify string names for states and inputs. So we can automatically map states and + /* + Specify string names for states and inputs. So we can automatically map states and inputs of this model to states and inputs of the python model. */ - std::vector state_names = {(char*)"POS_X", - (char*)"POS_Y", - (char*)"YAW", - (char*)"YAW_RATE", - (char*)"VX", - (char*)"VY", - (char*)"STEER" - }; + std::vector state_names = {(char *)"POS_X", (char *)"POS_Y", (char *)"YAW", + (char *)"YAW_RATE", (char *)"VX", (char *)"VY", + (char *)"STEER"}; - std::vector input_names = {(char*)"VX_DES", - (char*)"STEER_DES" - }; + std::vector input_names = {(char *)"VX_DES", (char *)"STEER_DES"}; enum IDX { X = 0, @@ -88,7 +80,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ std::deque vx_input_queue_; //!< @brief buffer for velocity command std::deque steer_input_queue_; //!< @brief buffer for angular velocity command - + /** * @brief set queue buffer for input command * @param [in] dt delta time @@ -146,7 +138,12 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ * @param [in] state current model state * @param [in] input input vector to model */ - Eigen::VectorXd calcModel([[maybe_unused]]const Eigen::VectorXd & state, [[maybe_unused]]const Eigen::VectorXd & input) override {return Eigen::VectorXd::Zero(dim_x_);} + Eigen::VectorXd calcModel( + [[maybe_unused]] const Eigen::VectorXd & state, + [[maybe_unused]] const Eigen::VectorXd & input) override + { + return Eigen::VectorXd::Zero(dim_x_); + } }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ \ No newline at end of file diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 7abb76c30b905..5be847b5767b2 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -14,37 +14,25 @@ #include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" -#include +#include "learned_model/pymodel_interconnected_model.hpp" -#include "pymodel_interconnected_model.hpp" +#include -SimModelPymodels::SimModelPymodels(double dt) -: SimModelInterface(7 /* dim x */, 2 /* dim u */) +SimModelPymodels::SimModelPymodels(double dt) : SimModelInterface(7 /* dim x */, 2 /* dim u */) { - // TODO this should be in config file not hardcoded here // Think of a way how to differentiate between "simple" model and "base + error" model - std::vector> model_descriptors = { - { - (char*)"control_analysis_pipeline.autoware_models.vehicle.kinematic", - (char*)nullptr, - (char*)"KinematicModel" - }, - { - (char*)"control_analysis_pipeline.autoware_models.steering.example_base_error", - (char*)"$HOME/autoware_model_params/base_model_save", - (char*)"BaseError" - }, - { - (char*)"control_analysis_pipeline.autoware_models.drive.drive_example", - (char*)nullptr, - (char*)"DriveExample" - } - }; - - vehicle.addSubmodel(model_desc[0]); - vehicle.addSubmodel(model_desc[1]); - vehicle.addSubmodel(model_desc[2]); + std::vector> model_descriptors = { + {(char *)"control_analysis_pipeline.autoware_models.vehicle.kinematic", (char *)nullptr, + (char *)"KinematicModel"}, + {(char *)"control_analysis_pipeline.autoware_models.steering.example_base_error", + (char *)"$HOME/autoware_model_params/base_model_save", (char *)"BaseError"}, + {(char *)"control_analysis_pipeline.autoware_models.drive.drive_example", (char *)nullptr, + (char *)"DriveExample"}}; + + vehicle.addSubmodel(model_descriptors[0]); + vehicle.addSubmodel(model_descriptors[1]); + vehicle.addSubmodel(model_descriptors[2]); vehicle.generateConnections(input_names, state_names); @@ -103,4 +91,4 @@ void SimModelPymodels::update(const double & dt) // Calculate current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; prev_vx_ = input_(IDX_U::VX_DES); -} +} \ No newline at end of file From 014a2ad4ab7a5e462bd7a10e5aaa877f75fbed08 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Sat, 27 Jan 2024 13:46:36 +0100 Subject: [PATCH 68/97] Add model descriptor to ros2 parameter file --- .../vehicle_model/sim_model_pymodels.hpp | 5 ++++- .../simple_planning_simulator_core.cpp | 9 ++++++++- .../vehicle_model/sim_model_pymodels.cpp | 6 +++++- 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index d49f7f727c555..f75669f54e5ff 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -39,7 +39,10 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ * @brief constructor * @param [in] dt delta time information to set input buffer for delay */ - SimModelPymodels(double dt); + SimModelPymodels(double dt, + std::vector model_python_paths, + std::vector model_param_paths, + std::vector model_class_names); /** * @brief destructor diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index a8a60dc2e7cf9..12f75e585cc12 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -238,6 +238,10 @@ void SimplePlanningSimulator::initialize_vehicle_model() const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; + std::vector model_python_paths = declare_parameter>("model_python_paths", std::vector({""})); + std::vector model_param_paths = declare_parameter>("model_param_paths", std::vector({""})); + std::vector model_class_names = declare_parameter>("model_class_names", std::vector({""})); + if (vehicle_model_type_str == "IDEAL_STEER_VEL") { vehicle_model_type_ = VehicleModelType::IDEAL_STEER_VEL; vehicle_model_ptr_ = std::make_shared(wheelbase); @@ -283,7 +287,10 @@ void SimplePlanningSimulator::initialize_vehicle_model() acceleration_map_path); } else if (vehicle_model_type_str == "PYMODELS"){ vehicle_model_type_ = VehicleModelType::PYMODELS; - vehicle_model_ptr_ = std::make_shared(timer_sampling_time_ms_ / 1000.0); + + vehicle_model_ptr_ = std::make_shared(timer_sampling_time_ms_ / 1000.0, + model_python_paths, model_param_paths, model_class_names); + }else{ throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 5be847b5767b2..092b7a74723d5 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -18,7 +18,11 @@ #include -SimModelPymodels::SimModelPymodels(double dt) : SimModelInterface(7 /* dim x */, 2 /* dim u */) +SimModelPymodels::SimModelPymodels(double dt, + [[maybe_unused]] std::vector model_python_paths, + [[maybe_unused]] std::vector model_param_paths, + [[maybe_unused]] std::vector model_class_names + ) : SimModelInterface(7 /* dim x */, 2 /* dim u */) { // TODO this should be in config file not hardcoded here // Think of a way how to differentiate between "simple" model and "base + error" model From 5a1b29901dd30924915bd96a3ec1e0f2301f0837 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Sat, 27 Jan 2024 13:47:30 +0100 Subject: [PATCH 69/97] Change char* to std::string in model descriptor --- .../pymodel_interconnected_model.hpp | 2 +- .../learned_model/pymodel_simple_model.hpp | 14 +++++++------- .../vehicle_model/sim_model_pymodels.cpp | 18 ++++++------------ 3 files changed, 14 insertions(+), 20 deletions(-) diff --git a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp index 8c153c30c7605..1565480af68be 100644 --- a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp @@ -151,7 +151,7 @@ class InterconnectedModel * @brief add a sub-model consisting of base + error model * @param [in] submodel_desc descriptor of the sub-model */ - void addSubmodel(std::tuple submodel_desc) + void addSubmodel(std::tuple submodel_desc) { const auto [lib_path, param_path, class_name] = submodel_desc; auto new_model = new PymodelSimpleModel(lib_path, param_path, class_name); diff --git a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp index 3993cb44b066d..eb682ddd1d809 100644 --- a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp @@ -16,7 +16,7 @@ namespace py = pybind11; class PymodelSimpleModel : public PymodelInterface { private: - char * pymodel_import_name = nullptr; + std::string pymodel_import_name; int num_inputs_py; int num_outputs_py; @@ -40,23 +40,23 @@ class PymodelSimpleModel : public PymodelInterface * @param [in] param_file_path path to saved parameter file of the python sub-model * @param [in] py_class_name name of the python class */ - PymodelSimpleModel(char * pymodel_import_name_, char * param_file_path, char * py_class_name) + PymodelSimpleModel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name) { // Import model class pymodel_import_name = pymodel_import_name_; - if (pymodel_import_name != nullptr) { + if (!pymodel_import_name.empty()) { // Import python module - py::module_ imported_module = py::module_::import(pymodel_import_name); + py::module_ imported_module = py::module_::import(pymodel_import_name.c_str()); // Initialize model class from imported module - py_model_class = imported_module.attr(py_class_name)(); + py_model_class = imported_module.attr(py_class_name.c_str())(); } else { // TODO throw exception/error return; } // Load model parameters and reset the model - if (param_file_path != nullptr) { - py::object load_params_succ = py_model_class.attr("load_params")(param_file_path); + if (!param_file_path.empty()) { + py::object load_params_succ = py_model_class.attr("load_params")(param_file_path.c_str()); py_model_class.attr("reset")(); std::cout << "Params loaded" << std::endl; } else { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 092b7a74723d5..ff8e578da3bc7 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -24,19 +24,13 @@ SimModelPymodels::SimModelPymodels(double dt, [[maybe_unused]] std::vector model_class_names ) : SimModelInterface(7 /* dim x */, 2 /* dim u */) { - // TODO this should be in config file not hardcoded here - // Think of a way how to differentiate between "simple" model and "base + error" model - std::vector> model_descriptors = { - {(char *)"control_analysis_pipeline.autoware_models.vehicle.kinematic", (char *)nullptr, - (char *)"KinematicModel"}, - {(char *)"control_analysis_pipeline.autoware_models.steering.example_base_error", - (char *)"$HOME/autoware_model_params/base_model_save", (char *)"BaseError"}, - {(char *)"control_analysis_pipeline.autoware_models.drive.drive_example", (char *)nullptr, - (char *)"DriveExample"}}; - vehicle.addSubmodel(model_descriptors[0]); - vehicle.addSubmodel(model_descriptors[1]); - vehicle.addSubmodel(model_descriptors[2]); + for (size_t i = 0; i < model_python_paths.size(); i++){ + std::tuple descriptor = { + model_python_paths[i], model_param_paths[i], model_class_names[i] + }; + vehicle.addSubmodel(descriptor); + } vehicle.generateConnections(input_names, state_names); From 14eaafb66870c98b8d1aab5db73296ab24b02c12 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Sat, 27 Jan 2024 13:47:57 +0100 Subject: [PATCH 70/97] Minor code cleanup --- .../vehicle_model/sim_model_pymodels.hpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp index f75669f54e5ff..04a5329ff696f 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp @@ -76,7 +76,6 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ }; InterconnectedModel vehicle; - // py::scoped_interpreter guard{}; double prev_vx_ = 0.0; double current_ax_ = 0.0; @@ -84,12 +83,6 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ std::deque vx_input_queue_; //!< @brief buffer for velocity command std::deque steer_input_queue_; //!< @brief buffer for angular velocity command - /** - * @brief set queue buffer for input command - * @param [in] dt delta time - */ - void initializeInputQueue(const double & dt); - /** * @brief get vehicle position x */ From 741743a8cd6f9f0ac52950c786952891eb24b879 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 29 Jan 2024 13:40:20 +0100 Subject: [PATCH 71/97] Minor code cleanup --- .../vehicle_model/sim_model_pymodels.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index ff8e578da3bc7..51753722b85cf 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -19,12 +19,11 @@ #include SimModelPymodels::SimModelPymodels(double dt, - [[maybe_unused]] std::vector model_python_paths, - [[maybe_unused]] std::vector model_param_paths, - [[maybe_unused]] std::vector model_class_names + std::vector model_python_paths, + std::vector model_param_paths, + std::vector model_class_names ) : SimModelInterface(7 /* dim x */, 2 /* dim u */) { - for (size_t i = 0; i < model_python_paths.size(); i++){ std::tuple descriptor = { model_python_paths[i], model_param_paths[i], model_class_names[i] From be5305584fb6e52348202a893a300828167b18ec Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 29 Jan 2024 13:42:07 +0100 Subject: [PATCH 72/97] Add method to set time stamp from C++ --- simulator/learned_model/README.md | 13 +++++++++++++ .../learned_model/pymodel_interconnected_model.hpp | 11 +++++++++++ .../include/learned_model/pymodel_interface.hpp | 8 +++++++- .../include/learned_model/pymodel_simple_model.hpp | 6 ++++++ .../vehicle_model/sim_model_pymodels.cpp | 3 ++- 5 files changed, 39 insertions(+), 2 deletions(-) diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md index 1605e330384a1..3ffb4a2d8c254 100644 --- a/simulator/learned_model/README.md +++ b/simulator/learned_model/README.md @@ -60,6 +60,14 @@ class CustomPythonSubmodel: - path: Path to a parameter file to load by the model. """ pass + + def dtSet(self, dt): + """ + Set dt of the model. + Inputs: + - dt: time step + """ + pass ``` ## API @@ -83,6 +91,8 @@ Constructor takes no arguments. #### ```updatePymodel(vehicle_input)``` +#### ```dtSet(dt)``` + ### Example Firstly we need to set up the model. ```C++ @@ -111,6 +121,9 @@ std::vector input_names = {(char*)"INPUT_NAME_1", (char*)"INPUT_NAME_2"}; // Automatically connect sub-systems with model input vehicle.generateConnections(input_names, state_names); + +// Set the time step of the model +vehicle.dtSet(dt); ``` After the model is correctly set up, we can use it the following way. diff --git a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp index 1565480af68be..f98de77432f41 100644 --- a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp @@ -191,6 +191,17 @@ class InterconnectedModel } } + /** + * @brief set time step for all the models + * @param [in] dt time step + */ + void dtSet(double dt) + { + for (auto & submodel : submodels) { + submodel->dtSet(dt); + } + } + /** * @brief compute next step of the PSIM model using python sub-models * @param [in] psim_input vector of input values provided by PSIM diff --git a/simulator/learned_model/include/learned_model/pymodel_interface.hpp b/simulator/learned_model/include/learned_model/pymodel_interface.hpp index 0f0df0ea5be95..8f92bbd94d60f 100644 --- a/simulator/learned_model/include/learned_model/pymodel_interface.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_interface.hpp @@ -6,7 +6,13 @@ class PymodelInterface { public: - // virtual ~PymodelInterface() = 0; // <-- build fails after uncommenting this + // virtual ~PymodelInterface() = 0; + + /** + * @brief set time step of the model + * @param [in] dt time step + */ + virtual void dtSet(double dt) = 0; /** * @brief get names of inputs of python model diff --git a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp index eb682ddd1d809..280d0cc95325a 100644 --- a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp @@ -108,6 +108,12 @@ class PymodelSimpleModel : public PymodelInterface return next_state; } + /** + * @brief set time step of the model + * @param [in] dt time step + */ + void dtSet(double dt) override { py_model_class.attr("dtSet")(dt); } + /** * @brief get names of inputs of python model */ diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp index 51753722b85cf..e0e2f577cee83 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp @@ -33,7 +33,8 @@ SimModelPymodels::SimModelPymodels(double dt, vehicle.generateConnections(input_names, state_names); - std::cout << dt << std::endl; + vehicle.dtSet(dt); + std::cout << "Python model loaded successfully " << std::endl; } From dd38d72289856e5e73376e89e7ddb83699f27ad8 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 29 Jan 2024 16:09:07 +0100 Subject: [PATCH 73/97] Update README --- simulator/learned_model/README.md | 73 ++++++++++++++++++++++++------- 1 file changed, 56 insertions(+), 17 deletions(-) diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md index 3ffb4a2d8c254..0e1b9e9299388 100644 --- a/simulator/learned_model/README.md +++ b/simulator/learned_model/README.md @@ -27,7 +27,7 @@ Using this Python model interface a PSIM model consisting of sub-models implemen To use this package `python3` and `pybind11` need to be installed. The only assumption on Python sub-models is their interface (see below). ```python -class CustomPythonSubmodel: +class PythonSubmodelInterface: def forward(self, action, state): # Required """ @@ -53,7 +53,7 @@ class CustomPythonSubmodel: """ pass - def load_params(self, path): # Optional + def load_params(self, path): # Required """ Load parameters of the model. Inputs: @@ -61,7 +61,7 @@ class CustomPythonSubmodel: """ pass - def dtSet(self, dt): + def dtSet(self, dt): # Required """ Set dt of the model. Inputs: @@ -83,15 +83,54 @@ To successfully create a vehicle model an InterconnectedModel class needs to be #### ```Constructor``` Constructor takes no arguments. -#### ```addSubmodel(model_desc)``` +#### ```void addSubmodel(std::tuple model_descriptor)``` +Add a new sub-model to the model. -#### ```generateConnections(input_names, state_names)``` +Inputs: +* model_descriptor: Describes what model should be used. Model descriptor contains three strings: + * First string is a path to a file where the model is implemented. + * Second string is a path to the file where model parameters are stored + * Third string is a name of the class that implement the model. -#### ```initState(current_state)``` +Outputs: +* None -#### ```updatePymodel(vehicle_input)``` +#### ```void generateConnections(std::vector in_names, std::vector out_names)``` +Generate connections between sub-models and inputs/outputs of the model. -#### ```dtSet(dt)``` +Inputs: +* in_names: String names for all of the model inputs in order. +* out_names: String names for all of the model outputs in order. + +Outputs: +* None + +#### ```void initState(std::vector new_state)``` +Set the initial state of the model. + +Inputs: +* new_state: New state of the model. + +Outputs: +* None + +#### ```std::vector updatePymodel(std::vector psim_input)``` +Calculate the next state of the model by calculating the next state of all of the sub-models. + +Inputs: +* psim_input: Input to the model. + +Outputs: +* next_state: Next state of the model. + +#### ```dtSet(double dt)``` +Set the time step of the model. + +Inputs: +* dt: time step + +Outputs: +* None ### Example Firstly we need to set up the model. @@ -99,21 +138,21 @@ Firstly we need to set up the model. InterconnectedModel vehicle; // Example of model descriptors -std::tuple model_desc_1 = { +std::tuple model_descriptor_1 = { (char*)"path_to_file_with_model_class_1", (char*)nullptr, // If no param file is needed you can pass 'nullptr' (char*)"ModelClass1" - }; + }; -std::tuple model_desc_2 = { - (char*)"path_to_file_with_model_class_2", - (char*)"/path_to/param_file", - (char*)"ModelClass2" // Name of the python class. Needs to use the interface from 'Assumptions' - } +std::tuple model_descriptor_2 = { + (char*)"path_to_file_with_model_class_2", + (char*)"/path_to/param_file", + (char*)"ModelClass2" // Name of the python class. Needs to use the interface from 'Assumptions' + }; // Create sub-models based on descriptors -vehicle.addSubmodel(model_desc_1); -vehicle.addSubmodel(model_desc_2); +vehicle.addSubmodel(model_descriptor_1); +vehicle.addSubmodel(model_descriptor_2); // Define STATE and INPUT names of the system std::vector state_names = {(char*)"STATE_NAME_1", (char*)"STATE_NAME_2"}; From 767f045c797f89cb33ac43a033916af4c9cccfab Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 30 Jan 2024 14:22:45 +0100 Subject: [PATCH 74/97] Split implementation between .hpp and .cpp files --- .../model_connections_helpers.hpp | 35 +++++ .../pymodel_interconnected_model.hpp | 142 ++---------------- .../learned_model/pymodel_simple_model.hpp | 115 +------------- .../src/pymodel_interconnected_model.cpp | 125 ++++++++++++++- .../src/pymodel_simple_model.cpp | 86 ++++++++++- 5 files changed, 260 insertions(+), 243 deletions(-) diff --git a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp index d43a69266dc37..264311e0bc29f 100644 --- a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp +++ b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp @@ -1,4 +1,39 @@ #ifndef LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ #define LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ +#include +#include + +std::vector fillVectorUsingMap( + std::vector vector1, std::vector vector2, std::vector map, bool inverse) + { + // index in "map" is index in "vector1" and value in "map" is index in "vector2" + // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 + for (std::size_t idx = 0; idx < map.size(); idx++) { + if (map[idx] == -1) continue; + inverse ? vector1[idx] = vector2[map[idx]] : vector2[map[idx]] = vector1[idx]; + } + return inverse ? vector1 : vector2; + } + +std::vector createConnectionsMap( + std::vector connection_names_1, std::vector connection_names_2) + { + std::vector connection_map; + // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is + // index in "connection_names_1" + for (auto name_2 : connection_names_2) { + int mapped_idx = -1; // -1 means that we cannot create a connection between some signals + for (std::size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { + if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same + // and we can create connection + mapped_idx = NAME_1_IDX; + break; + } + } + connection_map.push_back(mapped_idx); + } + return connection_map; + } + #endif // LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp index f98de77432f41..e9f34dbe33aa7 100644 --- a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp @@ -51,76 +51,28 @@ class InterconnectedModel } private: - std::vector createConnectionsMap( - std::vector connection_names_1, std::vector connection_names_2) - { - std::vector connection_map; - // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is - // index in "connection_names_1" - for (auto name_2 : connection_names_2) { - int mapped_idx = -1; // -1 means that we cannot create a connection between some signals - for (size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { - if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same - // and we can create connection - mapped_idx = NAME_1_IDX; - break; - } - } - connection_map.push_back(mapped_idx); - } - return connection_map; - } - /** * @brief create a mapping between vector of signal input names from PSIM to vector of signals * @param [in] in_names vector of signal input names from PSIM */ - void mapInputs(std::vector in_names) - { - // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index - // in "signals_vec_names" - map_in_to_sig_vec = createConnectionsMap(signals_vec_names, in_names); - } + void mapInputs(std::vector in_names); /** * @brief create a mapping between vector of signal output names from PSIM to vector of signals * @param [in] out_names vector of signal output names from PSIM */ - void mapOutputs(std::vector out_names) - { - // index in "map_sig_vec_to_out" is index in "out_names" and value in "map_sig_vec_to_out" is - // index in "signals_vec_names" - map_sig_vec_to_out = createConnectionsMap(signals_vec_names, out_names); - } + void mapOutputs(std::vector out_names); /** * @brief add unique names to the vector of signal names * @param [in] names vector of signal names */ - void addNamesToSigVec(const std::vector & names) - { - // Check if the name is already in the vector. If not add it. - for (char * name : names) { - if ( - std::find(signals_vec_names.begin(), signals_vec_names.end(), name) == - signals_vec_names.end()) { - signals_vec_names.push_back(name); - } - } - } + void addNamesToSigVec(const std::vector & names); /** * @brief create of signal names from all sub-models and PSIM signal names */ - void getSignalNames(std::vector in_names, std::vector out_names) - { - addNamesToSigVec(in_names); - addNamesToSigVec(out_names); - for (auto & submodel : submodels) { - addNamesToSigVec(submodel->getInputNames()); - addNamesToSigVec(submodel->getStateNames()); - } - } + void getSignalNames(std::vector in_names, std::vector out_names); public: /** @@ -128,108 +80,32 @@ class InterconnectedModel * @param [in] in_names string names of inputs available from PSIM * @param [in] out_names string names of outputs required by PSIM */ - void generateConnections(std::vector in_names, std::vector out_names) - { - // Create vector of signal names - getSignalNames(in_names, out_names); - num_signals = signals_vec_names.size(); - // Init vector of signal values - for (int i = 0; i < num_signals; i++) model_signals_vec.push_back(0); - - // For every sub-model create mapping from vector of signals to inputs and outputs - for (auto & submodel : submodels) { - submodel->mapInputs(signals_vec_names); - submodel->mapOutputs(signals_vec_names); - } - - // Create mapping from vector of signals to inputs and outputs of PSIM - mapInputs(in_names); - mapOutputs(out_names); - } + void generateConnections(std::vector in_names, std::vector out_names); /** * @brief add a sub-model consisting of base + error model * @param [in] submodel_desc descriptor of the sub-model */ - void addSubmodel(std::tuple submodel_desc) - { - const auto [lib_path, param_path, class_name] = submodel_desc; - auto new_model = new PymodelSimpleModel(lib_path, param_path, class_name); - submodels.push_back(std::unique_ptr(new_model)); - } + void addSubmodel(std::tuple submodel_desc); /** * @brief set a new model state if it was changed using PSIM interface (mainly position and * orientation) * @param [in] new_state new state set by PSIM */ - void initState(std::vector new_state) - { - bool state_changed_externally = false; - - // Check if some state was changed externally - for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { - if ( - abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > - 1e-6) { - state_changed_externally = true; - break; - } - } - - if (state_changed_externally) { - // Reinitialize model - std::cout << "Reseting model" << std::endl; - - // Currently initializing model to zero -> TODO find a way how to initialize them to some - // other default values - std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); - - for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { - model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] = new_state[PSIM_STATE_IDX]; - } - } - } + void initState(std::vector new_state); /** * @brief set time step for all the models * @param [in] dt time step */ - void dtSet(double dt) - { - for (auto & submodel : submodels) { - submodel->dtSet(dt); - } - } + void dtSet(double dt); /** * @brief compute next step of the PSIM model using python sub-models * @param [in] psim_input vector of input values provided by PSIM */ - std::vector updatePymodel(std::vector psim_input) - { - // map input to vector of all variables - for (size_t PSIM_INPUT_IDX = 0; PSIM_INPUT_IDX < psim_input.size(); PSIM_INPUT_IDX++) { - model_signals_vec[map_in_to_sig_vec[PSIM_INPUT_IDX]] = psim_input[PSIM_INPUT_IDX]; - } - - // Compute forward pass through all models (order should not matter) - std::vector model_signals_vec_next = model_signals_vec; - for (auto & submodel : submodels) { - model_signals_vec_next = submodel->getNextState(model_signals_vec, model_signals_vec_next); - } - - // Map vector of all variables to - std::vector psim_next_state; - for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < map_sig_vec_to_out.size(); PSIM_STATE_IDX++) { - psim_next_state.push_back(model_signals_vec_next[map_sig_vec_to_out[PSIM_STATE_IDX]]); - } - - // Update vector of all variables - model_signals_vec = model_signals_vec_next; - - return psim_next_state; - } + std::vector updatePymodel(std::vector psim_input); }; #endif // LEARNED_MODEL__PYMODEL_INTERCONNECTED_MODEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp index 280d0cc95325a..fb16cc4121a6e 100644 --- a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp @@ -40,144 +40,43 @@ class PymodelSimpleModel : public PymodelInterface * @param [in] param_file_path path to saved parameter file of the python sub-model * @param [in] py_class_name name of the python class */ - PymodelSimpleModel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name) - { - // Import model class - pymodel_import_name = pymodel_import_name_; - if (!pymodel_import_name.empty()) { - // Import python module - py::module_ imported_module = py::module_::import(pymodel_import_name.c_str()); - // Initialize model class from imported module - py_model_class = imported_module.attr(py_class_name.c_str())(); - } else { - // TODO throw exception/error - return; - } - - // Load model parameters and reset the model - if (!param_file_path.empty()) { - py::object load_params_succ = py_model_class.attr("load_params")(param_file_path.c_str()); - py_model_class.attr("reset")(); - std::cout << "Params loaded" << std::endl; - } else { - // TODO warning that using default model params - } - - // Get string names of states of python model, convert them to C++ string and store them in - // pymodel_state_names - py::list pymodel_state_names_ = py_model_class.attr("get_state_names")(); - num_outputs_py = pymodel_state_names_.size(); - for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++) { - pymodel_state_names.push_back(PyBytes_AS_STRING( - PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); - } - - // Get string names of actions (inputs) of python model, convert them to C++ string and store - // them in pymodel_input_names - py::list pymodel_input_names_ = py_model_class.attr("get_action_names")(); - num_inputs_py = pymodel_input_names_.size(); - for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++) { - pymodel_input_names.push_back(PyBytes_AS_STRING( - PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); - } - - std::cout << "PymodelInterface import name: " << pymodel_import_name << std::endl; - } + PymodelSimpleModel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name); /** * @brief calculate the next state of a python model * @param [in] model_signals_vec all available inputs from PSIM */ std::vector getNextState( - std::vector model_signals_vec, std::vector model_signals_vec_next) override - { - // get inputs and states of the python model from the vector of signals - std::vector py_inputs(num_inputs_py); - std::vector py_state(num_outputs_py); - py_inputs = fillVectorUsingMap(py_inputs, model_signals_vec, map_sig_vec_to_pyin, true); - py_state = fillVectorUsingMap(py_state, model_signals_vec, map_pyout_to_sig_vec, true); - - // forward pass through the base model - py::tuple res = py_model_class.attr("forward")(py_inputs, py_state); - std::vector py_state_next = res.cast>(); - - // map outputs from python model to required outputs - std::vector next_state = - fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); - - return next_state; - } + std::vector model_signals_vec, std::vector model_signals_vec_next) override; /** * @brief set time step of the model * @param [in] dt time step */ - void dtSet(double dt) override { py_model_class.attr("dtSet")(dt); } + void dtSet(double dt) override; /** * @brief get names of inputs of python model */ - std::vector getInputNames() override { return pymodel_input_names; } + std::vector getInputNames() override; /** * @brief get names of states of python model */ - std::vector getStateNames() override { return pymodel_state_names; } + std::vector getStateNames() override; /** * @brief create a map from model signal vector to python model inputs * @param [in] signal_vec_names names of signals in model signal vector */ - void mapInputs(std::vector signals_vec_names) override - { - // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is - // index in "signals_vec_names" - map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); - } + void mapInputs(std::vector signals_vec_names) override; /** * @brief create a map from python outputs to model signal vector * @param [in] signal_vec_names names of signals in model signal vector */ - void mapOutputs(std::vector signals_vec_names) override - { - // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in - // "map_pyout_to_sig_vec" is index in "signals_vec_names" - map_pyout_to_sig_vec = createConnectionsMap(signals_vec_names, pymodel_state_names); - } + void mapOutputs(std::vector signals_vec_names) override; -private: - std::vector createConnectionsMap( - std::vector connection_names_1, std::vector connection_names_2) - { - std::vector connection_map; - // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is - // index in "connection_names_1" - for (auto name_2 : connection_names_2) { - int mapped_idx = -1; // -1 means that we cannot create a connection between some signals - for (size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { - if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same - // and we can create connection - mapped_idx = NAME_1_IDX; - break; - } - } - connection_map.push_back(mapped_idx); - } - return connection_map; - } - - std::vector fillVectorUsingMap( - std::vector vector1, std::vector vector2, std::vector map, bool inverse) - { - // index in "map" is index in "vector1" and value in "map" is index in "vector2" - // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 - for (size_t idx = 0; idx < map.size(); idx++) { - if (map[idx] == -1) continue; - inverse ? vector1[idx] = vector2[map[idx]] : vector2[map[idx]] = vector1[idx]; - } - return inverse ? vector1 : vector2; - } }; #endif // LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_interconnected_model.cpp b/simulator/learned_model/src/pymodel_interconnected_model.cpp index 00fdc4eae75e4..2a08e1411d800 100644 --- a/simulator/learned_model/src/pymodel_interconnected_model.cpp +++ b/simulator/learned_model/src/pymodel_interconnected_model.cpp @@ -1 +1,124 @@ -#include "learned_model/pymodel_interconnected_model.hpp" \ No newline at end of file +#include "learned_model/pymodel_interconnected_model.hpp" + + +void InterconnectedModel::mapInputs(std::vector in_names) +{ + // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index + // in "signals_vec_names" + map_in_to_sig_vec = createConnectionsMap(signals_vec_names, in_names); + } + +void InterconnectedModel::mapOutputs(std::vector out_names) +{ + // index in "map_sig_vec_to_out" is index in "out_names" and value in "map_sig_vec_to_out" is + // index in "signals_vec_names" + map_sig_vec_to_out = createConnectionsMap(signals_vec_names, out_names); +} + +void InterconnectedModel::addNamesToSigVec(const std::vector & names) +{ + // Check if the name is already in the vector. If not add it. + for (char * name : names) { + if ( + std::find(signals_vec_names.begin(), signals_vec_names.end(), name) == + signals_vec_names.end()) { + signals_vec_names.push_back(name); + } + } +} + +void InterconnectedModel::getSignalNames(std::vector in_names, std::vector out_names) +{ + addNamesToSigVec(in_names); + addNamesToSigVec(out_names); + for (auto & submodel : submodels) { + addNamesToSigVec(submodel->getInputNames()); + addNamesToSigVec(submodel->getStateNames()); + } +} + +void InterconnectedModel::generateConnections(std::vector in_names, std::vector out_names) +{ + // Create vector of signal names + getSignalNames(in_names, out_names); + num_signals = signals_vec_names.size(); + // Init vector of signal values + for (int i = 0; i < num_signals; i++) model_signals_vec.push_back(0); + + // For every sub-model create mapping from vector of signals to inputs and outputs + for (auto & submodel : submodels) { + submodel->mapInputs(signals_vec_names); + submodel->mapOutputs(signals_vec_names); + } + + // Create mapping from vector of signals to inputs and outputs of PSIM + mapInputs(in_names); + mapOutputs(out_names); +} + +void InterconnectedModel::addSubmodel(std::tuple submodel_desc) +{ + const auto [lib_path, param_path, class_name] = submodel_desc; + auto new_model = new PymodelSimpleModel(lib_path, param_path, class_name); + submodels.push_back(std::unique_ptr(new_model)); +} + +void InterconnectedModel::initState(std::vector new_state) +{ + bool state_changed_externally = false; + + // Check if some state was changed externally + for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { + if ( + abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > + 1e-6) { + state_changed_externally = true; + break; + } + } + + if (state_changed_externally) { + // Reinitialize model + std::cout << "Reseting model" << std::endl; + + // Currently initializing model to zero -> TODO find a way how to initialize them to some + // other default values + std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); + + for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { + model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] = new_state[PSIM_STATE_IDX]; + } + } +} + +void InterconnectedModel::dtSet(double dt) +{ + for (auto & submodel : submodels) { + submodel->dtSet(dt); + } +} + +std::vector InterconnectedModel::updatePymodel(std::vector psim_input) +{ + // map input to vector of all variables + for (size_t PSIM_INPUT_IDX = 0; PSIM_INPUT_IDX < psim_input.size(); PSIM_INPUT_IDX++) { + model_signals_vec[map_in_to_sig_vec[PSIM_INPUT_IDX]] = psim_input[PSIM_INPUT_IDX]; + } + + // Compute forward pass through all models (order should not matter) + std::vector model_signals_vec_next = model_signals_vec; + for (auto & submodel : submodels) { + model_signals_vec_next = submodel->getNextState(model_signals_vec, model_signals_vec_next); + } + + // Map vector of all variables to + std::vector psim_next_state; + for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < map_sig_vec_to_out.size(); PSIM_STATE_IDX++) { + psim_next_state.push_back(model_signals_vec_next[map_sig_vec_to_out[PSIM_STATE_IDX]]); + } + + // Update vector of all variables + model_signals_vec = model_signals_vec_next; + + return psim_next_state; +} \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_simple_model.cpp b/simulator/learned_model/src/pymodel_simple_model.cpp index 26f1b0963fa89..6bd86c1a53a1b 100644 --- a/simulator/learned_model/src/pymodel_simple_model.cpp +++ b/simulator/learned_model/src/pymodel_simple_model.cpp @@ -1 +1,85 @@ -#include "learned_model/pymodel_simple_model.hpp" \ No newline at end of file +#include "learned_model/pymodel_simple_model.hpp" + +#include + +#include +#include + +namespace py = pybind11; + +PymodelSimpleModel::PymodelSimpleModel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name) +{ +// Import model class +pymodel_import_name = pymodel_import_name_; +if (!pymodel_import_name.empty()) { + // Import python module + py::module_ imported_module = py::module_::import(pymodel_import_name.c_str()); + // Initialize model class from imported module + py_model_class = imported_module.attr(py_class_name.c_str())(); +} else { + // TODO throw exception/error + return; +} + +// Load model parameters and reset the model +if (!param_file_path.empty()) { + py::object load_params_succ = py_model_class.attr("load_params")(param_file_path.c_str()); + py_model_class.attr("reset")(); + std::cout << "Params loaded" << std::endl; +} else { + // TODO warning that using default model params +} + +// Get string names of states of python model, convert them to C++ string and store them in +// pymodel_state_names +py::list pymodel_state_names_ = py_model_class.attr("get_state_names")(); +num_outputs_py = pymodel_state_names_.size(); +for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++) { + pymodel_state_names.push_back(PyBytes_AS_STRING( + PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); +} + +// Get string names of actions (inputs) of python model, convert them to C++ string and store +// them in pymodel_input_names +py::list pymodel_input_names_ = py_model_class.attr("get_action_names")(); +num_inputs_py = pymodel_input_names_.size(); +for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++) { + pymodel_input_names.push_back(PyBytes_AS_STRING( + PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); +} + +std::cout << "PymodelInterface import name: " << pymodel_import_name << std::endl; +} + +std::vector PymodelSimpleModel::getNextState( + std::vector model_signals_vec, std::vector model_signals_vec_next) + { + // get inputs and states of the python model from the vector of signals + std::vector py_inputs(num_inputs_py); + std::vector py_state(num_outputs_py); + py_inputs = fillVectorUsingMap(py_inputs, model_signals_vec, map_sig_vec_to_pyin, true); + py_state = fillVectorUsingMap(py_state, model_signals_vec, map_pyout_to_sig_vec, true); + + // forward pass through the base model + py::tuple res = py_model_class.attr("forward")(py_inputs, py_state); + std::vector py_state_next = res.cast>(); + + // map outputs from python model to required outputs + std::vector next_state = + fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); + + return next_state; + } + +void PymodelSimpleModel::dtSet(double dt) { py_model_class.attr("dtSet")(dt); } + +std::vector PymodelSimpleModel::getInputNames() { return pymodel_input_names; } + +std::vector PymodelSimpleModel::getStateNames() { return pymodel_state_names; } + +void PymodelSimpleModel::mapInputs(std::vector signals_vec_names) +{ +// index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is +// index in "signals_vec_names" +map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); +} \ No newline at end of file From 89ed5e9c22c2bc0741276c167d6ca423c71cb717 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 30 Jan 2024 16:11:21 +0100 Subject: [PATCH 75/97] Fix bug multiple definitions of functions --- .../model_connections_helpers.hpp | 30 ++-------------- .../src/model_connections_helpers.cpp | 34 ++++++++++++++++++- 2 files changed, 35 insertions(+), 29 deletions(-) diff --git a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp index 264311e0bc29f..de76f84979c75 100644 --- a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp +++ b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp @@ -5,35 +5,9 @@ #include std::vector fillVectorUsingMap( - std::vector vector1, std::vector vector2, std::vector map, bool inverse) - { - // index in "map" is index in "vector1" and value in "map" is index in "vector2" - // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 - for (std::size_t idx = 0; idx < map.size(); idx++) { - if (map[idx] == -1) continue; - inverse ? vector1[idx] = vector2[map[idx]] : vector2[map[idx]] = vector1[idx]; - } - return inverse ? vector1 : vector2; - } + std::vector vector1, std::vector vector2, std::vector map, bool inverse); std::vector createConnectionsMap( - std::vector connection_names_1, std::vector connection_names_2) - { - std::vector connection_map; - // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is - // index in "connection_names_1" - for (auto name_2 : connection_names_2) { - int mapped_idx = -1; // -1 means that we cannot create a connection between some signals - for (std::size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { - if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same - // and we can create connection - mapped_idx = NAME_1_IDX; - break; - } - } - connection_map.push_back(mapped_idx); - } - return connection_map; - } + std::vector connection_names_1, std::vector connection_names_2); #endif // LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/src/model_connections_helpers.cpp b/simulator/learned_model/src/model_connections_helpers.cpp index 05b14720b8395..6b3541b4e960f 100644 --- a/simulator/learned_model/src/model_connections_helpers.cpp +++ b/simulator/learned_model/src/model_connections_helpers.cpp @@ -1 +1,33 @@ -#include "learned_model/model_connections_helpers.hpp" \ No newline at end of file +#include "learned_model/model_connections_helpers.hpp" + +std::vector fillVectorUsingMap( + std::vector vector1, std::vector vector2, std::vector map, bool inverse) + { + // index in "map" is index in "vector1" and value in "map" is index in "vector2" + // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 + for (std::size_t idx = 0; idx < map.size(); idx++) { + if (map[idx] == -1) continue; + inverse ? vector1[idx] = vector2[map[idx]] : vector2[map[idx]] = vector1[idx]; + } + return inverse ? vector1 : vector2; + } + + std::vector createConnectionsMap( + std::vector connection_names_1, std::vector connection_names_2) + { + std::vector connection_map; + // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is + // index in "connection_names_1" + for (auto name_2 : connection_names_2) { + int mapped_idx = -1; // -1 means that we cannot create a connection between some signals + for (std::size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { + if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same + // and we can create connection + mapped_idx = NAME_1_IDX; + break; + } + } + connection_map.push_back(mapped_idx); + } + return connection_map; + } \ No newline at end of file From 5c018e2cb201450d30d49095c8e76dfecf51e545 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 30 Jan 2024 18:28:31 +0100 Subject: [PATCH 76/97] Add missing definition --- simulator/learned_model/src/pymodel_simple_model.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/simulator/learned_model/src/pymodel_simple_model.cpp b/simulator/learned_model/src/pymodel_simple_model.cpp index 6bd86c1a53a1b..6471aba30f51a 100644 --- a/simulator/learned_model/src/pymodel_simple_model.cpp +++ b/simulator/learned_model/src/pymodel_simple_model.cpp @@ -82,4 +82,11 @@ void PymodelSimpleModel::mapInputs(std::vector signals_vec_names) // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is // index in "signals_vec_names" map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); +} + +void PymodelSimpleModel::mapOutputs(std::vector signals_vec_names) +{ + // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in + // "map_pyout_to_sig_vec" is index in "signals_vec_names" + map_pyout_to_sig_vec = createConnectionsMap(signals_vec_names, pymodel_state_names); } \ No newline at end of file From de4755cccf4c8ce02b002f5713daa6dd0ef295a4 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 5 Feb 2024 11:43:19 +0100 Subject: [PATCH 77/97] Fix symbol visibility problem --- .../include/learned_model/pymodel_interconnected_model.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp index e9f34dbe33aa7..ce6c96e812c4c 100644 --- a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp @@ -13,7 +13,7 @@ namespace py = pybind11; -class InterconnectedModel +class __attribute__ ((visibility ("default"))) InterconnectedModel { // Vector of unique names of inputs and outputs of sub-models std::vector signals_vec_names; From f830675dba0c5f704407b33ed3bd4efb76486c20 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 5 Feb 2024 18:53:53 +0100 Subject: [PATCH 78/97] Change naming to better represent functionality of classes and files --- ...ted_model.hpp => interconnected_model.hpp} | 12 +++++----- ...el_simple_model.hpp => simple_pymodel.hpp} | 12 +++++----- ...l_interface.hpp => submodel_interface.hpp} | 8 +++---- ...ted_model.cpp => interconnected_model.cpp} | 6 ++--- .../learned_model/src/pymodel_interface.cpp | 1 - ...el_simple_model.cpp => simple_pymodel.cpp} | 16 ++++++------- .../learned_model/src/submodel_interface.cpp | 1 + .../simple_planning_simulator/CMakeLists.txt | 2 +- .../simple_planning_simulator_core.hpp | 2 +- .../vehicle_model/sim_model.hpp | 2 +- ...ls.hpp => sim_model_learned_steer_vel.hpp} | 16 ++++++------- .../simple_planning_simulator_core.cpp | 10 ++++---- ...ls.cpp => sim_model_learned_steer_vel.cpp} | 24 +++++++++---------- 13 files changed, 56 insertions(+), 56 deletions(-) rename simulator/learned_model/include/learned_model/{pymodel_interconnected_model.hpp => interconnected_model.hpp} (91%) rename simulator/learned_model/include/learned_model/{pymodel_simple_model.hpp => simple_pymodel.hpp} (85%) rename simulator/learned_model/include/learned_model/{pymodel_interface.hpp => submodel_interface.hpp} (88%) rename simulator/learned_model/src/{pymodel_interconnected_model.cpp => interconnected_model.cpp} (95%) delete mode 100644 simulator/learned_model/src/pymodel_interface.cpp rename simulator/learned_model/src/{pymodel_simple_model.cpp => simple_pymodel.cpp} (83%) create mode 100644 simulator/learned_model/src/submodel_interface.cpp rename simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/{sim_model_pymodels.hpp => sim_model_learned_steer_vel.hpp} (86%) rename simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/{sim_model_pymodels.cpp => sim_model_learned_steer_vel.cpp} (79%) diff --git a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp b/simulator/learned_model/include/learned_model/interconnected_model.hpp similarity index 91% rename from simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp rename to simulator/learned_model/include/learned_model/interconnected_model.hpp index ce6c96e812c4c..0cfc1a5bbcbf1 100644 --- a/simulator/learned_model/include/learned_model/pymodel_interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/interconnected_model.hpp @@ -1,9 +1,9 @@ -#ifndef LEARNED_MODEL__PYMODEL_INTERCONNECTED_MODEL_HPP_ -#define LEARNED_MODEL__PYMODEL_INTERCONNECTED_MODEL_HPP_ +#ifndef LEARNED_MODEL__INTERCONNECTED_MODEL_HPP_ +#define LEARNED_MODEL__INTERCONNECTED_MODEL_HPP_ #include "learned_model/model_connections_helpers.hpp" -#include "learned_model/pymodel_interface.hpp" -#include "learned_model/pymodel_simple_model.hpp" +#include "learned_model/submodel_interface.hpp" +#include "learned_model/simple_pymodel.hpp" #include #include @@ -20,7 +20,7 @@ class __attribute__ ((visibility ("default"))) InterconnectedModel std::vector model_signals_vec; int num_signals; - std::vector> submodels; + std::vector> submodels; // index in "map_in_to_sig_vec" is index in "py_inputs" and value in "map_in_to_sig_vec" is index // in "all_variables_names" @@ -108,4 +108,4 @@ class __attribute__ ((visibility ("default"))) InterconnectedModel std::vector updatePymodel(std::vector psim_input); }; -#endif // LEARNED_MODEL__PYMODEL_INTERCONNECTED_MODEL_HPP_ \ No newline at end of file +#endif // LEARNED_MODEL__INTERCONNECTED_MODEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp b/simulator/learned_model/include/learned_model/simple_pymodel.hpp similarity index 85% rename from simulator/learned_model/include/learned_model/pymodel_simple_model.hpp rename to simulator/learned_model/include/learned_model/simple_pymodel.hpp index fb16cc4121a6e..4af43209100a4 100644 --- a/simulator/learned_model/include/learned_model/pymodel_simple_model.hpp +++ b/simulator/learned_model/include/learned_model/simple_pymodel.hpp @@ -1,8 +1,8 @@ -#ifndef LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ -#define LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ +#ifndef LEARNED_MODEL__SIMPLE_PYMODEL_HPP_ +#define LEARNED_MODEL__SIMPLE_PYMODEL_HPP_ #include "learned_model/model_connections_helpers.hpp" -#include "learned_model/pymodel_interface.hpp" +#include "learned_model/submodel_interface.hpp" #include #include @@ -13,7 +13,7 @@ namespace py = pybind11; * @class SimModelPymodels * @brief This class is an interface between C++ and python models. */ -class PymodelSimpleModel : public PymodelInterface +class SimplePymodel : public SubModelInterface { private: std::string pymodel_import_name; @@ -40,7 +40,7 @@ class PymodelSimpleModel : public PymodelInterface * @param [in] param_file_path path to saved parameter file of the python sub-model * @param [in] py_class_name name of the python class */ - PymodelSimpleModel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name); + SimplePymodel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name); /** * @brief calculate the next state of a python model @@ -79,4 +79,4 @@ class PymodelSimpleModel : public PymodelInterface }; -#endif // LEARNED_MODEL__PYMODEL_SIMPLE_MODEL_HPP_ \ No newline at end of file +#endif // LEARNED_MODEL__SIMPLE_PYMODEL_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/include/learned_model/pymodel_interface.hpp b/simulator/learned_model/include/learned_model/submodel_interface.hpp similarity index 88% rename from simulator/learned_model/include/learned_model/pymodel_interface.hpp rename to simulator/learned_model/include/learned_model/submodel_interface.hpp index 8f92bbd94d60f..b1dd24a6b3068 100644 --- a/simulator/learned_model/include/learned_model/pymodel_interface.hpp +++ b/simulator/learned_model/include/learned_model/submodel_interface.hpp @@ -1,9 +1,9 @@ -#ifndef LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ -#define LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ +#ifndef LEARNED_MODEL__SUBMODEL_INTERFACE_HPP_ +#define LEARNED_MODEL__SUBMODEL_INTERFACE_HPP_ #include -class PymodelInterface +class SubModelInterface { public: // virtual ~PymodelInterface() = 0; @@ -45,4 +45,4 @@ class PymodelInterface std::vector model_signals_vec, std::vector model_signals_vec_next) = 0; }; -#endif // LEARNED_MODEL__PYMODEL_INTERFACE_HPP_ \ No newline at end of file +#endif // LEARNED_MODEL__SUBMODEL_INTERFACE_HPP_ \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_interconnected_model.cpp b/simulator/learned_model/src/interconnected_model.cpp similarity index 95% rename from simulator/learned_model/src/pymodel_interconnected_model.cpp rename to simulator/learned_model/src/interconnected_model.cpp index 2a08e1411d800..bcecba0aa09db 100644 --- a/simulator/learned_model/src/pymodel_interconnected_model.cpp +++ b/simulator/learned_model/src/interconnected_model.cpp @@ -1,4 +1,4 @@ -#include "learned_model/pymodel_interconnected_model.hpp" +#include "learned_model/interconnected_model.hpp" void InterconnectedModel::mapInputs(std::vector in_names) @@ -59,8 +59,8 @@ void InterconnectedModel::generateConnections(std::vector in_names, std: void InterconnectedModel::addSubmodel(std::tuple submodel_desc) { const auto [lib_path, param_path, class_name] = submodel_desc; - auto new_model = new PymodelSimpleModel(lib_path, param_path, class_name); - submodels.push_back(std::unique_ptr(new_model)); + auto new_model = new SimplePymodel(lib_path, param_path, class_name); + submodels.push_back(std::unique_ptr(new_model)); } void InterconnectedModel::initState(std::vector new_state) diff --git a/simulator/learned_model/src/pymodel_interface.cpp b/simulator/learned_model/src/pymodel_interface.cpp deleted file mode 100644 index edd0c0d2483d6..0000000000000 --- a/simulator/learned_model/src/pymodel_interface.cpp +++ /dev/null @@ -1 +0,0 @@ -#include "learned_model/pymodel_interface.hpp" \ No newline at end of file diff --git a/simulator/learned_model/src/pymodel_simple_model.cpp b/simulator/learned_model/src/simple_pymodel.cpp similarity index 83% rename from simulator/learned_model/src/pymodel_simple_model.cpp rename to simulator/learned_model/src/simple_pymodel.cpp index 6471aba30f51a..6ebfff3f27a6c 100644 --- a/simulator/learned_model/src/pymodel_simple_model.cpp +++ b/simulator/learned_model/src/simple_pymodel.cpp @@ -1,4 +1,4 @@ -#include "learned_model/pymodel_simple_model.hpp" +#include "learned_model/simple_pymodel.hpp" #include @@ -7,7 +7,7 @@ namespace py = pybind11; -PymodelSimpleModel::PymodelSimpleModel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name) +SimplePymodel::SimplePymodel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name) { // Import model class pymodel_import_name = pymodel_import_name_; @@ -51,7 +51,7 @@ for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++) { std::cout << "PymodelInterface import name: " << pymodel_import_name << std::endl; } -std::vector PymodelSimpleModel::getNextState( +std::vector SimplePymodel::getNextState( std::vector model_signals_vec, std::vector model_signals_vec_next) { // get inputs and states of the python model from the vector of signals @@ -71,20 +71,20 @@ std::vector PymodelSimpleModel::getNextState( return next_state; } -void PymodelSimpleModel::dtSet(double dt) { py_model_class.attr("dtSet")(dt); } +void SimplePymodel::dtSet(double dt) { py_model_class.attr("dtSet")(dt); } -std::vector PymodelSimpleModel::getInputNames() { return pymodel_input_names; } +std::vector SimplePymodel::getInputNames() { return pymodel_input_names; } -std::vector PymodelSimpleModel::getStateNames() { return pymodel_state_names; } +std::vector SimplePymodel::getStateNames() { return pymodel_state_names; } -void PymodelSimpleModel::mapInputs(std::vector signals_vec_names) +void SimplePymodel::mapInputs(std::vector signals_vec_names) { // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is // index in "signals_vec_names" map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); } -void PymodelSimpleModel::mapOutputs(std::vector signals_vec_names) +void SimplePymodel::mapOutputs(std::vector signals_vec_names) { // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in // "map_pyout_to_sig_vec" is index in "signals_vec_names" diff --git a/simulator/learned_model/src/submodel_interface.cpp b/simulator/learned_model/src/submodel_interface.cpp new file mode 100644 index 0000000000000..b7edded274e6e --- /dev/null +++ b/simulator/learned_model/src/submodel_interface.cpp @@ -0,0 +1 @@ +#include "learned_model/submodel_interface.hpp" \ No newline at end of file diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 85ce6986ee111..8835d568b213f 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -16,7 +16,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp - src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp + src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 4e738776d475e..e81d4fef00abb 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -211,7 +211,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node IDEAL_STEER_VEL = 4, DELAY_STEER_VEL = 5, DELAY_STEER_MAP_ACC_GEARED = 6, - PYMODELS = 7 + LEARNED_STEER_VEL = 7 } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp index fcc4baaffcb93..fe044beb95cf7 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp @@ -19,7 +19,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp similarity index 86% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp rename to simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp index 04a5329ff696f..2ca4cfbeb8359 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ -#include "learned_model/pymodel_interconnected_model.hpp" +#include "learned_model/interconnected_model.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include @@ -29,17 +29,17 @@ // namespace py = pybind11; /** - * @class SimModelPymodels + * @class SimModelLearnedSteerVel * @brief calculate delay steering dynamics */ -class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ +class SimModelLearnedSteerVel : public SimModelInterface /*delay steer velocity*/ { public: /** * @brief constructor * @param [in] dt delta time information to set input buffer for delay */ - SimModelPymodels(double dt, + SimModelLearnedSteerVel(double dt, std::vector model_python_paths, std::vector model_param_paths, std::vector model_class_names); @@ -47,7 +47,7 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ /** * @brief destructor */ - ~SimModelPymodels() = default; + ~SimModelLearnedSteerVel() = default; private: /* @@ -142,4 +142,4 @@ class SimModelPymodels : public SimModelInterface /*delay steer velocity*/ } }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_PYMODELS_HPP_ \ No newline at end of file +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ \ No newline at end of file diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 12f75e585cc12..958abdad34c2e 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -285,10 +285,10 @@ void SimplePlanningSimulator::initialize_vehicle_model() vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_bias, acceleration_map_path); - } else if (vehicle_model_type_str == "PYMODELS"){ - vehicle_model_type_ = VehicleModelType::PYMODELS; + } else if (vehicle_model_type_str == "LEARNED_STEER_VEL"){ + vehicle_model_type_ = VehicleModelType::LEARNED_STEER_VEL; - vehicle_model_ptr_ = std::make_shared(timer_sampling_time_ms_ / 1000.0, + vehicle_model_ptr_ = std::make_shared(timer_sampling_time_ms_ / 1000.0, model_python_paths, model_param_paths, model_class_names); }else{ @@ -489,7 +489,7 @@ void SimplePlanningSimulator::set_input( if ( vehicle_model_type_ == VehicleModelType::IDEAL_STEER_VEL || vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL || - vehicle_model_type_ == VehicleModelType::PYMODELS) { + vehicle_model_type_ == VehicleModelType::LEARNED_STEER_VEL) { input << vel, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || @@ -595,7 +595,7 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL) { state << x, y, yaw, vx, steer; - } else if (vehicle_model_type_ == VehicleModelType::PYMODELS){ + } else if (vehicle_model_type_ == VehicleModelType::LEARNED_STEER_VEL){ state << x, y, yaw, yaw_rate, vx, vy, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC || diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp similarity index 79% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp rename to simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp index e0e2f577cee83..46280370cdda0 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_pymodels.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_pymodels.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp" -#include "learned_model/pymodel_interconnected_model.hpp" +#include "learned_model/interconnected_model.hpp" #include -SimModelPymodels::SimModelPymodels(double dt, +SimModelLearnedSteerVel::SimModelLearnedSteerVel(double dt, std::vector model_python_paths, std::vector model_param_paths, std::vector model_class_names @@ -38,39 +38,39 @@ SimModelPymodels::SimModelPymodels(double dt, std::cout << "Python model loaded successfully " << std::endl; } -double SimModelPymodels::getX() +double SimModelLearnedSteerVel::getX() { return state_(IDX::X); } -double SimModelPymodels::getY() +double SimModelLearnedSteerVel::getY() { return state_(IDX::Y); } -double SimModelPymodels::getYaw() +double SimModelLearnedSteerVel::getYaw() { return state_(IDX::YAW); } -double SimModelPymodels::getVx() +double SimModelLearnedSteerVel::getVx() { return state_(IDX::VX); } -double SimModelPymodels::getVy() +double SimModelLearnedSteerVel::getVy() { return state_(IDX::VY); } -double SimModelPymodels::getAx() +double SimModelLearnedSteerVel::getAx() { return current_ax_; } -double SimModelPymodels::getWz() +double SimModelLearnedSteerVel::getWz() { return state_(IDX::YAW_RATE); } -double SimModelPymodels::getSteer() +double SimModelLearnedSteerVel::getSteer() { return state_(IDX::STEER); } -void SimModelPymodels::update(const double & dt) +void SimModelLearnedSteerVel::update(const double & dt) { // Eigen::VectorXd to std::vector for model input std::vector vehicle_input_(input_.data(), input_.data() + input_.size()); From 603f47d70735470cecf4fc1099475d31eea2f8d5 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 5 Feb 2024 19:30:33 +0100 Subject: [PATCH 79/97] Improve documentation --- simulator/learned_model/README.md | 14 +++++++------- .../image/python_model_interface.png | Bin 88467 -> 94435 bytes 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md index 0e1b9e9299388..eac18750399a5 100644 --- a/simulator/learned_model/README.md +++ b/simulator/learned_model/README.md @@ -16,7 +16,7 @@ This library creates an interface between models in Python and PSIM (C++). It is -Using this Python model interface a PSIM model consisting of sub-models implemented in Python can be created. Each sub-model has string names for all of its inputs/outputs which are used to automatically create model interconnections (see image below). +The idea behind this package is that the model we want to use for simulation consists of multiple sub-models (e.g., steering model, drive model, vehicle kinematics, etc.). These sub-models are implemented in Python and can be trainable. Each sub-model has string names for all of its inputs/outputs, which are used to create model interconnections automatically (see image below). This allows us to easily switch sub-models for better customization of the simulator. ![pymodel_interface](./image/python_model_interface.png "PyModel interface") @@ -24,7 +24,7 @@ Using this Python model interface a PSIM model consisting of sub-models implemen -To use this package `python3` and `pybind11` need to be installed. The only assumption on Python sub-models is their interface (see below). +To use this package `python3` and `pybind11` need to be installed. The only assumption on Python sub-models is their interface. ```python class PythonSubmodelInterface: @@ -81,16 +81,16 @@ To successfully create a vehicle model an InterconnectedModel class needs to be ### InterconnectedModel class #### ```Constructor``` -Constructor takes no arguments. +The constructor takes no arguments. #### ```void addSubmodel(std::tuple model_descriptor)``` Add a new sub-model to the model. Inputs: -* model_descriptor: Describes what model should be used. Model descriptor contains three strings: - * First string is a path to a file where the model is implemented. - * Second string is a path to the file where model parameters are stored - * Third string is a name of the class that implement the model. +* model_descriptor: Describes what model should be used. The model descriptor contains three strings: + * The first string is a path to a file where the model is implemented. + * The second string is a path to the file where model parameters are stored. + * The third string is the name of the class that implements the model. Outputs: * None diff --git a/simulator/learned_model/image/python_model_interface.png b/simulator/learned_model/image/python_model_interface.png index 28b217801d57b9c3a020d7bf5444b4e42e641c22..d81caf7a94000c60b8d3594613bb481cea839c65 100644 GIT binary patch literal 94435 zcmeEu2|SeR+kZ(J5`#op$6m4wvV>ynQ51=6&DgVM9ZJYxicl1ltzA(lp;ET7bW~_T zlqHIit%yYZuV*D3> zCn~QkDkq2c_Lg>V#XI}pJ$$7-9sS@EeDC4q;OgY+=rGepPF7AqN>)xvc8wWUSyWyV zi-rHlDoV>@mA22c$2&WE&fajNSFo$QJ6=>yca5|x+)Ba%X^w-xwi5h3{rqSCHgfg! zC&D$NvZ9@$!pt?F0Oa0sa?-LZ;Tt_idp9q*i<~T07XHP;C0$n^M<*}%nJ%@}%%dIs z@Xj;c46rcUu4m}06^L^RHgr%3QV!5onQ0T?=;Q0^=9g(Z@I-;_g@E!86yw>dCzz0vf$81{@cwed6JGjmclAB3@tAz)V=&5b#Xun0u zewT|9+~n)K(eCL%)bqi6yKMAwaCCF4F;?&s?L^+kIxPftht*(sa7Bi_fyEAZ=PPG0V_ zuQ%Jq8{Yh@yCWA&?eQ>jRCAisY@4ftpUccuLQK&jD-t&wu;nHGRI= z{eN>i6#^C~>*lv%gMpEjshOSU2CT9y&AtNg?*6lbJ2O(genGU+_VM!fbfB(MuvA>2 zi>sfbsW;vpX&wj$2G?BtJlx?EvI4;gxNCX2d-=d;PcKhs1d9?eHdq~gjx-aTx#W%i z`NgR3`s<_n^BC(ndU-hd`2@jjXYZ&qJ20~rErpd;n7tDCvw>i-$Ssi0xXiJRH8kVE z&swGPm%AfPf6vT8o7su~^*2C#<2UwUvPRxqNp^$wwjfP&r9giV#Q-xCDLJLtG5gbJ z<+~~P!9@M~JN??Md|7utPYZ1Z=1z+&ZT99)i-O|pT>XWnW!6jn`HSVxofdi7*%zTr zi;D91i$zi82Wv&)FSu5Id#Qkvr^a6=cSoWo5`MrbIeI$G{;6&6j`#I-wg1|WeeLvq z!C%SAIy!t6ss0B`WzIsN#jm-`fOgHt(H-yS8t`=h{L#|*Jze;D&6;?*0;KxlM&;Ig zV;Rb_YowJFXI`K7G+!@&AA85yU;aK9IrG3@eC&4y`J>1B;eDJP{r(4!mDi&B=PyCy z*GFi!HPuc3R?FY#%@zK&P`SA=_18!BH%vuEf$0|>@BxCV&IDLNy6($Wi8Pj7!eUvMQ7CZ+}(A-9@o2}^HIW7^fPFYWS2kTuTK zOh;#nowklCw3@v{YyThI#oXe@w;=ey&8R*8rJEwIY!2)Nw2Jg<4z~66^YLmjw-i1qs{*Vhm42ifd@VYw#8rgj$pmDM75 zu>7y?V7$dZXOo$xnSt@9@9*=k@&AlHeqj9Hd-T7x^~?P(^EdS;00Bp(h71rV<%Fd4 zum}JxLCTK%MGyYpyUAHz;P2)m*U&PeFKH7kZ~ZL?N#i1D2zo9WLJae_xgjco{R1(n zobq>|)YsL++uhO5+a2%e>FVih2cP{R=Kv<>XU{m-I{yE)0~&uyvjgCbzt5lLXp=rS zo1^{muVV=EFpbDe&Iy&Kem*e&a1EiNk+|qYwE+ay!G|w65AW#!AHDqjW^o^odGHw^ zkgF$SuyWsEEhWSU zAd@iHkI+nIuK6g*e!*maAq)CZn(&Qt_|MoDz|1~D7J!~*r4=C8LdY7pU`f58qNG5( zpp9_9h+$B_)<#z2mro{+KCVC!BhZ_AaV|=qYdc><`nlFNGb2AB;%4X%xG%K$A$v<( zQZsqquQZ$;xPKUjB&AFXi!}eJBnx)?znP*-DbrTemoBXN)h_&PQ*`<7q24dJZ|@*K z7bs3rtFBbg_q!drchJlasrbMH78?+e)I0s_&0z$TX>tCGsnKlj@9E9I2Zq$_z!Q&D zHED5twy+CkfZ&6f$dC8;^Md*+LUOnxl{p8*{d&xT0RrBa3JR!pGm9&}i4niWx6F0U zzvRyUZcspx1{`U|qo^n^t*9vb<)7IS@LW@(dDpqX;Qy3?QSiq3Y<95pS9aT}ztPZA zcgsesgVgUJy`6#P^8UpzJ8NQi0<8=^i;@07(o6o^rKHL~*#d{z%dryKd@;_+u8=qxO{Tu;JQ8R+s_5a;ZsGR5TA!&c(S>N7RR$?Y> z{a!NSYvS}bqzd1g-mhBnxB7+uIoV2s_H$OX9Iejx*ETVl(&<-p3I7??|Ig;&pMsQU zy}i7GwDOvt3s)+$st=k+`7`RQGvodHp8vN6#WZ(4htrsO?>WT7zh(*$?FauN<)Lpy z0W@{U9J85YwJNhI$e(DovA7n}ML~YX1E{Lh`?Dl*U`l{%EWG*+_rMuK%^jxtdtHZ`|hJ(IkJkcxh_& zul(dUbN|;%E|q9ubdC%7VP>iLt?QWa9DgB~@FT|mJC~sCgUA)Xa7N#zc3+~^uScf8 zKmz#1-2Tk~O{B2j6VCrT*qSyA|G*qjZVpmVo(;FO)&9Sd1pZnF{=R7XcWk2gQ?kH+ znx)Qev5}+2uQ^%RuNlstV7cD_8iYUjwI2L^P51Xwxzs%FOS<=mCW61Dqy3iU{RVga zRu$(jl?BpP>K|BB_@<=rgIeIgQB@}jMoLNf zo7MRjia69`z<%M#PuhWEKcB8TcP|L_TiSW9GhfX90HO$-_=B7vsy6=++q`*#^9-~# z&4{*d(iRqrRE-t&8#iQ@tJ7OqN8GHVi%|{lysZ(|Ie1%{e&5ZT9D@4ZOD;1?$5^4> z)K~>g?!8)h_vx|gYkbe2?|7#cRNMaQ^5x*{8#gAKds8P%(G__apU)d;(6J|k(TfPq zXVm}s&w_-qJbY!vXAEUHRAKHHU)qE>p(jc|e_T(`9huV9dClxcjfOunN>G$nd~EJG7@Zh{0DLbOVk*x@8zH*8b-=p#22it6+Z^&gg{3 z?&Uw~V9LsMwDKp(Y)=V;DO zJ`ClIyNR6KGLi2mPos;`p~7GlnhASBe|y}w<_5!Uqo;v6*MH;Dq8}I}JYgwe?KS6| zNg$<5u?G@%BnKr*f;y>B#u?pOjWpNWz@9Ha+yJI=leSC9u`0+Nost%Q* zSo!i(|4) z#q@ZGz=TKm!eyu${@v~=yQ-I;aN70a$hP;}|OpVkSS7fH+KD{q0jnv_?QJ8wYt@L8=@bQ{Q!W6aP zvIY2zkoSz%MYbP5eH^NkI2|kZvJrt)pNDkE_Umt6-s(k8O&!_z9V)kVw9*=^} z#P?5}Pq$oYz%7nkEK?%8`r4x{U3D_0%vSFCIuiq4p*(TxKTX)7u%#cL4OPIXa<>Qf zU*eapj9RkTbmN+>#Lv^?mC&B|qwA})$t*?B0<5@$dJah{F3fi=3!9IBymi~b=G35< z$12UOJUDgI+4Oj^sW1m!Z?4v}$dnA@G|+C>ESURLpW?W>c%x%xJ9 z#_qxE_}3TCvpn3WWMi>=;9BJ|=k(>eD_qZ547Tqc;;WP5Bn;jfZs#l^~Hw`bSHSZ*j^HLveo z3F(Gqq{YRZ1M-#3@}vbU%7Sl-I`($ieV*Jl`l!5;9^)9V{=V-zf4b4l!5eNRd}|fnFR_&vold}L zRqP(R#RY5QdP^^P*;!pHv11O0^In}h@qYL|DdkOZtdu>=(6KFE*}WN|>r#o&_s^Gp z!H+FSyD|03IP^ffBKLq(>bkeJ!t={xKdHV|?ek`*+C38!} zf>h-~%j~W3PIaSdvj#7P#CX=(4{@mB=Ld(qn{5KCxOPxjY|YtF8zNIeKY#4lyrZmp zUOZjNVj}*Tcjc1Z+H@ptZFYJd+nylTmysc7kC?w^BYgVU@j1%%e3~M^mIO+u!cgYc zQmw5Qm*bfES;^woTtf%fG+WD;Ouo+E%v*~~M5$`d!zq;QCS1P$^1w-%M~s+-r;#Adf(<1kJG)5Hhnb}*b$<%y0~-S{9XIqOr!}K zQkX?u3uIU+m+y$3SiQP*>TUiKVq_8%_EGT}!2qfb2x z?6{>z4g&K!RaRjkUDSX1rgT&MryBH0bO1U#SiZzTvCaMJf#|K1m(10JZyqf*oWV}V zF#vm**VNDAvQdut6i#~Pw>auMT@)9A!os{6|I^Czu^%+hm{rSbI!DT|_OV!9 zHHRZx%^90GG&Y5D!r zcU{jN6B8sh6dluJ4ypY>CclW%&?@F4urCT#7IYbNf+x7~vL4BK;C1bFW81YUo0(YN zz&t-P#VIRj78PY?pc337xOUKU9?pK=g>zXblMd5Q%oH?1oA2`uOJ0Du93A6hMliBF z&K+|)P&)EpFAOaudBBLQsjpE`W_b1y+6m=x!GVlINuw4V8vr*q;wa#C0)CPhoxrE2 zL(~*wlx&wdhSoZ9rQb)Y!W~U$vamnc$G5L5P0LD>5qso*%~kr%(XyB~FHd-}L<{UC z=AJazW*6eG{G2TO+<%x@-fe$ib*bgacmi?ONEXi~3{Q_qI36{O*Wn8;77m44D}&=9$k^GNty(aGs*9xqY^ z>pJB^2G(;MaLmt{=mbu2@asL|#FKX&VZ>TH=j2x(z<9k-u|=&v9Y>DiwpAuROxjK9 zd_oFjDRpkjLYrpu-^w2job@U402z;;e5o>%+=TBH3R9w!=|wFiv-MvpvK_rGn4u?+ zW8O!1QjxoBHTY5sSl5e#N!&OslrbirIjV@>R+2PscJ;1s6xXP`62>-@4ZjyW(l`o> zVnzvpG3*)@DtDv9i*OLowzcv5jxb@*?$eXPoO|%0U_mZt>p`}Q`xs25;@71V?hyFI2OVy}XfcLnJr5bS0gPctud3?}JjeDlSKrvkGXO^6`400v_Es+nNyqBOt6ZdkW#VS2JLE^v4iX1xK7oXnt7JJEjh zGNx;wQBQ!c(n~I$yJ9QacsLa!Lgup4rayeg{OW2wW|?}T|5I?B*z2p?g;2|3Xay>ZM!cZFhBvvZ7H2~obj8C`?kzJqmJ#U9(B7f1Tm;NmzZ zGg4J}wYE5EU)zaeo{mytbrek4f!sU)o>GTeu<0SMMcB9o@e)t{bnAPxk~fZ zWdqb2;;zm^VG@;fSd|?+8L_;2Pl%5?Rl>`(9&|piNknr=BqzI&ShSf+yoQTikD#>% zIVx_T_z2O$6am`_Hp`vMZS{!hf;ho5CW|U?r*X_G*28bZ7Sk;X*^f5iKTeU}N(>cD z63puii7?U4;%LYA*lCvFYEMa_I7JE^n?+j2#3*?S6>K+H?#*n+C3@a?buLAg8+*_u zr?_de9v`QA;6+C4u(kvoM9i;o7VJ!mggNPZ{g8cBPZ`OTnieuq8Yn!GG4a281-SmeA45JX|T&CQ42?zDZu2_3%3(p*$3B zSkOqk?!^PwF^{bw!$&o(akZ~xwCF3cv(!GDmU&x=$MuwFg6%CYc{98nf%#`-IUHzO)q1ve^*komjQZD#tnImDxR!4=Yt=*gVhpOV9TyPAD!iPqED28L!8Zt}ilq$q^;m zBewI%W2<;`n+_l9?=w@tKOJ~7RJi@SgibJgFR}nx8*ZKXLAqNdl8H9GySM1^4_KSg z1rSO0h7F3&c5!YK2{HJN@c!ndqu&!QnF7eGE%1|B`GdqA3mneUs~sNKe$bg|4H$oc z%-sevPWP)je>*pGJJZJi@NatFGs6v;i6~h{m$~273Wwt>qh92YHQ$xN$r^lSHrjT?!NMfuO+ zm$H(>`dfkxf~?Q8>8l5D64qXdz7$U%j7bP)M;Fh1yH?FIe|J@h-3QIl)P~y|4Ueu2 zU&gMl&RCFlahcSdQ3~g$yKK+L93+|${ALxU$6v%^r~+xXblI6D$v=14MRI;~zDLzS z6+3(hk7f^u(=o{8L*8p3>uU2?1XRC8jfO4b8UCA0*dRW}rki<4tH_0}m2+mbbp!O! zYS-?368)BVo5&PWn*C;W7l!ahi5jY}->jKSO?9qq!X|8kQM!yXvenx+CcPSIh1YEL z{&~{E3)#K-+RUHYuEvBvvfqXg!}nSw(YI;}j)^qHt=&DFrOv!gahyMM&k>E+`Pt~i z=0iJfEjEN!CKd^b)<0Wv<|TUTnnKPZE<=66v6ii5gBF!#qTvBBnv4af@7rqq+)@J> zz6F2N;S*dQ9{TBB1|AZZB?!C0?Us{YfyAgy>pt|zwiCM_msCS4#P%{H^i$pwrxT*t zhS&D3`#kkd79d#^{37e+wY~37bdZ!f_Q=Pozi+AJD|&ZdwI);mlHE1+9lrJIm6QFs z0#0fEO;N3*2t?Yt&1Owb$=jd060}%c`>a`!)ttZugZS=&Y17qI>a=uoCu-bBok^$n zY1)%5IiJywJ3M3Wk1qo@`e zC&}0otMS@E(D`1`4fr~mjRaGfe)=_NP5#ima!&T+O zCUGCKA&J4&S89d24jW4E7pjLL<0@?3nLP|CSM>Y^izsgQReDoLwv%}fE{1y-g{`v5 z%8PH|sk_3|e5qaP?gbbDCn~P}oLeL8bm>|X6G9^?_on(*0C3?_x0Dz{NFqJ{Acqw1 z3dYKXT-NB9kTi#h-@Ddq%OeKaSRPOiq=SmB5^8`;@aNM4m=;SFm=q)2%dQ$euAu4R zN>==EXZZ+sD=C`Izr*w9Km`n|KpW-y)cdz}GSNZ3X-ZQsWJ32(eFkzsFC8c^VoJ#S zCzUWJ{74qh!Xss_N5?L)795F=!2Ay$q0|-Fk-y{K;tj|OG>KJ&ps24OCd*6DtW7!# zV_Te;&cIL1Jcst39&3( zOwJi00T#rr$V!C!3V=ij;RAFVol2+QFU~D_yP@+20-%c_pR7nS-LxFZ8-^(y#@d*d zkSVi5PPwr}8lx|HsbCAYf{V%04X-%~>A|lr3^!=8>NGNLj&c(gITB2H10%Ot5T}64 z+wRw>bNYNk*6FwvI^2+J9y_GFVkMLAQ0eEr%8)Xjc4iCZk#o=|!cz&mM~*HJe7dU= zC_@3uoV*GqJg`qOr5!*G;L8_s#d>slP3;C&E?#JB5P55bXORTr>c0>5?dHs_}J{C^wD~S8E@yZeE0zf9ChFN|E4}dn+J^fnaWuRH)(g z)s_+5E8tPk_}mL+p0vOA3__&$Ed|%0dR9C~@AOQd(A{0uE!t3zy!-T1UKtti4$J4` zl28~SUdH;RYML9g5R_=j ze{>D%Ic1_dUZ5)K=Fsh%(L(7$C%kS4ErmQ^5j@~T8(ihl3P2Okn)-1kBDaiubV%H| ztdjJ4jI8Nr8TicK{t2-i-EIbmdm3lKw$Io#otGjQCwfxGAWmMAlYla)I30Q8xrZOH zj?oMPdt{k(w%Pb^Ee1L#nr991eZwGR-A=`&0>OPJH@7@MCex1z)=1g|j;7O_XD0l# zdglmxpEKPB@$kK#amf&ueKvKLyt}_R*=}!8F_?Fh8)J|)LJz0Za(OLN+mZ)vL@7AO z0mp1BNnXwU!5F;?Z2(NuFjB?H)tj3JIwVW0sB{Rp72@sDCai4NW(%1uK}9k z%L*8&VO^8q@V)yoNLb%%VByQCpUHs|+<|5xjW4|84_S+ekT$)V>g}nwb;W6G40f|^ zJ8I>5J!P9H2cgz^Ukw9cAsr(Dwcn|qjZnw4f9{WFRFP2uZs+BR#)iaX9M5QVga2RI*w=X7!t`-(Ilrxfh zz8%M$zV6YkJEF<4{Ib?DXi@s*6i&4`)V+#_WRV(?1=Vc~GAl~Lhu@ssL*l`uQVUFJ zH~=^2M@eHYNz{1u(1n#)LO*#7!b)bwiS8Gog5lDFVd9GnobT9x$q<;XTk1bmb#KX& zh)al$%sXoo22@f7nB1_L<*q8u$_wYyS5&1!ZcpK-XA029^=Y2z+!G*Kp}({4zCH=H znZc*A(?cr_+NXX1A7+2p>VBow*<2j+oq+3uoZGC~5+{V>o(8meU^^D4Yr4NX*lP@L z%a^QtRQo3J#vb5q8?Q#GVaWz4HDWWmDN^sQSLnyKmc2d=Wf8KH?9)O$RDcVRw7+H3 z>m~gUP*jk0j%|`(GMq&a;AdZ$nB~GbaEuP^&1V(1Y4CyAIRpmUtTk~rA4gy%Ky)jy zpW$bN0b&SSyeyJ+XTtBnhH(oD28L~hQrHwksFEldoqyrF=jVmDXP?V1gl3lKc9Hq*dd^JnaqIS-z`EHm+D!G5Y4 zCbKL(@tEpXpBesi6x@%FcwVC}Pb83pDue4rfsu?18Goi^4U2x4BgaG%jo3YJjWr=qD-zs}D!?1wjU26D%+H%zG#);Dw_} zkR7jibEE(AM$=%xFck<3B7O7Cm2-8&)Dn*l2F6iB{aMI>5rX({c*VAyk`_lM+s9fF z(isWC@RM56Q2>S3fGbkX9|JFIJONms5_tLGv%3baHAf-ko>*|Rr4R3}(4uJplnWnt z0sUdh4Ir@+;qP^qJ0{uIjn6|SQAZ4(T6-&FsPH^uIUo30n{J08-hQFBOCqvx#KRcw z0@uxpR6<%FEKzI`WRizUoL;V!o!g~rbm?+E2`h_smTrE3y*Hf)K{>zw&KjQFK4qza z?Lqlx0lhg-0Cl*xi%l!*0TQ$joWL)14~5Onxw4@Jsp>FE<}|!;@JL7I`!>vXo6_F& zXQ4A;>QldA?&|P!$c!JqND`-pvK@2?g3J76da&*QKTmt~^Px_^d ztZye>Pc-I@3t&$v4{`iPkyoO|u`@q}0!;XzNWvta5$2p~&HhWT2}ppH)^&UcBSOcq z!|Ii0#G)$n88%(+lcFNGf#b~sxJ_}J$<`ZGA;{j6e@nhELQQ@BoYn(zYzBESI~t)1 z!aYSbeA*4f(-dAtxkiRezB=F7|1z-jqmAT&A|u37ZS=s)H~YKdS0o{+^HQh2mIhBG z7w=mpT{tI0%)AOOIPm6Ee!%0Iq&QCTs}$o)lC2`zxmNK!pYt^>1Q~Z{9{wr`qPfBZ zQ$R$%t-H*vFx6PW@KYSa^UX)uc6|JjTx+aDG8*Sn%YKlC2qtJWGEU5soJ8eKe<3LZ zGr;e?yxNg-jjA|_6z^P+GpwaC@8<@|NP{bFIimoy;%$5>$~htl^2q3>uIYFU?u*v4 zCkdQ#YC!5^bMh``{mef4Jz<<7^eub8xZ?z4B(;0u)wDgHyC#gD$+(QcqANOV-j@Vl zLmKI=W0B>n3}keo#8m!$)V~9m@UK~XyRIU=A-h@@!VQaS>Ao@u+g9X&R()#7oExae z$ggf=2{AW-Nih(NFW!N;yq^5mO_p@*S&3z!?pOv|C-dG${uv({BnUo-^_Bylຖ?|Aj|*f=MyS0%?tfInp1!1>r~5@Cvg+SzlwDUxe!xlKUBI4DCWD%oXG$I^ zOweFMDK~STO!hcUFJhuVcYi>qe`L_r3tX9=(Rl0#iKT1r zJvj2#1$hJL#nbd?+beOwGqN;w!m8N$0?*sI+57fFvu7POg(d(v_~-Y&e?AseRG>W8 z@>uwsm2T941T)fE{_;D5@wc4PL>sv05521<1{ELz z&rG1V&f&V!<^hcwUeKqu-7JZ?SsVlikRNNKX5qd)5CN(^hYmBpZE=HfJIFS?&MP)q z52-KLE`TnmjUM)YBn-}XK4Tx-X}#FMaC#<#vZ`Rr&1bsM$YxX7^;>nS#W5$6gWtSn zdsr1wb9P$RTEeg?d#40?{?*dY234-Z% z-(L^#yb?&Y74IRB$$PZLu&VSk&`|i6JiPh~cc_|m0VROafv5$YB7m7L9CEUc|YAe2{EAY|0R@!V<` zU@@7wY>D_3&{btGy9o?uB%+x^lyDae?SMk-0=3wZdi8N@URBU$?NOfSjM$w5QWm*g zY6wH2{-P8iQ=^tUp%5hruN!xFA3T6rI8K=`H4ea&!xj=**>ZXglJWX!g*@)+zGn6D zr}FL~h3W$8?qK&Q=sT8Q9yB$=>hNYGAiH?HeRsDPLRWSx-}@;Dba9oy9WD!iSs3m+ z9Uqlny+ZOX@EKh_iN-A52bF9qS6k;O#opzhax`k-3F3x?BrPJ59A7V1Ya^YCFrdF$ z%XjwvZRMHPWX;hOU}h^8u&SYTP&w8!N;cpt(V^W{#2BeU6vrlI{GRYjqLin=v8q4w z2Hai6HT|Fz(~|1U>9VXHg!Gz8W;n6=KOt+P-A-OBC zL}!>^v!O2r9WjBuuiQ;K9l!2I^u&u~JN|Vb&&->C-&T~vHGCHcevVUtT2A%rKkrcn z0o>_c3g~z^`onq|YJ^TP^Q$TE0p;9gBTXZ2goZ`faZ4Le+Tow?7iWRo*Kp5B4N}mc zC!GUnjUqyZZ&pV1f(zO?iPM6UPl4thStxMSaCN@+=#8@O9vC22&>(tGKv|)ZfA^cl z?yX6W{egl%z;8&lN3oD~5mn`=_i===Ml#a6DpiLQ9)*m+A#BTGCDbD2kr+*fiifY@ zm6#AF-(9%3o6Ypnt$V?gPJdFvdsr7$!ADjQYF4=Ausu1ulyJq(VGkSHEr{}nod712 z7S?+gdr#p`CUX`RYMojNWRv+&g;xNekh>8u!o3uK5Y+-I@+*)HbH%*m%A$b z93zsTUe;fq-omoz1&-P76YJW(%n5cQ{rlP+8_i6~8cZZ}eT=7)!cbFiX@Zt-fgx}2Qk7#X~n@hS4YzVcltVd5aV zwE(nz3+Z{))QOgQXUYxt<#CeYx0+juM&*_r`0NO!d2Lih<#Q+uF+yUzdjeE({FX>n zsSCLE19WJAt(-d*H$iZswqX#4(i^y4ogqv`7Fr%PfJtI#TsXAk4R|zGM54Jmx)8Gh zV}fSa<$D#1BMoorD%v^VVNNdZW(*$y>hFV2(c3nJ8uPsU;pG_|bGsbMAvRk)ohbCw z@8QOci?L?GP~Tmz1iN1cCJk8-%LC?=}3t9+EE ze9Kq6Y!Jfck5;aA#C=GL0NGT=Mj&OdBFq?u%Iqj>an|)y-cxqh-m_OX8O3qG%@xEQ z;>dd17x+3da4S=Mip=c^p(rLccHI-TzNCg`tGJZy{MbhyAhADi7u{K#s{n%3>XCiD zTTVB{eJp8Q8&pE&5JnG%`R?wR2nzT-gEuF0Wo})eKQ080)8!)j^qwuM+-4jEVxx$q zw9H$gc#>GZ)IB!SxU5{9k!uj^x``bvh~&-0B;D-V5u1pKa6ME7JzfE19YIFJJIhVa zJQ~*4d{p*Pr0!DX5b?apvQxE)(3hi@08LIRqgc7y*N_&m#ZmH6lE+4_gaR47Z7D&S zVSepb5bDjT)~H-pZ)35%GO~I+G)m0NE@tN`t#Cg$;+grQuK;u<@cOC82<8^FD{% zV{F^Clu_xN$F^>0GTdT@Gq1gKc=-pTmKRZ}{Fmf#0ZzwV&$Tq&9cC)mT8Mdx=xmLz z*j5C)EWW9YNAZZbR4rb#E2={*&F{ekPw#UT5(5)RZ49p1_``zIUW$v%Ga^H1b|$b} zkPg4$Rr5j{t_q@zic!4zo3)4W(L&7i2?k=vM4*5Y=x~QTGN?%uPF~Wk@VK?j-G!a7 zkkM~sa^*9dn6-y^oIxrchu?}+hL}HBs7lIPbYCLrZFCwF>6a2vebYoycARpK3gjYi zo?2=oCu!VN*W#FZbg{u3{%VVRZ|NSZ+P3Y=Xtv^tpIK`P0MQ?M7bLw0eiL}e){bb~ z&2%A%#q^mvPQL1aK@&v=EtruDqn5>wi6Y%B;ZAfWB@X~TP)jg6`Yal~SS}k>j+-?I z7KdBk3K>0b87OR%9;~Ir_+~T53*a)dID%}@+w>n9-sXMV)5%t8Ud5_DYQk(a34v0; zu9Iqu%i4~B?# z5_gFv@m0S><4S$*&l=z`Xs(8zxI z{=Vn6YyFF6CRiz!sm(+JNwhbSY6jIvH}kYMzkFe`sK5apnoG1xo6+h%UmHe*2mtrENU*hc%{WAj9O7-Dl|6 z?n6p0G6#i%`e3S?lZ7s(8_I2CuW*VItc>B^Z+horW1pdo;VRdYaF05aY%?EdCZtwo z1d(`^k2&sRzUgv^Y&Em6o6vn{{N3HQyIgn%0sy3@({zY%^Ttu?12VmbdP6VC2E?u8 zBcJR`JH zO7mv!fdWyWY-Gw*+(I{76Y987YRzkrQp%3=Cym0M06KfVN!mhcQM0x0Mx-FI;O2__ z$MRM>!hL!75yT8q)XOF`9qKRyd<6?z;w)O|tv??w|(06SH-+>it0xl3fzD6o8J`P2h zX8z|>LALRRYxlthQQY2a=60*z?o3oDH?y zz4>mPn`r+)Ac>o_!gfMVczu<%?>){Ut8Px!qXs$`UChh- zszb3JE0suRw-1QXcRw(cIVdw^tUhKYxHO0FJ*Zb|+|oe%ve72)*6qEg2Tc!9Qn6f=6C0Y^hOCEvlFW_8iB*T> zKR+N@?eX-2+lO1t+v*RS-}C2j&(kc1@^mGrUb5yrWL)rUEqIkkp@xgsZ<(My+)HO< zc#~&&Ze`IQM`~y;x5X~0B+?(>BYos)kc_ zb?*^8Ux^-0FK-p4cwa(_L3h`Go%LYKC?T>=x$j`#QM)4>Ni`cf{3aT*)Amw z$Wr2E=W<`D715Vubq2$Z1D*I~B_U|KmZspcu=)A9Y`N%q*fu1XWT<|{CK;)%w8#)Q zb%j6bi0U+N*F$zNxhtuDc8IdL-gDUSJ#oXxcmN7(-P>Xxe^50Qv*T#TZt|XxeDk~h z#1xFkrQ4IQJ$T-Q;5)A)9OI3H8j>&Hd)hnfUYj_Bua8Z;UlK6^(H?M`S#DS&l&mX@ zyW@D;!3S~g4V$Q_TYIXyvpnK7gZ|ls6nGNpu-d1IZanO-VM4;v)9YuCZe=H+Q-;Hc zA6_K8F=3OevY!!KOsWwH5OrsZ#kJNb7QPkzBz+Qw()mJ5c?n_pbM7$>Kg^k!;QZ4| zy0(F8Y}eAuKdBSa0VtIPH07o3oqToLTeEN3i#IR=SjwAIG13Rpw-SXLol26>^23KK zuJ$0iZz}i@X@<^xG&ZmF(iQ;^P$F$63I#G%a6DaQsY>DwP!eYLgmP(Ih}o<5{>56o zH*Pn)vql0_ngMW$@*k_Vk8PUqkZITT^fOOWYuOif`J12Hyr`t%`N~}&kE(ep0aaM9 z(S3q%B(Cnn8J*>oCD{uKn$@u-PN%;hFZX}6x!VyH35ZwVd0nViyDYDr0XYab1_*Vdp;+{7mVw(406?u@UuqJ;zdmL?=F6=bgsK?rqR=(;5dF3=;V{awv z`q-{LkGg@iiJuNnc+BKBy3R|7Z?!445cPoF+tax2;e*!M+NWJdkSL=F5@GeXTu|hC zz_o}ZamYF;d7GNZm68<$Z^>ORmgg>h`>>O5OWi6)Y)@=?-DyA|53XciyP0OA=gD1R zvEv|yf|f1-lvo`qj6w%!zcnLWfyJ%NBuS;@x+1xrVfGkS#mOC7I27ZqcVhdUV0p}y zE)W`Gu3&DyEK2F@Dp$tby}*HNgC(nYqDGFpoZ5dXQ`?5Idllz2gy z$AezGSs^L_*r^`{>8)Gw_MQDRrQSXU@KNoFeSrq*1D>YIW7dl{$4ccGoQg$9I5T{%UJTQFAIPu1L;Kj? z?i**$kyiyZ;sTk^$W{Sv5G0Q7(Nrtn5V>4!XCds5tvyC5CA!}Q$`qRm>YzpS$H1q{ zyzG6YEv$t85IP72r^%5>a5{*LVd znqj%ylgkbliq`~ZoWF7T1(NtCX!L_27sXu?Edq20#qEc!pRV{ZiXf3t-?m(FsYHaL zhG(NFx}+;# zq68#3QO`n#ci^K1159>CvmGWS-vc3FM_gUdL0gnxWr(LZbx&d0Fl3;7Y~N|Td)Uq( zC8E!z_euGJa)&xd4!2AJadJ$mD6GHl(^xQY$~J4&{2^b4XrweX zh$I{#A={IdokdhGI?GiGueu>#{Vhs&QP7(;{j0;PQN#?T>bld&d-jBbS@8$oS!)hy zFAxXHpjntL$FDcv_G>?Yws6ivxE*?U{yB8F2PRwy?R{AiC)1`2Dmxc=ub8IpXAj#! zGasKaxSXf(j38pGK@_oHEB%cj#eNOCV2T;{f_0QzSdoTls1V*PjrTYSwLdjjFe zJY9EDYkx(t%x#;wskOg-zpwT;N*<9;(7-SjTgW&RwLyU}vyM@@C@_d2aMwFfBd8N@ z5c66JK=}b=yRhi;p8V#VtMn^`J44#JW#XxNat7!^P@iajmLBy)jRs-ecuj)$${QnN;1@9Vhlsf#{NyzZf-gJyGoS*MK^Qbo(5X~!EMgYC$Ki8 zQ8};&`g)eLxk2xHYyadUQZw|Ba>*~wa`u$V#3k5{86ru!3ZkeuF>i?7k+AK=-;Yo7 z-o~J8-^=r?o*ZS}t8qGLL?{77{g%Z!at~TV8N9_pBj^EQuN8m$-VGA@h}j zYH4kENP5lMUi@l{nx+}9WqXISttOoTLJ}V~zq~Q>PUgnUTe0+$hr-5#Iy&oey{6Qx zpm>{5^HD9{|9+)KUfeHu^F&SR@g<|eP@cVAo@@sUU?G%}1>?tq`1@0%^LOxB8P5op zLNbCDGKwH79R2Or6*6?IDHb!b z>C^ZLZ2sS#0+qB1pm@=_2R9D_uxT;9PphGMBU*bXTFI8cc4Yk9JL0S$++v2^-4>9k zu^p#Nr_Q$LMPIcG{WGFX8zpUblOjZZ*mEfPtE&074~=j8dDhnt3eZ8lMbEs8- z8ad1@9ghqndP;z-GM_%4jOJ&0XuLjTDFKl`f*4+jvO$OhBS|14#g53(uhwk4!`TC=%IMe?a~i zhO!z8XCueSn${7)!rXk2>TjH1PqlcZ$13TM{AW0h%REAgS#5~HzpY)oXmugzc{6u2 zBI|P``$`k<{0z=}yWZt|5-yuwx1%h~8}#ucsEURbA9w?D#I7gK83*}A0T(i)L-!i5 z(&r1GMQc5Q-Q}gHEZ)Ky4w10MBFh!?5)uU_@l4+Nj2LPD`I{MUOG z!=tV$T~709-dVMsZnp@?=Hx-?MKK|Oe4$C9g1J*FgNAJXH$Zk+dyn9-Ym zKjgKD@!*OXvu|cz!n0*5;wevgUrj@R-2v?~XPraAH&t6Fi%O@Z++hy8pwc|46H!~V z?|dC1puc?M^~IDrQ4VbF7_OG}q%Km0&4}ctWFjKermoV@(_TxB(V@uB09#**1845i z;}ox{X(V0jGWq;zywrYzGhNN{(+S<(a#W}B)TNZT2T^Ju(p2BnmW$G2;4}INW{NO6 z2^x6Di5v;{=H9|Ck-RjI!Y72mBp=w{8-dmzHd#4NUg_oiy5Y=&x4R9S+D@QNw=c6@ z2hx1+;@npO!qiwX?dlO`FXvtgDM`j zr~&Foo5vbpIj#-ft`0@7Tb#}BRPUVff};BInnb1j4eQyR&dcFOi<_Yedxut|F6d2< zeufh9)?l|6^jsE*lJ`>| zkYZYIa+GRQVop)Xtw%8lr$AP4z4ttLC6VAl^o=A-(fAakl#7Rb2~6-itNLAVYDfrD zpE`w(P%=J!bjAamu|pH~j^FI4>S>IPaNj2FBiQwUq8tSIwJMIRS+IJegw2ICR<&V` z7AULJ@w=^LE_b;0*u3B1@IkcJ+W{!ysd}&P9MP2)^TFkxu((9u`~qjB{*=w_sBQ2o zWj%!zD^d`!bB25KQB1fZk{J}_zqZ{#&G}jplY^LY>gC;{&@=c^1kOOD0>~cN@2WnjHZGi2}?1P7uQ%L1|?tSiz zV6A{`X4%4{iJR75fGtOm$<CTTY>Uq5M%jq5#`kh-&-_-SO-9K9!0cz6qsWF(bJDdi=3A1MoZ{qm9Bbx~mZ;_J^8zb`FWWGm>i7D(w^GAaD!A4r?KNtY-D_;LRcaNN$p{ zHjA2kit{n!k!6~1c1Tw*hzvTW;tlaArtP9I1vx@x^V0IGOc+PxTqs1e!W?*U>2Xp^ zMAk0k5SWH_mvx9NR^`c1!rxdC60O9PZXatJfPtDgUNx|!saQH4k7(BKzS5pR?_=@k z-a7%ilX$5-l-yCC^qoodEUqw55rOxS4SL9S!@lzBcjwIa^>4r#c|Bz-;|Ic}@5O?| z-)*lXhKQ_xwi2t{OF?4tQ+4^P8Zp8Y3@;nGb1{Hmy%tmJ&7>J^I?exT~ zyu>Buh`?moo!` z2&R)!_Ha50kylPe_*BADE-yWtZ*BaRRTRNJ!U6A;C}|LdhPiNe!x{Lw`_)0f!WvDF zkLq1$5HJM%iYzP_U1J%wpbbm#4E6k!BX?Itif!$-N$?S=`v zfFT|?Aa@yJgOnzO4#cdL9xBl5_oUMqvbRm)xdvmPTjy0Z0vKAD7b?dkA5r=Cwju_r zuWy&bL>NHtIUO~;C2nPXh;|>RZ6X{kGXDSQdh4htyYFpWdXSnShaN&gK)SmTX{41d>5%Ra zhE4&M7LiWr5O7ElP$?xO1*JPgeb3=;oRrm_dfgVeeG*sCkQB) zv|!I5IbU_KFgte?Tk z+vY9oHgk`l!3yz)4BgEu(=}kA<1fnuh$Hy(hsGN!tzg0t z$rVky>=PkMsBn)B1uc#8g9NBUUp&(m&!>CnpPx#e9n2ZMlNz>l_V#bcRT3Sq{{SC5 zDMNfLK_C8d5!fI=qr_PJY~I^O8jaZd{{h}jxA6#-W@H|4%REY80L0oH+j9WFB2TRD zs6d#j;s=<+ryGb=Vh%I!C6 zHY&s%w3q&<7JmHIN!GevKbGVHMkvKdma#~n*aX}_sRkR;##%4Lq{!+vkb!N5sZE3; zMR`c6sTqcXN#B0ku1>N8cO`7nmq*=0V;WPwnth7VXU1OrXt5`&B;jdfhffl~JUwY2 zDHK92b8LVT5kj6^0b*etcgwn)veRbBOmQ~{#1;PU5xr)(x-oR3Z=STLm1O(f?{sd( zUD0ZD1?k2?d$h4t*AuoWb&rRd$RA0PJJF1LlS#(tJWU@jL?Qv>EdcyaTz>aIYW3s4 zK`pmKZtbC|sH|L~=8A%^*sGkL1y&pU>UB^c6qOj^JcxOgmi8hp{4S<46;95Jx1U5F zoiwxX4!#0a$S1e0f1(yA=+jqM6S*9}#%7t;4OmI4?%6IgC1B_Bl5FGl%`A-$QHPsJ z4l@W=zVRmN@Y|2Yrt5+*yi87AzgQ8+|6k z=r8c3+slRF+ zAS3NhaGoH0ZxQdo8SG7R#fo*?nw(_MHINb1m@{A!jgePV67bLP75x>EMLSa4`}ts$ zPxt5M)D=TIK<9Yzo$p-tF{U6wmsOjm($FS%6-Nxnt&^T}@j&mO$Ly_JDOXkJP z_)Bo1qB%zE83qLtns}FwA^dA!Kx*h2b>91RKOBn~*%5MJkt_+I*cHzzo8h++I(^UK z7SzsVW*(oU)Awr9S9T0WR-9=-U8x!$t8s{JAPNmViT$!jqai9nE4=E#}w6&5pf zqBC?h0c)@IZyKnwB?tn3$4E+Oh)1z=0Z^=inunNc@i&;J@6VAioXvp|-_8e6E;qd@#FgqhP!wL`213j1y9%3k@~=x-&PxWN<%qx&MXm z98iU1%%I7zCG*5Rh@N(<#DpY}4_f}r{WOt@4k45d;iJ|(Dt zDGO`Wn@{TR*3)|Rac5LBkHAhjS}{+7zKg>4GfK4WYqdSxU)wT#bVX0X!p4*?437R* z!%taWOfh%@n3y~E&V2JC=ziWZvdK&K99YU_e)3t4g7u(DVXBVU5wb{++pWwZ>#97t z#`Qf@aDe=Ua^_3=sqU;Pvu7Z-bRNk~J?bmt+=D(jPh`9FF?gw<@eA>ho z@t+}Zp9!#LhQQ#5J+x7=Y$nA4h^#;q0%*>pTx-mM`d}#*SqmUr)?9L>fvs?V`T3eSXUIoYWG@yLEc}l$!g?om18+dw z-$}>4Q_T3YwaJ{n)vfuwCfn~6eeJM4Qm?P~kG|VWf4R+6s#m~*S_)B;f%i?~Yj3!3 zm&6Sc7F|~LzfTgvSSz=D_}<^TIt5QsV~8axHEPgD`V4o*GEcihFnNcj0Q6>eD)ilF ze$+~2*9-=nt%TkR)&m~+4+@nS0_F+!TuQ(@7?Wz%=>{EPpFBLuxOO!f6#+grJM;E0 ze+a1b6_Dqb5jPZfDMBlo8KoG=LtxgBv=uH~5?_YrQmp~u#h4i`NwH9p0EW^@7LdRh zA>VzM>MrHGdqD97KLJNEpwp%DG>R7pWr#AflO#|vuWH;ANTa1AXbDB0KQ6=jjPE!HK1zebpr{qOp=`wpjVHJlHe1Zj;k_*XeuCj z)x8IR2uA*U2oGxk4=7>f)vV}^y65ovG=9VijBF^y7 z6j;TgVNZ#3{1!`rBo)m`;1$lOz^}8Lynt2 zZhQotab^?n*P(us`Q4`^0*q*jXk)0IxaZH!>1KL8nFDXD?y=PInC?fDpa+;_{%!Ri z#&`s{Cz}cawEMMRBdQb6s-nw6ETrmyN1pL+-6Du(_pIxpb4)BUp%X` zmpO#d5E4)m&_>0a(jKs?f1;R30AhbGZk4|~==`@dWOzi6CjCK*1` z^k`j@>BcIm$^h@ri&`-2S>a5QfM6wV?VBTbdJ6iGy}_ecUxyCvkA~An{$vNbZ?c%4 zGie+sXj{}z54_}@Q!a0MIk8hoyKMGII$0<7jBL-F^H5AguV8Y}P@VYUvF&s@_3O|Y zfGqA75p$9pY=SmH(r~nyYhE3r6#hs5*#^E%p-Q0xebqjc8us9tc5!PU4-hEY^O4rN z1|JU<3-cY6U+siDyRP3WBPbeyW8RQdvM-|b-#=XW0}9qZ%(+oMn7n;t8hr8}z8%!v za=2^gXP$^2&w}qpw5Yz~?fyN*0_azP&|a5E9%OT(VKzA>j;zx z)j_3sIq}UO>GAMuU(-ePb5YTg9dagI2LBtnVV$W58n3k>6znsER%%FI;vBMQprgTY5~Eo)+Us-?eo%k?XJ7ohCPI*b9DbfrES-5 zfa}EK@;1VY$Ktta;4L3YYj$RdThq+Hw|y8Sm*N#rtm{#XTx;>a)#y#KqH=LM1xCL5m&s; z$hTfvsiRW1lGo2g{kZ%zD<(ysnhU*8c^8CVk6lXwBeddt(xK7FhFi&XN-8u5Mk`%v@tiwnWL5bXG>qW(by#3 zWixy}mG=#~ck3fLU<~JBJF!N*AA24A^PT1hvF4Yi3MP4Y&%-o%#d$&grEStWpsYU# zXu|86=lvC(blvu_IW$YSNUyJF^5Hv$R2+JOX+^$f$?M5&j43NxJKN6?5y9(b$t%-&`2vtLzP=^Fmi&X zr=>q;6@I{L$0t+IErzHjt8^W{r|7Cszxu-bTq4B}-W*md%5EbVK=-1;m_Yj{Ppn=G z@a80u6UiI}jh_s+xl(E+fqvT~%hwTLI`H;xvLNU;sOmncF8Tsn0x)x&Bu3qDghd1& zX+j!y3eE$22sNwjsEf_a=;p@D)bUR}u)KR>w5F9y}0<<7Y3T zQy{#1>3FY=iF?XPba+!;y#Ae>6U&k816b2n*JHDn1Jm(n&M{odf8XPEX+Abhb@t#j zn+LGj8Nd)=&tFq3(Cu@(8cO>DI$JQ=%>6S!Dih{LrCQW1RtL{9p(gG$doxHpc6{&} zD24zMkTF^MS{sF-bLfxvcV10CJBwXRXKE?d6ddJKeAmxx?_u5){D6pRc7QC9%(_fn z6m1ZR|1pR=x_N;44n0P&FmJ4Z7z_S$v0()SeiBt#^y;=q%;+1stbFy~C?c0lZBrb! z6z+dC1=+0aV7*))ZLA1T2S<+15a!vwg(W}oQ^y|=%LE#@d%4_5@lbbpFRN8Oz z0|4O!;4T2ip+E1}(ZZ5Bf3dXjr0mf6sKsP4XsoqzgAwU#?r(hCUE_%<%V{j@-s? z&&{eJaDPP8vQz0mwYgF|081`t;5 zpI?URBkwLK`eY8i%cgR^Z!q~AE#Zis=H~w&0x?%jt2H56MHUO&FKNIdURPU|zjq~i z+a0inCo9vTM}TFoj1l|9tci|4xcdLHhp{X^$F3P5i+U5u!~`!`)=q?X-#$X-b6i6~ zzqP`7&!choi?aiIGL3GZMAmm;R71kh@Jbbs{^_~Ykm76^0dnBM? zEXu$LI1H++J*H{7;@h)|#1aY+{T=6iIruPu${M)IqeaD_ii)34FSjl(L( zMYA{H&r^m3KWhjo!(z6YZ8}a2=QmD9c|)kl0w}B}Nv|ASZ%7wl{ z3-F`GKruvn%1fW$o-TXHz%Rv()~efd8zczj0V8OIheth?gDjxCQild>cE@9-vI7BruV(z?O3H)JU2g&{X)`JU*L9FY`8Y)Xck*R*6K zbK5lK;w{(j3}3Ek$reUB#~079nb)nB%zelIXvqzdhj&o>b{l{bM1%s6-W#tH<7PBd z@`_BY%W_mUZR`<-SyIMKY%nvckqXiZPeTfJE*azZ{%2(^1Df6G0ebw(x=$wPe3zJC zn+G_O_7Mj_#Q%cI10Ow|f%wbN;k{BHF%eb&i(KIyns)I7K1~kBXI!pauQ^6U)_rKQ zy6m5&gZbeYs5ju_4+QfC*C{9gJxXn)@wJ{OV1a~w#O{l=j4;;Q%i5U|n_%3g8Ig{C zeGWS8pG9oZBr)(F7|(yj(`lR5LrS_qljW;as6S$<8Tp=rMwYx)1JD&HaF7 z2ua?^iI1Of(t_F$EL8<4qu$QgK_rByK&MOVkM1F|w&q+zgW1>L0=_2;0nKF`V8 zDSWz~Xq5pmhl6_n{th%@#SDHxgBM*Q9E|n2DYS{HY+kPsD9hZ0Gok>b#FW#{&37tz zK&^6$S0?lmM4TM?NyBc}Ov^N?CdqtW`qryOCekl_meneKxq3UjRXouH|*jspOr! zYyNXh4#zo!tBEiL-msYwo$@#BM?uwoYm@aFvbV8Rs-E`C>aeSxb_dx~wW$(l+hcQE z4}9Ta{$>3X4A|WAevjsY0&O&=rN-j0)){YuX&e87C77bD9Ux!0I!Iom4rPBJKUa%! zAiVekwS#E0t(6)!EPh(+qDz>;>%g({cSE#7e@3IBrHLp|p$B`|EDVMT0>Ojkre8Ur z@lwRnDaInQ?0Me_EeJrU$gHi=W!607L@OSOzS+cF)~WP+2AV9RKzN3~R%si7&adg6Z1jRp%>71Za6Po8t#(U8`s%o0W#ApaijIWTwf@{P>O5hZEJdL zv;$Xwh&s{14fmVonEvv&Om2%)@JVso=Xwr!fW3a&d7h#6l!T;{oT?u}!c0QIq-c^1 zkIn)z8O*zH;$90_DL^>?6dn4t-PdV97oUB zpIg;rL8JU5Hbktn@?5~B13!sG6|x})*4~5)ofqfBgYt*0q@V7CZE%zSk%|JCCG9(F zDDprwf-$D;`h(8_? zJe_G5F-s#o^Z~8lKPz?MAeMyt|5NRD263Kzn}X~NY}ui?HVTB2EPXMkwzU9pK!9J(8iX@bv{_pY z;L#*5q(Uk8zawJ790!se@GTS<;mw{Z!PJ18v8p8#Eouec+FZs&6gDzBJpBTyxNsn# ziexe(UL^+;s5vg9(;Z+Z5C@_rWz}Sx~VL?G8Ses2x1V?5ApQN+GMx(sC@O4@ew;HYjG}l%ZL_2V5yaeGayf;C0kwr^?zFHQR#$R80A!=?w%F za-c1i&)3}_{TVsQi<3iL-g?id|ILJ9$_=x+b=+(L_h({@QAZN_&3W0An!$TVg3TQ@ zGuPn%hGyYtHynSFPdv>DregF6{m zQ*Kour9!D!4Fvy{^}gu?~3*@qH?bt-g; zTJwKPR3Ro*YHI+R6tFwT>Rw}4&W<6eagEWmr^t2WQ*0j#0=dP3JpbAHsFE_!Nw*ty z&~rlHV}rP#Ep;m|_o^w6C4#585!Hp}r_MLD&ktcmK@>mz@hYvZ;98W3_RQE&-EzU) zX&=|u+pLwCVL_!9q;;xS4K}h)bIvX9HpJ(TN(k>=i_ZIyoD@l|glGee>dlpVTCiqS z8`32Lh-c5U<#zRMm~}(k4BOaYRpZ#t3;Ht)>-xKEhFs<3^}{F+he1L(A7TLJ{V)-3 zv%+!L*3gN&!Cx&5jPmj8(%rq>KbMo10g5=Q3Pqhf!3UfbCn5)AMJ?4^`-qcxI{ptY%(9N%HDFwm# zt}oZ5oVmgaiFp9p@OZlmK z3?!N!Xpr*b-8^AZxk6uEHGHYwqVtkRxyh7Fw(5dLrLXUQVQzWjH`9w&2DH7*^Z&kL z;sIP@!z&%d!vL9WZZKpRF_Dd?`pWWEmt#w0%1Q1W-G^LOjx|lMcC9;TOr82>e+D}^ zq+R^~`(~}%E1F(t&X&d{YgWGjg=f?gjqf8J;0iP)ZMYImBCjSy7<^D$W2f?0)^xto z3>>c%Gz?vvix!z_ zD!~dEY@OBl1z=U8hx1X_uquKI`|Cv&`qsRa8kubc#qJzH$Q2amGs_~8@t+~47GV98 z^e}V(#;tO9c6Xc?30lqRPuM5Meo0Ym14QZ7iGSAxwSLor*CCLP0FB@ubq5}R0dal7 zp6y?|NW}3bNR5*7@1VJIiTx@Z7h&5Eas2NOo^_$`TjeC~FCypC^7I3LvMITC_2w6v z$o+V3V$f-U0302&gYpD&M4*7j4ygu?Y0hfqI>3jmB7%j$JN?3k? zIM&(0wK$XCRE{U-^w`mOE)<7=C#ktzEzIs)!C-^nA7_tOp7$U(Y@;IhqFHZLL`1-& zaj>z6d6(|NU zVGdCdv(YK%dVeJgJe5;v z-T|7y`AoELVv+bRyGWUc{GrQ}Ejy?5dU-gBCQz6?8-I9$h|Oq%gzJE3_KEo+2hrLBVW0@|Ddfcp6UE;^4_t&iSCmYINjH`+ zj^4$RM2JN!z0Z?unVeL--x`4ugz_Ud7h64F-hCjCTDqWKV?@eFQ6u4*mZHYbg&sM6 zX|S`s5)BIryPIx1oUW`ALJNVv6aVl+G#dpw79q6XE)cgF;P+1+Xk31EFMF*~Y%~Yi zi>K|V#yqlpb#bH@*#oQf4!Ur;0$AjYPXzq}vv3S=m`<1PGbBG%fFy(8)w$uiBa5?_wOn=TK~R0!7t}IkF?~QC)eqROeM%ohBp-mI%wsv2r8orzmhUL z^?gD{*b@ygW||AtR0#R+TLxe?DFusYsM25EaJi~9s?D-B-LK`CCe#EDkVFzp{2vGC zMms?F#-KqbL4IlS$P4G`sv=$r$-fe$b|u&Nu+0usp52 zU(W1pSVVuczpU66!D3LZ@l2p7c`OGhAIjx*OTJI;SATBBUFceW5@|pKM`Os&S7A;W zf`%fkJPG2+s+|QTRx-7QX4kpfKQvC*3Q?SH(brK$Ma~Ga-=$#mxpxeLy0<@ItESgZ$97~Q9Hic+u`zjcRj6&x*#=I_W~!iB6gc$oiL zK!zbI`dttiEsG@)X2r}MGXCk&Ncyo}`$Qde#|A_g<>~H%z)N(4k0|1 zmm}Jk!(hwa)H`8YA}u^XP-B3o>cNkFf00Y3yHlgqee^XePB#J2I>4M}4tG#?q>psL zcSHVBQ6hY-7paPpOMt?dUu)&v3_k6@`0j)XUz791SDHpN;f=RClCY{e((oe}d%atV zQ~Z0qoAZx|Yac0hrB>)914QZhP169ANZ%&t>p075jI@u&#eivbF z`LM(HY557lrjOZr|05|XiK&A6sWk#4e1%}5GUACcDn}4Z=+iP=Pq6Xz5IEGDcRBw3 z^Si+*ol`#`&$(%g?xTa5)aVLo%J+AKSsinoTv{R;&;aN+07@iGpgqwWosB{4`@yV3kyZh?%9q%v&Y zI!(3Qhx{GO@f(Lhd8Bj`9_9p0NgX=E%W{*p|CXG<(p#n_0(ol@U&Q!?M5)~PL zg#&WV2HPbu9GuGoyIx2CiIl!_a{XO>8@BF&K0Xp5zYcYi5$0LRnI%h)B@<&h9;BL^$DCe1FGm?cru@BLZ>@T0GE>?XqMZ zcvchsH|a@#6?+yk678d8%K2e7>Z7J76}p}l!l!ZQPlVdx#}rYK z1A)-SuPxv|d4?ncx)}82TU6d!*W02YwLpr^F0;hU%NF z5XE>>KN1U7o80a?H5i5r6jX zZv9E3=dZ+YYwY{(nDGm%-{I&$V~PDZT+V-&;*YzVe&+0tH4X+wC;5-*|2x0s&ZQ=fB z-66lq!v6lzKpA()U0A=N6qgw{fGs%>7&zn)ez;C+F0QxNLTW}jg_*)GzSjscN7To*BtTG&0W&y17~XP8O*f#GLVTEkDIQLWz{uu#hbifw(jig z;(He1sUxb9IOOR+ynuDbnD1 zJ3DmGEelphgsGkETQVa`2_g05@7FRL6v^8xG@zJin||k^RM0el{Pmd~9sggm{qWBJ z!li;xLK`p{B5bpSY#fjg#LQyqnQ620$B{L~31tk{`uKS@kHcrGa6RbV26e#)$Sh^V z_#VT@JYj0(FDGZ@x5maZo10&|O1`n#x?SU4z2Nu%K7@?_GS}fW9%jkRmML5q$g;LR z0E~617+!m{{~_+?mHERwynq4uLc9*Hj~#e(b}2BHL8g=jGtR(I z%rt?84~@Xj?{xlN%wV~!5>}ZnkolLHecX_0G)lZd#;<|eH|T-13Sm|-`adAjpg5P7 znbd=vP?ogHwmcR-_wOW*zfs)Eu+6P(A^VnyPX4&0pTq7I7O_O}#Dbw`o-tk~xgB;Q;x-WpUIKB-Jg z+`CNA4hp)2bNaM?UCyA!sD=)m*N7_rxQ#9G4{VFD~+^~j*?kf zlzThRc}RBE`2U^E@$72u9|FewSXwrRIYNFHir(tF82*!!f{uq*`6u42HiZRkmfCO( zGw`hf&QtpdO#QhPqTPko&YFaRYmaW`#o#f@7==M>)sIu1>?GwaI5E2}p&87|SL&V% zzlSuheyyx#zF8h|zw|bZP+h+DfTGn>m#nkJ44aCviUs>Rm^>D0t9Td?5I`y|DVZ2; z`i$dSORDv&^6GX1w{c=f*&_BfU$lqcFdW=C;}#B^kj4zooAZm8k&#u@PJUcCIb~cn z(=FKB^&;Vh-jD8(3$?hzG5h5QI$LSYR5eAUElsKu^V^1vIW*v30B%XnSP9l_Ra?3SrsAroE8`qazXM>KwWm@ zVRdBa$K#*>-J3r9N_TmFC`EAc)sLptj|;ZnB~u`CnOj(*{3nYCyEBZN1@&b+<;LF5 zX#v)0kHo*1t$KmCP_Vuwo*k@pdXtO(yKueTKRl~(*4-FY!~#CMhf*gsmTIo> z=`&azAx8HY&%1jkAGL{F*v?Qm#$`srb@Tabkj{32q^oc9q~qQCLO)U@`57@5=ROR^ zr%Y1)`?QzOE$kNOA7rI=2M5DSq%-&5&rTYdFwUojK*4!E>p;G|^^_mN7#b|XvIK$b z`?!5GY46|{y~W!eUd%RRXQA5ar_ys1+x!7s(c<^FA)#9$Axu3%9o^rfy1=up6N009 zzZ{lt^QKxU{n=GWW)hJ|Gr~UBFd~26BYU#%3@QiS$G$oBbuVSnzU-lt_`hE&|A<+y z)X>5}^gTGXo+I|RM-lOxlzF6UG=+D==Ho6P`-^TehQTG>V9n?gVCL%f!S}@hi(3cj zRYMOrwPJWN>pxGi6zKonY%HL9RqbPPS9Tux|M!M(O)k%`s-Qnz*`ous=f?4OpTQt_ zm@@2vsb-^@$K0v<&|If4}8 zj#4uI{$2lkFP>budw{{y?>0IODm&J!Y=&c1r+yEGWBfwLx8HTKWAlR>8yt_8znpqU z2H9ixMf7B_xDHEOF~q+p_YpH))_08nPSPy*bZ!#!pJ93IKO~2Awo5&^`n{{0j+wE& ziQ`vIigrcyZ*QCu_$O{Y+Kq+PF``|OBq;Njk9Mv$3=-Eo8S%%4R}my~mxP{;jWZ`#;P^TbEXpmdd*+Q-h!7ib|*b1hQFlf|$gv zv$ZSMY3z)0{Z;@X$SFhRUIf2R3TMj@WRnOu@{TAA_oTDzcAJARL~7Q=ko@(gGQVD? z;b%lkF(EMfQ~6<{M<5w%dasl?Kvw4CS6j5)#f->3VMfNhs!Qfa})y)H(4 zpQ}~3hlF@@cAfpn21jU>LH2w~68huK&OUr0G!CD|HgwB`O;DhX>mVVPJ}uMgUF!r} zI+keiuW(5^^L^^*Wo8OodPe~Ea((+tQLSi;QF_6Qh`PCT8-3u$9oGo-Dk){YwrdXb zOhddu?(iFiUZwZ{U1grIY=&x^EBP&7+$JJ#=Vg`fvr`YnA);CQf2NewEHPZ@?&?Th zoHtSARImA1ac_5U038fnwrez>I}o~Ha*2*WHgBS>DoB4O@<;t{!WhdL_S@M(jNPX& zo$#J&3Wm(~ubf^9`48ls=cCoJ{oRe?*jirQ`j1mjR(LuKwWbBBR9wno1DqR=5DaM#U8P8BB_yX z!hGUw^shf*ZFwpDpI;v@9Qb`9J*(Ai^jNtjW-b{@V zf&g3O^l1O!LOjr`BninX_A+E+o(0@Oj4_U*%c(CYTZ~MjXdcllmkU4hrQqpq1%(@^fmkOqWH% z`#a(K=rh5I-@6MP`hC&(kCu8Oglq-~f6liF0_5!O9|j&%q1$>TkPms1*?J{v@@Xup zr2zTRt+-!~IbI+eCR1(N6pu^7pmFP7R%Y1W%M-4UU#C2Ed6Yb+6S?Bvj-xsJLhg%F zfft9j?8b7ReQkEVr};w`C*?2Hyl8`6TcIG zgJg~qdVcy;ib@G&j##9BFI73MQApDKC8g1thaHYv(|fx1{upix?ZUUTa*eyoVpLGu zUyj~xc^s_trxWu1p@qD7^tp66dkNU+qLB%1 zWwn!os>G2#0p&KKO?TK|nOE8c=IMe1xXifD=Rh6<{IATgxXXMi|HJmjaUyQ>@Y^u` zGM%EH(oZFKS!U{%-4RruOEtM>RDg?e1|DywS1GHW#awj}m;#w1(DWCX5bFS`Z*UIAV4hC__b zh&tZ?vLl34$qS+I3Lpif`4dR^Rk!UhDUGmGW}*m$1cs%+?l#{l1pe!JWYTC)ba8&J z1J<)GV$d507mGyRRQjDO6`-fGi%OtMkz@7QoQMDwEdu|Bl3IyY-t#-=oofD%Vwu`s zswB|}jNr9`_#Btcrl}4CU6Q#ja0OP#Pi{q0|pM-`-lN@Jk1sCIWcn)?Mb@6tG;snVN?`etdqNE{2IgDjOQy zpsF;QC+WwE;rHh%K%C76>x*uqy=Gj7I`hB;-sL+Q+*DMtv0!8(hUjNS}&J_z@)H2pYv4`y78bJavRlZ9{X zgEob`>8$uqk@A!Bm4?-82^-)lnm%Pzm03$N*x;OZu{t~L-)Hgg?YzrBvW-VZ$f`9) zzC}pMh2AgMQ!XvKL2z}nHca+xQNPNV|1N7+$giyQTlZ9YTtL`*eDc$P`ga7Ce<}i* zD|b7gJ_0F!e+aZ>9M5(a1OyznXY12bo4cb*>;>1rYZ%CIf%@y;kGH(aYTbbukPXPF^u&) zIfviot80YK!^(Iz2hS;*5tldbaz|%_0 z-pE6L$K@ruk_G^S%2gmFhQEEYaQf8gf1q&MV%du+5qw^!u^!2QkEGws>w!Ltx){pg zf2nbXnTDl!pXgW}Ivf?Y^bV-Pv4mkUVZSZ(jzrQ|3tJby#XP)eD&I@Ag<+#G`7|Ds z=sDb$grQdEO zdvNdpqqO*Byq=NlR^-bKCP^~P0&{cbJ**@S%_M>`A@0}I1baJLf=};=LjU8N3Rjlv z!DLnFbUXUK^)<|-UNXOA$EXV#u0E5)=IEKuM5q@^+G;gA)GvAHp+D0@<*U4i>ZN7Hxos6??+|1JN z%Iu8gTZGyGCPogGqYwb5YhALVSAszBl#JjNY)@2Ba&M?Og`m;Gvw%r=>$40r#EMVf zx@$sLeo}3*GXN_k>N`G!(9v)9J!nz+533vx#gVaNg0IYDU6yQvu+|YSclpvF(14ew zgmB)c9U`$zmkRI_=iYpY7#V_o1lNm*gQJAaXW#7KFARjUKJjGN69;>(yh0J^p^UNx`Enoh5qYgjaLejS-nf5 zdbM)?)T4Cui%ZTor=Hh^dOm~`>RKS$sgHqqtvP)mJ;DQQk$`h|QA5R`2j7!M<3KT+!8FzMRo$?Wr8i_zGGGghDd3cd> zWcE)G97u9g9PX~h32*=Y>Ch;OGStrEHevl?%jb-Qk23LL1VD7{j$@k$6`;(0F-VqU zSS}I+ADaDLT{Tm3Mw*jW~#9W2fS|ivBCZwnd(PeSCHCf#>k6LByx2wj#~g`GgPG zrO3P$?c+kys5&H^mlr5#Ke_`uLU&DyNg^?)vJvD79JoKNmH|LPldyNBtFnL)cUAAk5?@rvTQJOkHk<+ZcoFZ9_6B_}qrj*o4B8 zqI~f6-p@vo@t+e#>$bQpuwvkAGFlRTgBjSDmv@fw?95T5oaIub2{c{)j91*?m!!sY zcdKQVt>ww=MBQYkedbLV3wC*qsuA6j|LpSO@zAqjXERTAqrIAplu4hM*MVdcv|bbT z5yqV6+(eJqdP5%j5*~}NZp>yi(4W#w(Sd)b6Yg!} zu~-UY*4AjSgu!6>Yb~6HExqhti80J}cAAfJ%`y4WCIsG%Hl?K~oMstEaj}X3~}P!pQ_G`1qzfCRm7jLll0JzBpoDa@_j(=~><5 z-xcj+QNO6C|5*QM&0RicSDniUx_Uq^>TTu#Cr;_@hxvP25r&+EDFW!s%aiq3JQ%Fv zl8z^6UE@E!%g;ag__KHm%s&A*mQ!(cD^RJIyBx9iTN5cYw)x=~r7nc`23#QBR)GxYy z57XMm_CeLUEKmG2*u~%jo?Q&1bRaoOI+Y)4n~uwj7)|0NMN@Zn#?H z?MIyaZwXPn$h!Hzr|Vb?#_6e{Zh!7nDPH(pN5sMztCtGnpqzUZR5pZ_2hR*wIia}6 z?|<@>LbE_7&-9HWIu%Tx3e>s4d*!3cJZnk9AeCy6tRw##v>+O^?|>*U zvPgcVO&w1MkRnD5UI87ubUB5$b)a(8X?FQy`^eJfUABKFNet5=UI>dcm-qM4$!1qG zr%xquS08k41`Vg*YFNO=C*5?T(XEkCO{R-J9Lb{D@0}e;rst}U^bj1;tmMlaSNYX$_0labyI0WES661m? zFyBCO38pSNRTAOy5O`E=mbJ&PQ`ZaYz_lS5=MBPQoDxC^!)om}{nuKrKmq(deFS0G z;4qFZiHleI6W&vOio_xK1nLKTl)57*)1q|;lj(0gi}v1M`PSrY1gif=R;rs+cQg=4 zjW2*9VE1SR)WUi-585^X;pCHhs^B>QTcUIn82#gj-!oQkz@I(}?*-Xq)lV8FsuA{= zi49K7-$4*16&>X_AIKY)W1y)$@>(C!9Y~~J4;0Pp9yrfn)4Uzm2ZX+o+Wq#l07oDd zo24rB_hp0a30NFtYr;PdO%2d*eh= z``vNxxZfDx9pmwz$1%=!uf5hU=A6H|zKy713j5qZJx;fZ+Pp610-ptA%D_XB7d!k6 zrG9qGB{5J@zZ0G8+#1bh4$pXexCJJHS!C<2XYzqJ4Mj@_$VNV*2{cNRI6GGWP*Dj_ zoBgb(4zTxDz@Gr0&pNeId&iXJPo35Y5;6Ncy;4>j`>1CPfT0XpqOZb^A$t&u(JaJx zjksr>b$}j8SL*iAq=q__VX&sWtTWGquXZ4o{~tM`EDB{>)Y#z7REBX+X()}jWuS7p#L1rYQMxyIO(sN{(| zz4$Ww4i@)Kp!LI_k8dww&&4-z^4lDis7}xeO@N+-2HfCzZryYtHtSC|Gx@ai=dAx- zdV)RIl=KZpqW>B9?9JT)J%wliGNzUd)Bgm=Id~2pveA3DN?ENo;F3&o5I&${LYv^R zdCBLp1Hxz3O~QPXGJU2ND~d=txe2h*Nt|YKvSh18FObEIT8Bx>dDoD5>yl`Kx{6_G zK(Ct6($eA$ha3RXggKP3v^C(bxGh* z$?$w15<+DyG+OI<(T$0U3u2dv6|)|mtF;J%b{pNnK=ARL-d7IkP6(WzfzR)>15{_l z00<)c{n-RJ3>^;W_~_zzfkTnlL8XRLIDGu8&+L}cA!|TTNXTp>vayjD&@sgTpG=a= zd^i^nIt_6PY6B^p!D*vkOr{;MOzr1~<40lvjsSZsk^!&XV%!g;;3lyXORL$p+gn$J zKT*8I*0t=o@1I(4Q=>Sr*19+;EW`okSF0V4yMtJLq@211}6HZo+$d;AG7P4klmmV=;-A} zh<378i%6i@6%t|3Ic+ic0$@7r-u%go>QWCWe2&Jlv!cq|K&4u9GP}@de-N6eMIN*Y zAAbVCAY~LmO`RE1!D-c0SlICK=R2#2 znnaYG@bS;agTzQMQ3*}JZzSfW0^pHobo^8Opv1vuYtb;YWnqYb=j9>mxo|FNfkn^@ zv?K9BLZF(2;r-&=h7CSbo&5BMJ^gtQ=cZ!jt7Ir4#2+Z-WE2CnFo$!AgOXT49B^=O z0o`yAJ5fGLPvirWc90_i7wQ;EaI_;^zWrIstXX6ekTxo4+D|qHsDFHQ)Nwxx!xP-0 z4&aj@!G%9UJ}w?^f}KB%#VQ;>wi4+*t?!=HFysU-Wwa zi+aO-%kN@at7%>oRegwsNQM9UVq(e;aucYRM8+srLPcdF@va#kr}y5 z1rYL)+0TJ|iH6Im4#kA%iE^hEfPv^t5=a;2I1N8(IbUZb0eUf8OO=f@K@Vs)1c@QW z1|7bv>V`(0B|w|Od5?|=2MG`z^amkK;Ad!8RlBptND4ex8#bZ@iGHP=L4#t}DT$r| zYLhf<3gP}Ut~)*U35tfSDPS86F=>FdCFPa@B`HD+!aL;qj{qhIPikWPUoF4@lfn2h z|A#{|07hI+VN0KB06Mpd=lVyKKs@>;`yqg8Mdy^uBWXcO)!45Jxv!Z0X)FBj{=&QU zuV6G0zIg7nF;W!HU`wtXfv4$5Ye!C?@min~;FEu;R2=HkA_Hh$3@V~LmX(+TW;3x& z-nXGvs(OTG9+~}@g)PC{&rBf5?CtK=NN5Oo`p)Ha0D!6-Q4Y~Z@uVN$_7_G+jY;RS z(2dU2qK62fPDsfRa{>*krPvJp&Vv86cm@a+uP|7QfuInV>AF+2d)2)T+-jY0{74;v ztV&jEK9a!5Game{mFj>Q=u6@`W$eW>5}AJjEx#!V(V5AYUT_{Fmn9`!6@X*+cZdyb z1Ax8LeR@Hdsr(%P+BKX_#V`0e)gd(XjfpvOH9Bv+`|>AB6ohE^`3``Q_{Hov+7r*- z--rW*fzJqvmx=Jd6S8IU=Zp06+mBI`vwX_&`96=F!&^U}2o*9gRx**lWq>g9859%$ z185|mW36bKPZ5EUz0@?A(BoHs`YKT%EPwzLf_YSfv8JK11L)Ea(*08nEqf4oVb_5U z*ZWl+g~~G2-!Qo6p)?X_J2%Q4(2eJJAv?@4qCx;!K@BE!G4Q7vbm;mh=mC+12XO+Z zexhGcnq#~7H4Q&9R=Vf;p6oEzPvQrPW{!jT`os;4EdXU&}1461QBa}k7 z!|fdK@BT72U^HK8XkZ`KzRJVW^CxZA5yY<`#9m7r8=F(GzO=KV;PK1^>25}M*%djoC_E#*Los% z9q)L+Tc~zbO7e`%zYF@S)xtbjD02OtD4iRMU7qC_{dCSK*1aB2uRT0VTwwI}dP_3u9?puQ{LY?NmJP#Z}@!#Ktt zEApUqVqZT14^^|L|0;%pOoJ&Ggu-6({u3CYi&AD-n$cZs4`?(R5AIsqtK*hM_{9ez zMjLIV7X_$uso()INRb}1AcM)&50Z;PeVsrcmZjwO^zHqJ@)H`VjP`D-Wn=b8(v?{a}Q)07iR<>ZOTYk*N-M!s`kbvf6!hwQ?Tdh+S{z{*rXMHH(mT!brqYh>dA@nuTq}m2Ps^M^^%Go?96O z-PKsXe&1wv3px6>jd-24BUT2jS9#?F0Tx$yJERSr^e6Yy<6y;A2a{tp7`!R=twmjs)CTzJ$-uX$uLY8zzB=jE| zx=%;zcjAu=oTQhRFC#uyq^+kk@q1A|DTw-rzAw<5fD90?y#d7wVlM74Kmtfh*< zK62v{*S3xw(TYq)05>y63&@&(tG!$~MtMD?a!2OFAQhpumD4S~2mEEGza0umgh^BY z^(e{hZ%kBoTXxw$6n}8R9OHz?CG^ts6Mck_sx`*qAmPI?xv>5pO=^5W+-~+m_DOY} zrcTOPEC1v5oh^8-mlmJ{euh^6$Jd_<(#L1;ZD7OeD#-djyuJWpyZ1>^<`8aVxvV3j z1U1E6KT1yG*_9D)M0$C%LXf7JclIaolRh`WZD}xc{`OD2A|MULr8>H~f%p959F>5d zz6~vKs>}ld#oN@ilni`C#+3PT%_i})C|L0WcfR6aNRJ5#|D~K9Aeb*k zuuBGip@wH?i%QO42e6<9Ko`Oz(sfMYAcd$QlKK+-E9w)?&$Jf@pXIBkX7jbJtXn!Q zv;AOyLu3?(L${7ejy_tzlOEzJ&en}9faB-p%kxfM?eO2f`*?PyZ9z4iDT{jUhIy(j z3}_6w5kF=VvoJL`sRzDAARlo}Jx;xJOHH>fAuauj_Dr$HQ=kB%)p!~Ca?$E39*2Ml zfyaMM0MS1gVG9I47Q2zoZ{<_I!ijMN4t<*>2ndkix8@Yye)3{+Z;c?5@^3qTJ70-P zb*chiVj^hKxEOC9wnWxrmU@~9JTB9zg!s!|{XPyfYP%IxsrH#W<(dQ~Aa+=@5k;~1 zw2bKTk|t8$2$Ll=N*PdUhj!l5OYa`%6+<{{*=Ps3Cf|!(3Gk9KMLs`!3rgnXIZaHi z0`%Po@OKr%HsAdHKj$QS*l*Ww{rzTa65Xcd?!BtpS@&AO{@WLVru&(ZkT(*xQUIVA zGFE&|6YfWeF zw&NQf^|3u3y(Cg<`s-wbnRT6XK%>IdYikw$53fq8?em$#hhw#4)5TQzUmi10d33!b zp}N=1*d(dUhL(;BhJ^m)Fx=kT4i530G1+5b6~F+W9x1R#{8EiKre$4YQ7_o#t8>@d zYSW)$+#JRGPXKao`pW1-o!c`a`By@sdsWel-7CtIzxaYWmleLso-H|8VG?r6a5%=Q z;5PcB>4yJZp22yVfPCbrc()+mT=;sCtqce5{nr2g-#4N_eA3F1>Gb@*vCaRB-xon#77?Z?Ffq2U6c6_$f3bu5zpRh* zrGtcAipk5SzJZPZi{JM{{FpX`@gDi`ZA+kNlluxyeZ-|d)KuoQ2z z`&ccmR=Ndk&Mu=Tm9L6NkgUMN97QG-u&;W|M@e}AUUU4r#f;BO z*ppc{Du>em{;AN<*Z`=@x>o;Rbz`ncb(V#JE%Wx^&m!+##>d3 z3R46X#UZ$1DwXd=rp^~G%XCjaa+@1jlxtP;MQ3s5z{9tXkAJic{Cc{ZC?e!>5qNhm zq8?huXuE4&YP<4NKXCHCJslz$W2;U<_t+MAxabE}fBbE$B?9!lcpS#iYh`y9r@_0k zxqTV{bB2o_GefKZXoFi6Tn;TI#GEa6R3L!5I};@ImGg>ki-k&mN9DtY|2o=+&7C*q ziw$)d#&L>VTN4bb?UFgPXM~1hM3q>EH)TYTum12!MTQi+R*5j$Woe7$>(0KPlMo&= zRJ-98QJt#n+R(oFU`IEFLLuU#lv=e?JS{?7!o5xr#{mOwrjUvmCbnGL`+RmhMK8Oq zfLjRoGRN;s*6U4IEWGqQ+enY9)1is+@b*o;8~KT2#w>}@%v}=h{6=M(A->FOA9=dk zgZ$>|7}u3p@MFWBvrL12xs0%`fGeV81o9zZh_Tp4gJ^WLKYXo#&~@ zWS?YWnA*I?i<;=eF1pELeG~?(_2B6OMFi`&pCCThCm{mQiRAYi|0Gt{+3QA1o!Jp~ zS4q&1sxGNlOG~=u}|5D%kGSpA@c5}>yAhY+g(dw z0*xH#C&8B(Ht*#x&LVA(NAZKH3^KKS7pW#g@%%!rxW7WMNM0ujqXKt8`iScFq}7_q zNn3niEeOp?Vhkd94Rm%!6yPg4)b`t@~1IfH@B8R^CRB43u>4^tzvg5i5gi}35Kb@pGM%_ z??)wq5w&(U{^k!9Xe^a?{gf2Xe&2if_7^OT@lP!Nb?yJ|7_ zmmcAuMll)f{?y5?pGvs62ZQ>AF1NWTy%-r@a$(=f5U*NiP$FG;+B%Y7Q8JhGWHlFI znQ8XL#)s3FpK%HYO(!ltYH~3oz+N(HHX@wct)%T~GRq%$y5yho-tBZY^Vi+3q7jd9 zX+clVEEnQPTwjQ%PnHzFgJf8A?hw12_ma=OLe0zP(T#MBN)7HUEyJ+7JKaWFi61n81_5>}-uG_qnhb-`6h3{rFh|DDB)-5#4{jqcW-hJD*K^WK>(S9=!IHYF}-%R#BAd z?2ZBp$ynpS((c#O6fx-gx1tBopxvf2i3w8Ye;E8eO*Q0Dy2+_C#hOA$%=z?}J9HoJ zx5(KCwZBTipqTwZoIU9MCb$;tia(xL7BN5sw)TGjN#9L@cxK|uFG}!;TEP6PN=9l& zIf7B?gN*4UE44!_^vOtOFGhpKpLL>z`s4&AdVE$4MZ@axT42Ms<_X!$e( znoOF(86PN=ZB$Yg1b@70&HX;3J6YiTx`_Q-cv_T|`oIn;(WtF< z0ojt*QCj6vtNbaH<~ins0#;Jtef{2@ciu2L*t=Ri7x7uRFfk zQlpx_tr}m{`Hk~yQ%pBTR7Nu~Cc9mi!HXClI8C7K04X*PSusmO zukgKv)A*z&oBk>&)h`b3W#LA*%o{!^1R1#3GR$beDY5&lJl`zdO<*Mjs9G|ZU~l;y?pRnsH@tQto2e5VVCo08k*_8IhZc6( zRp<}9mIQH=XUwE@Tt__MY%?f6Tq>)idy;Ra#l^VnFC%2oLX3@oRToH>S3ejm+OS7H zUdGLT)DGk(oh)!Q%9)rX$Pcq<7_!Wl|JVpcpMG||kX5w5TeEFTXUx)JdWA^{q^#FO zxEyR>VQ*dg6}rFf4WE7mrW8BpKu`R*knp6ImR3aeVM`3WUO%d!(6T1#c`>1QTQ)JB zR3{BQGK5a?fvL`X)N(MB*=E(z3p*$Om5Ks%z7 zGJ0SjHIPV`V7t`x7?%%?VWIYiNV_F&#BP7~(&*?CB^ht>pqzX6jMg2BSx!jIB>Q=# z{SAnW)YKXbOVg9)RfEqP!&wBL4E^~iaNg`YCe<4;Rz?ie0JScZK_Z7_e*dWDLl22FuX!{TCC=3 zJXQ6b7I#kx-%H?s@EvVb@Bf`tLzG@!AOw?7CBX{|-m`6y3K5&&!NXrR5)^k|Kj1Lx zBN0?AZtp9N5>bwM%prX^QO5@g%erxQG3x|ClIQZy zmq3>el+60*CB~=_iJBe=nhT;5yE`|&*4JP%D#3%(T-4Zp>A^>8CKs=F? zi*t2?&hnhFUaQv%Bo~d_YeR=;g~E1^dOH{;Yq~X*p;$!(WqPIEh=F^JgCn#Y!FaS# z9AIXCXT#qg-JpH`=n*8#Mmf6F;#oX>1=q_SZ24zzuAoQ?6PKeE?Irvy6)?Mr{}s@o zy#+@(RDspIi@idO`yynokusliup-##S3AC$Z+D3pcmZC&ygTW@qxR%x7Iex|r~DGS z_-3)<0%MKK25ayV^_bhT!jncGavHtBT~3v`2XCJ zwe8O8+UeY1qI+`<@subK9fj^`JS&M3KzGFIc_(InzpOgu7F`-E<8$MF`4iiBZEgY& z{Of!typ)JLZMp4{67r!&hBftAT#)d4r%;$OXC%ynI@tX+)?K6<1s@f)(T$6xxW4Xe zqA(AREp^C%khMm+&hZCg4--mpOeJ6j-(Bm)-H-RWt!|ljO=9Ox+;Te&t66}y$OxuE z2SZH7o5|UVu+VEw?Bv8y{dJdL4wj(`(zKt59DUm=)fj7=pqU5?M^Jdz+l}dBjU1b) zSmnK<@{Z~x(!#t@ngiJU&!~-rfW59Akj)4|&NQEg*W4^#Xtgu*a3&VHS zA?)UKAq9ab{a(A@W%VQ7Z=!^JU|yU$XWW+bXra>&o9t=(G<<=(3IA=Hl+>=cCD`+I z(%kXw4f$D$7+|g6?{ley?{{DmrR1s%W@3lmpPOyxr+v(FMohoPeEool+80h zr7i%$oBt*i)b!^^S-NR!G#2Ko@2G?k4Z^-RP!NVhKb=gdkF=6H_P0Z7D2HtRl{jX_ zYfXGoP9obc=b)dSOw;4XjVqF}ovP}DgD<7X-c)P6o5T7}OFyDdRiFy041cSkAb2Nk zxuwr;Pwa~x;+{0Z^%Dh*l-On}q%&Jp*RQx#V+qNtSnI6j+ATtL3}zrEx(HsEia46? zgVf=!#gzHUu~A0?+FMLN6mTdzGDtu3?ov*m%~^Jpu|HDMm!-QKn7PmWO(pZvJNmlK z!NB7=-h;G4hI9$kW$IdpISuA`{W$^dDV@K8c0P_Mda_I==l70@$zqJTNuv4^oo4C{ z_5WED;q-}&2BV_EeFn6qu@)ABr|n>hr3k&R4Z?f z{aX6<3uUa#6uK=#xG#UffRfInry|6~dEmiIKZ20{m8-30tQ`IT`_L+JW;G>&IL48+*)LcRDw+mJLfu&Qx7_g{DfXlZL(pi(AP+167YjQXdGaHU@$X_Q3 zdhyO5&TK+LvyxJM-AZ~t!llfwRX)BT#5Xi9!542^k>3D#lw#0$0&I==D&|=sm~s}N zhNXdPC!?aUi%M66C)ad=@S~eQ;=@n!6d*WEsFvP}LMU}jBVpcRP*H|Fyyz{%wFWrGKV6LpIk;;6Ru znMU&`xUJ@Khj_luA#i|L+#09d8}Q6f@C$Tzv@-fZpr(x#Y6+?9*dCWnr^lGYaHnkU z1%kM%C+Q}GreQIC-iS+wW-^ojaWxHAy$)GZ|JzbHZ~{Zt=Rjp>cqiEdQf=T@tLs*J zC4L&}>}LZ8^rTY2STW}xMBzbNV6wTFw&Xrcj0@bg?+->LhNCmC&?etne0x+Q0zfoi zGtOwc2u^3pXTVa9zX%w|2i6gAfs-#D@5D4)efYf)fMdok2~>-So>^7m>LjfFNa|39 z5%doAHfyuZ>QEe@Jj?ACHt#i#V97V|RZ5MB$!-G4bQ690NFD1E1yWJJh37dD_^AcMwjrMnh@q05G z=yYAYFf?kX@bI)RpskgE1|Q^z%R-Ba%P5&@fxm?S3CVIL*U=-zE|82vRQ||QP46E% znQX&l#`^P*4Q@5|N((SORbhL>)hoJ`PbCIJW^)sZL2dNx`=wI}O&So1Er}*!nJE6> z>NK!dMRgzz#-WM)yl6YWgqcMay^EhQgbqt?3y?5l7F__3mu1S zohkHt{;{w)UmSF%0+)!v<6<#YOuc#(_T6k3ob`4T?GVBRZoY&W$aTY4Avr3dF^5%3G=@&rg@s z{poEp>SuyJ0I!jknh1t=FXDmOk(hW(+Qhe!A-(#@u_c7!bsx@uPpDD%>O#!>mJ6oK ze^Au94>~YqJQAW`ok$i7uVQ09V{DJ4d!ol= z?teCBDO@PP-|pXLMKV~=XjMD@pb-9Den{=^wYC1Pi~@*m5kwS!&~u1=U&!YN@no&=hSpM*^_I;$nBh*?cf zBSFlp5qv=L?4Z36Y}I^dalPJSGNf$`DKDO_9!2rKfDj)Hm63sBK0G(JIQlwwHh395 zJt;OrQ)m+s1`}&pcoY)%Q%Yy&vCNkWhb%~ywQ4g~{*TN6^%~KO2r$r0ED>?=o zG5+mlz7oN=|1h7J!fi99%>#%afrFi#k2^}L#>K_%bIk$=L>@-gAO|-HvOs9**YJqK z@)7MpQP%~k`aWosmW;DVROcfI2x$>BMpc&`u(@l`wQt)3(M8G(pH7j1Ling~=37a*K zLfEOKH6H?Bz({HjVM;1g1}!;;H&YQt+Bq!46NzX(RPRj~%|Rxg&gezD-_9N1{( zat*I7zN83<(bw$o!ZN5j;(RZyfZ z-H&R&`YKo6IfJ2>K+~K1V7WO!p$!`YTN6Vw(!$I)Mw3TxF=qA&CJDVzvC&3U_MY+- zbegJj?I^cr+X{5K8qLN-J@D7G@yyXlt$9@UCX{DsZAzKBb0|ogqzs z%5+l8dkFxUg~5DBWz!>PQf6N3O@D&OWa@K6q51`uy{ceIgO09e^DX8CMGEmr)(v|S z+iV_kkgc2``cj{c2^WX^x9NczWwIZLkC%YLrxup>o_23?11wY~Kje8kFH3%}#Z-Op6Q$Dg3=ksfUJd*Fbv(-bfC#ubKs2=F=q8gFaS;`#W?KFf-$UVH8dYT_oqQ5ri4&yxdn%AGAxr*OZ!;1sB}Dbs-}9M(h^E<+&tKwn0|cA&=^ zo_6JEGNm0)f{k#jE&**_d|f}o0@+%+#ZD5(tM*3}jXt66*Y^(QF@vRC-<)UC#H;Mq zz3UUpmytoTUiex}W1KQ}l{kpUOsTAx)yA#=`fEyPIKyq+kxG;G6Wh0~{r}AY{)fkx~Z&j4jHkzmnz0vP|!fPn9&CHc%R~{V`OO#i7k@gCJP~>{6XH zo|k`~F5RRX4tl=X_bjtIWxTg9wLQR+ljUNXIve?FWsqqNXi{fuj$lDn(Vje{82Y)v z<;Rd-p`awTAiLybcb!cScP)$v`osN&0sB}RTRCvGg>E!5WGnT(hQ*%I@+Po{q$G9SLH$kR}iMcZV zlN622j3hw#O9T8jeo<2$?G3tkx|!#KPW2Ty)HR)HejxamYi-Bg$*%gtzVT}g;uRs!3=K@FNS+SV5+UA%RY{F&-oP%&Vc5pqpzF@quQz=&h#fouE&PIpfo0#Wspvfn9UD>^pxr>4JLT+GOMMD4rlS9 zC?*BC7%h>Nfz?}VMFcz!Ij`T&Q!~5A{3|`=4g>q2aj47S^%4P~=-2SH&%nuVJ+_yN zXQ_VATMs(VX3E#aF=b^$%hyeolcRXdXre&{Bajn9ziC*_2D!23(1nC~Vdl_C`VlIT zkeI63)E`pIKH+V0kW!VIlmv%8mhu7!&j21cNUn@(Ezhave_t-54YQkDzm(y6Yf?rz z4irRhG}9B!MhZ1Arf{N-U(auB2o4B&#cW2j_`Gj6*gGKF#=47dS&sYWO@P=ld-H9Z zwb_9$R1Mz91mKELTZ_c2jdxW5pfK_u62^;9ojdfVY+ z=qnT4K3K)iIW!@<`XQk-E4WVAdtHk~{@Xepnz?R*uau**Nytk5~P5Od5VC0&R*?-5P}RJQi#Q z1q?<;GhV3d3hBXs`;Svl@phKR9N2r`ymr@W~2B(L!Sv<|8d!D1S_rCaz z=FSgb@HZD57w5%V)(vHe&HuK^h)}(#lE7dNs^mzCxwh#1sHHZP*j)g&7>wr+TGS9* z_BXv^Kn)PNsY|ta#*5nV;m>C1eBJ)b>&A!dOw}bOv+hUiqQpmi0*H~L<@a$rje_rg zL@F}NPTvfud9xd|6|)dejjs=M?O0Gry)s!NX?8y#xc_k7jCaly_c{~P`3w|sXb}5R z1p_<@TRDM|5<+{?w};Lppu@~7ywmNEMCm1@%47(~T){4Z?}28&StBxRZ41HmRuQ9D zeHmR5&7gua@=at=*L{LlVjL*0@PlW=s?E?&J)rW30 zM??k&V*YaKCfV;NzK%Qmbz6#p5vgyk&)TlYMQA5mcgAR zsf4D@YG)g?46*^TBEf#&{R3Ury7CN3XwZubC!&xJSuv9MjP)aDKH#``@^@9Q*rj-V^ z%%G!YG0^(X*FZPQqf`L#zVQGCmL>tPfae3@wvnb|hQU`_lUZh)k5%)ndZl@O`SJGx{VN%+q;v%ez!!gG zznG^X?kWUOAOh}pWSTY;iGtXlOH5^PkYkW%LVCw^m-20mon!z00e5__o&O*hR^OFM^vS>dtwB6oNdATA}&*W3FmQFJ^TLL0dGSSyY!x`oP^YG^tA1|JD#Mk@M9_9;)4h>l*g!JU_%V*u*25)mq+@IDza_ zkAf3L!;^2$CXA>6mIe%2TNb=0*nOESM;ISZ!%+3XcMLs)O)WI#^NQH*>}bj^v?Ixi zfm$CE8nx1b&Fv{l53Zk#t_kOK4QgpjVEXMb#7hNT5p|d~nnIeZb^HORbMn0C(Bb)H z((Kr+d@&sWm59nvGSL`TR&ba5Nee)NKRm&1b(WqFqTQk_d^MSwG|rVP?ez6#&nt4M zT`XZIZ{ExOz^*ERqOGgWA!nP-*-Aha*umro_#c9#qJ$h#mAZqqo&tg1dyz%DzollB zs0_e9&ZY}rkpcLb>Myf;Qk91)d(^yTCh^iAQ+C1*eKBGFvrvVeUVW?ENk%OMS4r<& zC~ZM?P_y<@=pL?N$8o_K(x-{qp~uj9?qzc1=xj-5il27o6`Kwkihy0I&-2V8bovZXnwO>e@iJalqQj~`C7vz#*N_X@_)0%k6A)sq` z$kzYM`GAvI4g3LYhf4mm2RGC9adgB?H@^DaY2{2afPOMc6HngJpk;sTq91VbURM4X zBGWCpK`7qJ@R-VFBL#p+xJ3lQh_SGMDh6!Sj&;~sX`Ya-dO-$$A*F*M7#sv6yBaSw z_h_bJ;s$&`ii*4g3&n~`N$}-h4CJ@#{^+W`V2-?GcGD*WB;+3g0sx4F@Dw>Yi4zRq}0=QG_?UnhIYW|8v1M-IOZ&dSVw0JDH&GIX;adTu5-L;orzB4J5DSO z1-xVMK4oyH!WiGy7ey7b(+wK^Y9jbUXYi6w=jdMVv~!65kyS8X%~=e(hh(a)o9@7PL2;bu2;TTT4BVm01#rEnf*Ky zM6BjNa-Ry?J(cXiBCEq1tc0YUpB1kFZIPNaen$E4CaZ7mgL-+4*MfesnnMHywCZ8% zeQ+jL_ZL1j6yC#R6D>>FxK4ipcNc`B}Q84Pf?`hwvGH|@nMHjO2_ zkoy;^8^BcKDiZCi?SgzkFp|wyPB8N1wzJz#vEL9h8aE0jSd&|>u2B&(UN1%wwIS1pcT6yRHo7XI&P@`6p zLg2%IIt~Yd)EK2D6mq{44|~t9Tfk$RCGzoPdsM1J(@rTd>@w+b$*|5=p&O6=>XYDh z8tgl}PDvYO<@3aqmP@J|gt7pjVwo!SCAV6cn9`_rWZ)J zpDPA#FDCW*_VLuVhUok5%J-xGK~I9OSs37li;>fW{daZW;s-Q-_%W~tIP59peogng zldB?0-H$W|_(pnQ=sk3=xJig!s#6zI>@;<#(I{t?e)v4dmm)&=H$e}*H~=+CcKZu5 zJbF>h^+Lh#oQ~%et(S5$bfB21;5Zgol$eHmHw>&eI(?ejj>qN6P3#(!^sp{&aXfv| zQv+`uK7X*&M5*F%2)`zBM)|12BhhfWQ;l6TtQYPrbg5pE_SjOXas`j{Dm1*5gb3me zs5F6cCdq?ovRtR2H1`N4R3=NCqhRU@U7`IS=)jolKXgD#Y9tL<*ki?{GHqK=4}e*S z++rJJ)Ne%4WPXzsPDJ0XLV zYgS9*6Innce&prdU8|H!Y8Z9RtQ(6KV27W@;b0KucxTC*-j47ZXyWSk&chhws&7yr zy+Vp&o*(udjOG&@p_mb3rUPkO4FN7M@XDWkdq)O#UjI$I#~L96L`@D$WH#;k5Q8>f z=BWZvcPMyQsMqc04u6lPKhc8So3TYEQo!j3o*Y+GLadv6hYwasrcDiu?Y*mH>5u1j z45ZwCtaXx#izumII4hDXicpynNZxQ9cY$)pj1>uOT&Vk&(Gvp`qMffq<3y!70aqWq zQ&A>d7M;o(7oAm9LxpRugE8R*|7I^Ic_Rv2&)C8&9Zo<9+M3D~I~?WCE~#5AOOyq&An@tKg(`6XyiW@E^(U2o;y)|Di()5Q^ubpuw5P)% zHq(`kSbNmya%A|XXz%04^Tg^D9Uxl)GF;*mlrIJCmvDE*5v=?fqEuU)$|w^roZ~a5 zJJh*kvsOcxmh3gH!b%qrA;qfO-x34^M+|BViit+Zc~u_#L`9!v-~5p?a*52FU<>M1 zh0h~J$R_^TAQVQ+5eQ2wTn1r4*zP_YT11`)Rp!P^m0j#<4-~Nc0W>$aj z-#PwoDYaBE=zEf6qcF0sWe(|WVt?}@JNpSZMqeFiYNGs&y}2>#x8>JkpLg`oDU_-w7~Fy^Az3$dDO`MB^}wm}POA$|~#J`#f$e#|Mz8`Ju(@0+X0G z^rb4n+rt~OI1>%DiKb9_w0KT#JBK%_lH9bLgfp*?@6s8>1eq3cxx~nRigusjOmOry zvL(ScJ(3AhVu1r1 z`FRNeA0Mxztk>*GUe9t#p)Y zwk9MEcCY5-t$vLnxLEfYdr)I3b&JxY@V5L;pF(jnth#?T@GwR+0~nDy(@9Qn0HSwE zJ4dd+UE@o50?MP!AZ!nc_u}#S-HE$$cpR1_AxMfyFZSx&5NU^nqVnl)k4=Jk4vzUN zgPyp<;_+wU99LCyb?yhRuAPJSHiI4Bqg+qe56mAVczc|6hecq(uP0qyZM%AAM!~*? z)p8M5O&pkH0)*Mk%-|%@@$LZV>vV&oB4ZZDJ~PBv%Whtg0c+{0Qgb;-p)oeNZqEZs zwE+OPpfn0qD^LpB^{13tERZV7!1qfHtOv|GQYsged=RPdq`mzsAi_Vzk}emE_Rkt& z-W+s53;NOC@wZ9Nb`8lyD_W3ZN?PbgtnLU^o-+HsVs5uyV?Ybg5RY7c+lfZ}d8$ML z_j(bl7P!VT z54-1Fscl6KYyEXDBh_8X#vjIxln7-M03*rPGo=)AAiN%mlW~?GrTSf){gn|YUFs&? zdWXgV2KA17^B^CV`EAwi^A0@~Hg~V>2H(Z_$S~rDa!(P}5@c zbMTFu%f;WGNE%K)FVZx4G4sHhX-!Qp9yO5Soay#kqN~B@i`4^PD>;{d+^l6~>_}bd zACwPzqiqb`h(G3))Lu*A7+;h>?os}#)3h*=j8{(VsIkT%!d;pD92nSCW4#b)z>dx_ zNl-P^S>~I%0j2ogSKCZrV^d=-V3`z@b&S82Q$F zZF1fruN9rev)treBW%7|{@EznTptiWV0KX7p`yh-LyPO&r$0KHS8kqh9fsnAU#nN8 za>7D94bcO~BI^FD1t7Hd4d~p*Gft^mDz`2%6+LTQE;KaYfhOT4t{AcL8_ZS#brU%|>4my5s-50RHs2Uz<9xeMJP+Ec#lBtNitfYgDTA(%cg< z;H%VOdFp~;&q9dCj*(c81)sLXYT~K^TMi*G!)&a(ySM37|63SQ?MRN(SfL@yrUKn{ zU?QWlnV#2vsdr|d_c+hGdfRTO>Hlf(yMvnQx_1Q;38D}>NDHAz6GCr-^d=x(Is}j= zN|#=w1%Xf$=^)ZOh=mSH=)G6zMLI|?!ac$F_v-t7-`sm=?%cV5+@F(iLgr+jUDjUf zd7ib-4p~tA3~PRUI000$H95<+)ZpX#4Ua`u4IQ!{MMmk;rIMW@FtfKGgy9VIF*pKW z6_J=hS_p77qip}AV4XY7oG-atQorU-OyWIylzekHw#x^F;)(p1KJ1LC)Q?22>bq_} zAJdMKd-Zku#@9&nvR(nXQTa3b8VrxYqNlrhcVI>udfjQZit0`k!3{Jn+*3 zX>)FB#7<`17rwdJnoOFa#peb%fdo=`aX#*L+G0_lksali5(BkweAfHfDJK$8s9$^^K^qj$b@pN#%5WbOc+n`$Y_6>2SND$`apmOm#hp!?b$>Ag zhF5m?!&fd~coi&@5t#%;n+PHQCagQCz%TB=cBaXvFYnh@sX9T0?x)(s>fB0-<$_*% zxUJe#eh>U4M41NJOuFze)J02n-#YA$8vPm`)pI@c_^@W%c$Rkz$$Z!Exl#F^?~|!B zDrJT(lXj+P5#8Ffiot=sCuK?Y{Mk$S&0$3=C|l!r`!BNZIwys2ev}(x{6N{ z!Ym4w1xc$kvrIJ%x8-aGD{SV0pN+<=r~7L^`n3>MywAFOUgHFqqck#lTgVeQ(RBXX z@(y+)F&vpJm~qiKpT1w-Xz{I&aIqnx(RODzsn5v$EPdW`ZnBAue6pyCEs|syV|Wf&DJ1WQ1-y(>8X61qU0gdS6!Ib3&A!#Q+8H@Yx6W{ROQvwJ>sfJkxg)5Ulw_~&XMSHWZxv#HN~ENd?)w#$_1MZt zzE$p9OLQ0mWozXd1=M>KIX?(6RcZ{9ALXxv_!03t-TLLfov@ysOJI>$# z?j>)()pQ{#c0^KomA9;{6W>#_*!`V3#{Fw0G>@{lb?;_)40K1WJFQ${-M_|IUA^lE ztFQ+SmLRtt`9PY@d88(=5Y-u{QE%{&pScMokrJ)x@*R&AWuTdsHCizUG*kdD5l1^Gn-qd z?0BzRYeL43H9}kh%+r+kE1dbf_l{t=OYQH@Yelg0kp&W4YXxEhZ#~>oz))p&;X4O0 z7I&SH8`DNNc#D_t)-@>&{6xxhmc)j0XgG{MJ{l-ff;1O=4G05;`Vn!GF^igOyV9_B$d)5c!_9t7UYmP-KLW!K!N z30r6j#$+vDc(+>PZqkKkP~|~ArZ|jpaS2qAj<-=3Hl4!%`C9O`+z2&sx>QvTHVOQF zTz*mH*UvVO4CC%b5n_i7?c^JmoMn#?jf<)u(1j8f1rCr=4CFq|glhh5+P7ZGJzY<>d&+p!2#qVyB8=_Jz@MM-%AeADixL zNJEN-hW*;i6)(NV0_S8q5!(rY86NlidjsQ5@9DjJbWWAn__l07%e94t;{&SaA(hTS z32MkzAiDekwZZry6q#}&GeTM8;#~W5+kX2y%Mss7u5B~I?_s58y5;_rWp>>F5D%Hz zP$yUq0meu#CDdAY`6`X3KL!~7p&46%Iph@sRg2E zBEBJkpgC&^h7azUpJof23erFtam5BwFFF~u4#u$`BvexkkQJj27>V{rGnjgYl7rbK z*m4^jx6ZG|^6hDkD8KHcsBfm$5*V1ve`9fM5)?n5)3f5xeEPh9-l*C^spoimo&R1) zfso-f-XQXl3$0~LV}HMuEfZq6(HDxdTtlp6w@N_?)SgrWC`sr#IyhnxR($wGzrDxe zALVCB^w4;Ho-BE7jGywuU0%rJ9R^>oY)X;Vk5-GaDRyO#5AF~b_dlY=5Z>>QNfvvq z_WaB13{gkkmDXmVlk>NHX53^2>9;vs!IYJ*ntZR4gXUa+BFty_1=xYN|`J4U}K^X-2i$(WnT@NCQO_Kx~i}PNN8aP-j z`$3~!eI-wixl5;2;2fbq_i0Z=na7em`=c_N5M)|&ywApdPqRu& zWMfOunH&NPS-dSo1Xhy$in>8{AsF12|*A6{sCLydP0$2#pIPTI2aiVE^@Tw*)%o`eQBJFs{M##tFm{ zO>9{7-iUfvyoD2-4uZw}ROdBz-QW@GE?;hdJ)oX4v`nPB=dK;fVd=80v{H8rFPnPn z8y6nWnsuX5btpL6_WXxSRe3JeGeXGHcJSEIed>Uij!Xie#N)~OBYxe;)5B?B=+klf zldX>7r+t#)&Z-<*<=0f68g8@S^(BQd%(hfbg^NgbMy`D}OFQYTz@XkEk8v+wTZXo# z3KfwS-(vGu3}me|0KxY44~NC$@EC+*wB&|n+hWAD5EQT&lHJAz8%%Jt=y^DpJZ5Ahq*Vtuw85=RP{|HMs3?}bIZsf(>cLQ&~xff9F zB}oLI^Cr06k&V~9^?Vaz9CSk7yQ2S^EtS@?`{zr}y27lO$<$S-Vl=%Bt{NhN*A^sA z98Gx#ta6PIgYNC9C0req& zfNGFV=nt6+ggT#|KYV^ghDp%ElZ8Fk-YWHK^DR_ z$$9~;zNs;@lR!b{AoAzFJZ`k0GjWW8=`VLsTvcAdexIM@KM=?AV!BfC5D3$eboU|6 z(8>EyNyrR`C-!q{>O_LwSXNRIsUbo4?G~w&6v!Cod)jC=i%s0t{HiO;H2qo^_cFuD zR}u&FF37Pu4YRk3^giA3C@5B-p9!>-?HqJxb8g8=(*)0$hl=+ZzRK*i>xI|SdksDt zkQ=EF1riX!uFvvB03tLAhKn74mtxnD@zFn|3T7L5O81JjneC(g!n9O7f`ZHDgg4pwp6b8YHAWI~u7V&XE#0rYMSAql(IGC{|Fn%z3p+ynnh;-I~Cd zvYAMH`tvyps^a!pO(fPNgUN{qc@Vk4mtFo-kXuJ~JK#`!dK|9-JA)qpQ)aI|dyQy6 z8{?^1`vSP?*nyCX93RD*(aQG0p%ib$Jfu{ro5HD0n|*YKW{o7%jP#@vZO5Gtv3@I~ zSp;Ub485CiOf#sC#jFV2#_Pb`el@`qmqc$nQ>Ob6rq1t264!ydrhYLrHwD8pvD?$l za-xktCe*zXCac|%)qI}gv?qoO+njf$->i&unWn87C`Z$JTv+O&u^tf;?F0iej(dXn z|FNTiQfZ3b|QUtAyB!0AP^M36|tj=@|PzBmNk)pNQmxrf1_byRQ{s)(7gM z9%tkhFPECOQ;QycQek5^tfk1)U}&C-9oU}OPs$z1Cp(}K{S&_4q7n+-syc$oEq!=Y zIm?WNxAF3%$aDTxq!?WCt&)b;|r(4I)cBe&mg`~#m>JmSReWafKYxqRvo zQTXNn>B>N6uNnTaDfU^5%7_zxt{Tl%In^NwSR~Co+1=s*x)iIq=FlM0X-%1ZTt>+c za>M%d%FmKTJu!3#PkhOHF=$&N*BsBG%-t0WFf`28EJFB-d13ZjFzE*ml8;s26nlAy zao4DdlKmO%>F_(P@4O~>o8K;^j@j^oolj|EDL;628*k;X`W0N)UI~wP?-)*(s#mMn zKc!~uHg6a`-vF=GJ}2luGt2IjD={S+qQZ|l3=5A>7p$k)h*F3k$&6M(`G7{GhV-r#wk%%MR-!`nxS9~iN zIe5%!CV#lqSExd=N5zd%Ah6N!MrK~Ez+FHZNeq%2>gi@nhXe@IySTQ%IbH-ZpfuTx zsRF9_m35gI^;MLUF}La+{9(rS@O%n`^z0w?6%BhHhB4^?sPYGU>a09XKW; z4-j;gp6OL{6F%k$sx8Qd_)L!8SslUSV~dr(SO31J$)q9Kf}GojX}FUz6rqU6_NX!% z8Odt#(#j_bcexp1RP@nr$>yF!!OTHba#$(Dif5zskApv@ z%a(WFiQ) z7Qc62h^;NyKroO%2c%?qY6@#3XOig}JWQE8ln;`NC- z^LI4Q1{W#?U02C?brOV%-IQee4P&$Brn4%~!?$a_ms5?T9?r5qL_~LzQaIJXAtN7e ze~zMaQVjKd3;<$lW1}iw5WWK?3lqBI+6AMrEAky7_Lrb}QDUxfDG89y z3E-QlDI1ND#;og%1UzaMYyR;*IFIlHZvcx_`Vvi%kLQkENCOOv`JP1LGTsnnMj@`K zJ+P2rIN!@y9{yIK4CRp2)G}^}e3G2u%On|YoNSD=IjcM>JG+Vj%$$Yf6j6&$~ytpgnZ~^DJ zf8414XB+EIy&6i!8%tkDSTm%GJ6Lxa?Y@-QZ}d`1p9mI^y3tD?O_%qy^|80fgH#aF z^J=?sB`uh-7({z|b*=>+#}-~XqS6SVOr&PS%7_uZfa|K43u+~NY3t=@b7+=Ag0$uB zTDNP=DCd3J5OPI=wLS);N17OM)%+f$wvn`Vte z-c+JgXtxK<``12NszfX*X0&8+geeFCMfs@`;V2`bGdO^QpN7K$c^SzD%hK#3jp7{k zAL;qAC^Fl^Ix$O_&$p2oL|Q40$a`3F6KR*4b3vnth-Oc8RkpEFlTIxNt-ljNlFz1L zMFB$-9I%EANZyf{Wn1x8J$Ey!zZ5F?aN7G+mm|?av6TQrjvGFg&GEvUwVE+i*asSC z{!}`YN_WHxm`dGhcV^)O+6d|nrnS>&tg{j}Yw|yD>+C9hLa@=$qo}lNWDo6fT5b@- zDcN9+BwIevtJ@20(@}*al?+l#aW~>Vy3MmfWzP-)#?tcyne*O|0s7dvp~hsH46=IC zH66{XhRIRPyTy$YGX~waePI+M!4NmN5l!FaqUS94W-l)l1j~sPXSmIhc@%A?5vM%oKGR}gYhR|DGikReqvRwohHPXexI5IF{P_GARhNCr5?%qC1s zkwKRsX5-;!8w$hMI;glX)`GdtO5<;&aCjc(d#C0Dne*fXt;cP`91p1s1e7e@0yUvb zypU!<3eIdb!~o6>k?m?Xw%N&w5dO;WwG-(bq8Sl?YgF)AVKG=kjk_NrqmgZI!cnvc3;qLBMIFX;LymDo{#W_%jdTC;iXd(l}{js{jAQ#p! zc`P=TVowd~yu^Fik;P!5WC8}s1RMzT(U4&Eea?dSxC&73{a}@ZrL|JfiO(5zdJ5dmTIOZ9`3jU0G0pX(fW(puig# z1Ni>-u$X`t0A2dsz%(U+x94*ndulc}QD}KC0_*Fn^Ddx8Wd*ZXs`~Tbi9NJ~fnwEMvLXSA^0#4|fi!M!^R>&nNrP_u57xuVowMH;a|SiRW6T z^UhVaF;o<|);=ueXN`M7Za)sETNB0jm~O3}u{+)2>SSPRFHv9?J(^EneY>BqnapS3 z$!mC|8yI&VBD;xieUBJ^^qt;+o3X+B$|jm^R0jeKU>44Boe^z+ z0zu?2=rL3o8Zs18zmtqF+{CJg+{flTS)zQ{y z*xq#tThht4GQ!&$Q#`eMHAg#H11V}7EV<4D ze-OPO$gnL6*e3<}(=RXoBm!%Eqa1B*UZf0UDuDIt;lvZeZSv-YqoFcidpD=&YY0iW zi4PjbsKda3h6&I{uP^wW2W~$WKque(aA5`m$p#u{RPh$`>Z^5rU!8_a$Pl4dk1;0LQLj zN$|iin?iuRAw;*^k$muDC8>(P#T#tA;DY@#*Pp|_NT!d^w3Q~DfA;E0hZ0gK zi|pyF`Os3=740S)SD!>S2HSa-J_7N7emX6Y`eQ$uL&}B&WzMOkf8zms3(aYyBOKg* zJ^_jsPIzPmh8*%4gxYOR>ssSwd4wF6MBhB1$qb5r8sO9WK>95OpL?->rx7vyfX#F@ zC+pRAkNCyRE~c^R34%sGClI2u8~lMuLqZwSOss&E0&>FSSEnQz_MQV-$0;zGK@dLL zy0;RK3OLMFAMf@A6ES z_L`HW%!>z%r8Y}AnGX`Tg*bECjCC#IKgdq4Q|#9tQVL_xd+n|D60gMCySQ9nxUEZF zxk3R}ke1M7l@2=yKG*$WusAz?EP{r|2?RV2OK>!}EQsx87|CKALF4X8u{sPQOO8Hz zRTFKbIk>WMUFI*Q^4{TZrUFPP)yX=;)RT#l1)jUyv#O9K3#%O?Li;qpXKa?wrKHMS z`o|sii+)N%96%}yxt~k)1N)Nu$TwsXlzU!)Q<@xpF4<+O$whqH6!Y5%W5!}b)kJfN z&?Ha2i>$$VjAA#gCOJ3DG5t9S>(PGHSwgX@%@I*B@^lp{ka7BHK)gdM`|io7XD_I9 z`>nq)mb9~jAxnwEpY|fCw$#2V$Eb?;!xe$tsR1r5W(8(!F~I&%mx0r$cz9kBv2yRp zyVRZ%1DK|5Kd6Pj=d>ptNFbw9V>Nc#(H;us}myVTb(y$o>Wa|LDfl)r}z>A(FoCH4w zh%C3@IA30930v%-9V3yi8YP>iKxRXDymfyy;3id4I=yAk0Bs)F7BGkx5QLMY4;t%K1rKm`7b7wdaktZf--4}Zm;TbF@L`6 zb`YH=^FZ+k1_u}@U`LPJy;7)oRffumG*5uHs8F+ zudTK8o#AO^tsFo8q7(}4M~Vcna-YpHQg8r8R*mmMuR0t~T?K$I zenNjXY8~bs^=U>&m?!(`5I0&{)3hB2ksY$j0VOztlHpu2;b{+=Y`#IKv`;STc~v^^Q~t<*5@onbJ`nV+!&&yh*f>X- zoJC`@+DB6)+fJs$gjm&;A}S8bEMZKGGtsPxG3pqZxg6xRduSXexJTib`vANW&(LIP z$!2QVTd`8bwb@N)_-dgCP??9mg=L=hWqvW->x1a5MHed;2gV1tAJC3GYpFC;3UE1S z42j&^;b3WIqfTDc;SA|gj0Y!43&tSTu1BUwo>i1-_07N08;oJQlQER!YSpb%{Ovwd z)8m@_bm&2F4uOV|MH{;Y|8;?9^0wS0JB47gmkccs|7Cyxbs$QAOYl$A7|}$soUYQI zkj{TZ5^yoQdZaU^uUyqzlBKaI2R{ad=sx}|L=0a)pe;HyK$~3gGb`QJzf6wmUrp{I z0ycj?544NUC_MU5_4JUPPv0#83S>*Dl#tSx66g-N{ZBgJCmBw$UT;_*3TSU@l!E># zDV^b+YJmWZY{mcu;sbHLF@#O-rthVs{ov>{oF~dp4GrRCBj!!e(3SuffVOzx|Etip zR05GV@}Bv<#e)E0<5<^0>o6U=uOE)toaLZKC2?1R;zv5t$~`%O&eo26o~q-%)a@sr z!E3w1=&l-FN8F&tAl$YBLa9M@h-CP3Ab{*IE3WRMCpb>r`)~Cce`J;9(r__x$$*@s z$xRIX!1(9Wp`j5IhjX3B#%gqykHgodF}7-`udaRGlnr|t;IK4VazuCFxxLj-1>nFh z)4mrG%^Rnt=Rfwx$!^vc`)w9ceiwgsV$E5%v5wDDnk1r+jTV>D6+txjJi;HRV8dr4 zZ*R|DFMG&t?3!F_zamZnLyM?Ye-(}F4QHj!l<2W(>B(8w^wvzdIS9;^$ri(WObli2`XXy_Iy-N08SC?k6yfrHql%(2D057(aZX!9242 zUz7T#wvXJh2iXA9DOd<_$ zl%s8ySj67gUTN9kdjI@3L#G~YO40}*mN&s{cWlraZ_DS-mKuw-*Y{!IwWaXp6wkwG zz#om)!f~f`UMR&-S(NdbaMe9*Nf|$&@dv87%-R+tzORbk0x}d|<`m8=TuyKOcwG|t zghKs74grcU#|!X{N6V?UJ6Z9@JYE__?5oGILy`{w+yT=7QB53DN&73_bhsEKm5!iX zti12wGOfjUE34H|VvN!+6GSvqefM0?oMND)mK2Vf%DBBur=BSHi(%GAo;u1>fXh(R95XCeSFrn> zV@wPe{U-IHM;kD=@l~O5J7L!ZaAQ0@Mh6aB$NS`!s07%W5ZcxbHDw>vBfEGvyhD%~ zfU0!lvW06<<#==FbuQf)`>sx*Tmg-ggT)1F$~_H07=J)9d{c-!M+Tjqv1INxD7=<5 z1pHQAeaAA9`Q6Zo76CfXym9iW@{gqI3#)w2m|GdPmaktPkaY>ZHPV~7opPE z1bkaM`)WTDlv|AWbgKKlQChbHr1o~=uCVeL7R@}c&sZfEU!sdFZmh&Wovxktbf-+g zjQHb3()V+gcY@VeujU$*dAZF;aTXj=_PF zCk-`6JIPa87XaCaYxUE*ztozgEo7) z?nex3GAL>a=!t&cHgFUfW>9=1bGM1}w5!LufyL^(XQxc9(TvpH7arH#wXh>`V8^Vv z7rwLH04UiVqnvMv2#943u=FP^I6>kxq|fK5Nr%cbdVc!avNP}DMGVLcZe)G;l>kp} zn>TO8AISv6mTwpVZC_hj12^oy^81GM1Pou0j~*0?Uh`{LXVIu;{KT1nKpDRtma|i5 zQbvTu71Jw)mV6|D`XZ@>Vn}(AX<{KIlrHyyN1vU)k+kJf2)qrTS!;X6Ji5ONHrLVR zp+_OOqUDBEq6;hzA$l|D%Hh9CD?&vayNv5@ndRK=)#V4o#VIcRp@L(b?;r&PXtQo8 z={bDnd)}j>_qNhC3Bu2Wv3l7OnfOVC+^tpNCDFg;< zwh^*GGVeIX7KQLkkge$5nSOrzMPB~5y)Nk5ISG$u(d)MSUOQeFtekBfcwN0%;(`iLXfd#-oggl* zaywW+(~feMv8DVaS!;TvbK7XlJq8)11rn&PcKi1w<^dQV!^|4i5$5hb&diBI!&A&p zI52v*CtCYz3>i{b*Ycd`zxmW@86tr?Krn67>6I=Uo#xgyr)bqs4nw^`sgF!4&T;7i z?__0;36Eiga!Q5^t_g%LdViHK3@o$2rs`jbA@?4J5-JPtg&@S}eFjU6(ABEDaM^HY z<<&u7Jj|uL1Kn|*ucq>CW5R#tONNK5r5oA0IDZdk9X>51eO{;YBDC=E$I#kS5X`^j zJ%iG6dVUHt$YXI=O_MC=z*}}MWaY+=v`cLJFX^tZi9@yi4$7@6#44=_P}x2XIenOL zUgw4e!({$dvEj#oy@6&QV*`R2>`Js$)O{}C3l@(?6))*p86Tn5=N>{d3)YP?5Cn?M zZ6Ws|OCL>^GdUIfBs-Nb=QkK;m;e*{hSfA7ioC3Vn0L6FEx|y*jixU@n9$N6Ga$1$ z#PBcUvH=jrl(D5J8X!F37+ZhUVQhsH6}N|h00->`W4#*zC`ng=FYMgU>CL=At8H)T z{I?-!h0%U7cUJY6!swgkfKAAyh}QCQ9jL<5^^KB1*McGNtH3?@-9q`t<^4_{-L92Q z3cBo7W5vG(0bJM{kzTpZ9D;c$a*3Oo8E}d7twQOLUsVN=7JG6+`qtd{-C)LR42mDgW$c-6HDA2ap@kZ52;j$VUt%>?! zu>-1gVHPrDOf(S%6rC6|H;~1b$XyvxMVIRQdby&}mr8w~%`f!TgGOJ=Qhrx{HGuxV zK@nI~y%m1SBD4|ED8nE9(}Mz5^Xpam8nF2{!pO%O{|e;(x_}Nt1RZ&DrmgsXxuspS zTRJA7yYlx1grzY+BidCFvTSG@m3u|5{l6L;TGRT|!~H$}1{E|!S`pCe3MX&A0y+}K zsB7>N!@~Ld0za%mw6-1lG@}pL1fz?648i}Rp8wsize6TKW`L%I(M{dMtnm5!0>aW) zK*wEABD8q`Cv+cEOx2H*D-9n5^Y7^amJmJqtO=_R=-uJY8*2JbU-fst8r0DRwtpl&{$7=TTQ+z99e}?5u_m`c zfXF^5TW(z*NyNoL&kb#Ex%#q$`5)K(b-leZ@Z43#uR`;`*XqCCP|*WC8Pb~^{Ckal zZy`V6p$;zWKmY#yz5xQbTq?{i>)#*!SDTlg0A*$W`1^PJ|9wkfg8m;&Y>hf1UH?xv z`+MaYG%n34-rE0b_r)N>#=z<7?eeIr(U(s-p>u10zWm~VFgwqPT;TrQz#mI3nudOs z>k!OwncM!;X3_7r2?4XIK}8#2|Fh9wH}G2~0-8P+dl&S#qyO`bktCqi#0kc9{IL;! z-vCf20ggE>_aW-Ht^Mv{gAxFwb_nGhDgJEn@As*yK-0^o2vT$k`QKh{AON};&B@>W zkGrU07Y&NMX!|68cjk{CastqyS&6F6;@{f^K5-KRnoiw<>dF5OZh!ZRVTKW~6t_@l zCEkB*x`!EPde>X}#(!Ma8nlMR>A(Q47ZaZb~kr^Qwk(Is4-YaEiq_jwgic(}Gd)F<>NU}#(C@T^v zqyOh?l*Z|K&N^girI3k97gbWKy-P1)z%f;5i+{(rbi;?HR%5RL^oR;>^E{r_VjNIHt4i4<5 zRz?<1Mz+rEc4jVc2tK#9H?=aiGBaH{hntg|myMH)jhp8v7Y`$kB%Hz{$;ZRa&&zde z<$NOxGrQFrD%yKl+1MB{av$Vl=Y(7B(cH)_*1o;j4 zc~_1(xxsC@xVhLlS>O{{GZSljxC=KY7bpC1!J&gzPG;uzaM{7-vsNB$=3->AGE7NM zf#Yn}(ua*)?M!SXxp;VvxE@_O$IZ;i*~;E-^=-J>x!Aeke#ncucsiJ^9Ja8xx3Doo zj`GMba!SHy$Pdqf)yaWB?2K$z&sBlA}+}>vO^;XYu zfH(iVhi{;8uLB7@DA!o~(Sy@=FJsH+^Wt6r?YbUQBaJDou zwRc~+e)-V)Wm%cXwdr)Sw}<_v<9fn%Uc$xj1>kZPzYeTjEulX5->SPDdW>{?$adI0aS@TCTGXUjEgC zMyqyd@#Xf&^}lEB;GET^|Me>%(eXPA`T4S}y69hhS$H^CUu11rgam(F6@HA?KW>`YgGl{T?4a(1>d`PO5;b+3QuEM}(P1jS$5 z*-u?%og=J;%JpurcFf7l#>mCW?b}%UvsL&#L-;xn6?-el-d`M+o9{cz5ai@z7g)_h zYfp2wcXcu`TfOr4`Qpk0|KVeQFvy=h*2T!l!p!Agcr4F>KW1=Z5f?HQd$L{3f*W+W_8@ zGPQDJiMwcM{rncjPH?m35&xr`BB5*@zyc72jA|WJ9@N3Dlere@|w zuFDf-WMgH46dfi263qa0Ng=4p3c#}D>Ib$~rlts#`zdn!oyq5qL~1jP?60*V)C$1q!Es!*W%W)eN-$EvrTDp#5*%L0MH^M(L>J zQF&#hAMf+G@qfi0UvR(|)bo>w{+G7?HR9mc=C9(p7P{q;7-f#sj^G0TmY_D2`iBAh z-;a{31?XQbMDnc_L|>|=wX*fM;v?^B%v{4H>%kCWm%q(IECbj-5R!5W{(wo5to`l! z|Gm;{D-9gv>Jt9eQuF-jmYN5a5n0J~!9dN$$;!?Gjv=%rlK9Ml7~YgZf?eJ zf|TWdwMfCm{T-MRSVK+g&1cP2)*~qa&M%IXVSEv6H73z++B! zUMQvzHU`Nf4RzW96UUxboe2UOPC%^D%J-fY&6{dY(r zHo>*Tw-z4-IKgiJn>9F_;F?E$83NxghwzuJ!FhfxW&T0+OylyMesJ^Os)^V8*FW;ezgjdGT!U|4Ol{5f)_En@UniRX2>q7v zznu}HkgetMRgn^Nl{MxR90b?WQWl*qeH>VI_wAKvi*bu9}#U@tUH!3Y~w*$p1pVT-6_WzIR!)f8$x- z-*nb!)uSR&|4ID5%ImMM`~E9b|9>f|^RBwVs*Q39v9I?LAzq%9 z7_u$_t%3ISDSr(btmT0Jdo6(LXGs3HjZ#_t=pSvBUybx1+4bKRxxT%U`@69DS2W3= zIxp8M!}_cI{^u($1=iB&x)AWw%98*4(6JIZ{zM_+XY>Cd$@3 zeE$La`@ZPo=4AgOCL+J^t!Bk_MaZAdAb(!;{SM&ye_r(Quc?BoIg(9~7Zk-GTVK}A z{!dH5wcD(r!2f$1Xf2R^Q*-|i4>ZgytxQ0${a>j1*6i)?st8(Ir#gk zzF!*7f538otonXAg1@fmu1ZATnC=fL_6NxQhgN;dn=F4DbpLErs{-QBF#CUFf7$I$;vc9c{=FX)HX-~^`3v`VUs;JyY;`tfv@^8VHb(?AU`Qh7ozS&H}FSvH#A8e*s->vcc_RTCm@B8gHE12wu)?N6P zt>tg+_{E+WuGIa|!&G+x^0czH@}1en;1DdKLFFzW5hd z?sWlmtygw^`_kI6|8W28S0`fUTs7J?o95@|Wfu_oYSiD^^nbvs)*N?j1OgyS{bUQr zU&*U}x~-At57$%IrKUgOKF{wQ?XT`Eg5Q5XfdJPpw?zK&o+2ppZB@f2Ao!hk{)sZq z^7nlGgRhTV`^K=Z-_*I@IF>(M`)<&c4^}ULECOHYgM1rhiq_^gSXgK*d1=X`9{OYP z1Pj|=eE!@Mw(SC=>htIuA@~M5ff;;ZW3-*oa=ByoFR^*I+@HxlC9i#dTl;+u0|wL3 zYJIu;44ayJrswL1PZf3BPQ=HEI1jthY(3W$FXUW5T^lDf%=gxegeUfM>#4nTXq=z? z5G&w>qxBcmjoStJ4xywae)vImqsX;QNiKSvpYWC_p}-~~{_e+5i!9QEWVG+dz)!B0 zKr!gQruqJoUGM8bm!A|?K9CEf}ZITBC$5t)lNE72}1QJ94ZohU49@xsG?_(8+mE6x@#3!S#?_&*gd8 z$nz&(%(#yPERRN~nRV5&Lm7QNrrJ!-wWl1psk@s=`P!It$aYQL=fPWd+l6epY93kN^f446!BR$l(QGSBDH>5TwuMs^~9$)x9^!$oi)oos1+k%e{D1&3_rwo z`A1?ufn0IOaDIyV>Tp%maoYQFBMJu2ii|xZ5A+|7kR1{Nr#!#%Swor@eg*TV5Pn7KT))G91QUK`eMI&(2m^bmr2` zNOO(Yr`Pm4Hd&P;BNF?q+A^<`Nj_!MakTc2$an=>?VJkO?|Rxot6{+)47vqtvK zb7CE#+wDs~-pa9h@%SaoY7mByl`Z7@RG0I!g3%9S?Jq)i>fY44K90$F-CguS>h#b& zkvn;p9`Ub-RXxamA>+uYg{jk`bG-p>3sY4hd8DW_y8V^pIyQZn(-zk}9~Zign%|Cd z3L|583Z=jlqHp0E6AZq8p`2%MYSERphOkR6vkU$DmgZ9uj`aNgP4vEdrg(oj7ke1= zd!_DA6pnU$=s8s;5xj-lFA_Hp>q-G8oJ@KD^U4g5{!nI{PTRbkXJYfUcw1@W)QWeA zdOdiKr>nMSfm-vj{6(f_?SRR*_)Ml^1>?D$aWz%%-@F(tYYE^f2QuCs}EZE;Me#z&Dc zD?JdMZF`i=-ffZK;}k}bZ;CcNLnTn(k!Szb-;wkYUGyMsyJEr>2C-|%DF5rXf_FPGk`4VYPk)U7VveTL}5M&z` z_@Sk`2)BQk3J+3&g>@cS_8IF1sy{xr+n&lMQ&Our_3I^AFe-ek8+U)2P4ST_nrypk zzV?LmS;Fss^U@(ort*NBbl<=lc?Ql?OA8GAI<`tefG=3eu$va&;ZOEjHgcScuV0NX z%X5=LgMH5=wMt(z{R~Xq)44Zemq;6u5*qz9^JKojAaozlR({-l^AUs$!fj4j~+dBgZregNHRR|wo7lb?bnr+ z6NI6YiTSeFm14rTZ7nU-7sFQkG?@J&=Ii6oIL-8lt~N7mqfakGnmn>oJU(6(9;hZf zIg-1Uqvwyz6GzzjowxRJB6F52UKe}!gi4RfM=*$FpcwXctpPV|Dc zxkuxzSHO)!%CUFgcDC9fc;KOp`rYJTtluw?tU;K+Z^M=TA%$YOsovDlL9&J<12K_! zmamVaQxS!=(KuI+~qfhBe9B=%v)X-54dkICkl zcd*JF3GwxiNbwWOb6WMSt7yE_nllV~^wN&pJlIm;Y(XXLl9FT7&TYQ|;y1fUukQw% zwoIeko|E%z4qYAA8Mb9hpHh7QMwAL?E`1v9jCX%&6_=1PRysf7R!w_a>%&Ahh2X2} zr)lmx*9%p>JTYD8-&#CBQDpnF*s~?uLfOD}AWqM%y>ezW#^TzPYx)-C)%}u4;id1; z-*!_dZV*M|m=VJ(;F zOxTJ{<75DO9j-0GI7GRS3zT6)>_1xRi`TE%oYYxDR!#0=@lKOZ4pWgyF@tGgb%N>l; z>6mFXSjJJ8jXkGcdcWQI@TvIH)K>42JGXUrzkU`cc@+U0 zJ1=YMTh%r{=<5&XaQpns)$OIH#fMIq>o=b`&7N-SJUKr;-gkO;u626Bz!=K4Nf?U^aYaX?q>OzzWoz=blLb+m|&V}P`6*)Rf4$D zd|HA?jt=HYeivVkHmdOeTBmq!VfxuU%?$mPd`A=JW4qkD9CI5F?Q7p@5oh$`mSQ$v zi`FB8O#OB8a5^Xap@LDnq#F-t?`k8kuE`El05vee~9q)FZ{;c4` z*_Yn-%zL|BUMS{{rWbX14#!&zC!8uR0jrwuO3f%O%*Jbn#IEbJ`{dkpGNMDc*qCBZ3Hj=4NBpHG6ER2wcQ3H1cY0opWl@2)HLuJ z>^LVEP<{UGu{Q;sO=^fOL+s(VqQ5okh18Pz#D3iP;B_0WDv@B5gq*?Z$E8}pBcjZ= zHfG6!dPC&7(PecF$JmpBK@h9Mjzto=iwK$p$b;LL@CX1`g6kPg(|mB zTY(Sm{nGDWKYygHs{&R-qq!9aFA5)ERNB0I8Xd8ezR!&MhuyBuY@5ip-aD3afA^_{ z=LL4#!ae;4ap^YsW0TY7pJ>2Kljg1@9FYo?xN@C%>i|?0d5!~*g{F*C@8vo@L@}xr zmq!HV_8#LgdAymzdc!D5{pZ%Qr9sTrPl}%4`L@mKnyyu^iZo0E?h%F};YJyv(I?cc zMTci=`q}jeXJjV`ZP0>ZJlNu;>E{ z;=`BPY%7iSBk9MD+QF#}52dj5zHXiTHeKx;q_hiCkPed0T9_!ORpd|LubRK4{+q{R2!(m)-vV22)B`zJV9olAshmB*l+r6i7 zjK*fn@t#C+`MZZTU4D!Q!1~9>^LpGSs0XUfx$jio(e!3(hF&Gi!x2ie z{KU&zM~L11VP+=nqxIZk6cfa{3GncVuX*^H;nG!JR2H*>Ld`kWt4oY{a|oj&p!EVR zG73GH^D}RIdbDL(OWrrKn-IKFcKnPce+&@(l`iL@2+t#O#dCwv23<3map=h5D_kaN zQ2PX5@vEuz#on+fj6y5Okk&s4?~a<4I6DLWcFYHWskT+!L02f=$Fs73Out-Oq$CO{ zL2eTgkhxWlTku1ID`b{V+>C$^-&<<%j&#Ud`0+`Fle29m+0aL;Q+!3#JGZm=qI$p; z(wKf+E?#2BkZ?)K2Zm&2uWy8R0Or;!CSs+5GKQF`g-S~djTPrLa%UF8s|f9e@GY!6 z_HO40$&#c(sz-`g|6D_YcToQq@b`X_S~w>aMIpZUUM-$#lN}wZKS``!rdn|Dl3E<+ z!-Z#`y|G1q@7&WxhGRLhhKx_PTO$~9tc6-)N|ZNn_s0*B zxIjRhMAqk@cnMRi0zn7KZ(CtHU+}c#w5WwrS4Q&lqXu6}Uum=`f-s+HwH{Mu)NkqtxhsAivh{8Pb3JL4bRr)9 zE}%&cQZ_OvUfr?taJ8z`)8}{8ZXYXh8y_p4uFp=_D~jq?8@Ya#$}a!!Ku-NoC_iErnhO6Zef>qIM$dGH~TJ^61zrg9*{7{9t9Icpl=LDL6mF1Pb;F&#n zo8p;~qY39gu;iZp@5gEiWGrvV8kL zJ((!NgqxmAIA*{I6O@Y5kKPVzyBV96tzP`|2?R#y#(K|=y}_ITE^PbwBgh`MTBgKg z^hK}cBMR+{#`!xTe}%VXY(Ul%U58A|^lRp#x1<~wL{A>~xr{6~sfV5he8`0gag{tA z4t~&`^YI%tfa&lRLwJ^jUJoz=Zr!e$qc3is8wUs!YLpa=lo=9y2xcZ4DKsaB6o`^M z;+k|8=A?>-izh0pWhq@ESbY)@;IO8<2+Z@E|ZH)icXRuB~D=QoxpclLP?%G z4i+LKJEWmo*2UI$ z@<=(3Tk$qWSHt_9+nmD=Wq70t&P8VBTmW&;#g^s<3TY-kBu5>43Ypln?kb-%iyA@y z$6B6eZI7w!b=`G^q9{VBcb}N_BNJU^I^F>+*Kn6rg)&%fr20;`I^gZHyK}CN;HH7Z zmOZ5<=h<_>KGG{uV;CRPrj#QGC6tYi#lb|i|@k+G_miq5MA^N;+VQKL zA`)${BK2n-k=1cwhABF}1NYA2$-_yJyz0@&>#zPu90jL|2z!uD#tVr_l1$y_4jL-2 z?T^xwzE;>0bx8d=6wJ#+jyew|SjXBhgu0$N4I(+v7a<|)n3(NGU=y>M$}JoOi(oh)e?Seq2N zQDI5MNO_MmM{fd)QRAu|Nm`rEzs+jJvi>$J#JK()v#JBp_xea=D`DTs4*lvKdC@+p11f!oDm$T}N(8c}#s5 zH^ea74H`2!5b1!L>nP4{-bbPp={qFcO@agt6>oT})|XdqCCid>G;%kNSR^jr6#sp& zsu>-O|LQ0*{wUJ99KuE;=L#J_TO1PS8k*v)h*n2H=MiB(3pkXHWI*m z=S42188aaKXMvnG4^4RvKnl#!G#>!|{-DHT^#GXTN+auD#=v#0q#>KEH%>xKA zVC-JHryOI(sUU99rPwUnyv(XP(j^v%D&4%EI|sGT!>w%8`*1$f11jzhqQN7P+kjo? zYPG{`<_P3ETX?VAwixT%WdIc{Seq~6udG1d_$`G{iDp=VR7x*BEhJh?(Zfx7AKVo* zh1HV6OB=Di=*#2-bL?r6s!5lLhPxKhBPlR(7T?k?ql8OQ;T*(Qv!v!{5-T@-O_cmp z8phAtS_$K4UAA;z-=Wz}y>FWC0vUfqeWB1pb@|-$P2QU0l4x&*hq5#l0o7)}j&Q=Xzdg;WLo zGR1FIa+Fa1w21}mJI>yfLEfuh8YVXG-~rnysM>>e9~0IcbOfvMuv0Ob)bB1ygRUmu`@&^24?GAD zu(VWntcH~f+^p5y$^;Gs^F3X!ks)m;2~Tj5MbfIoIM}1Ux5?Eg8pv@qa;XV^D>rSR zI&?V{-o-LK9C?@JH^j!S8%Kyab%0Gb0Dw&acbK$0!egr8z}9x5;W!gSL_^fqErPvR zR04K!z`A+4=G7B0#~C8nn&;X67waYCi4VXmY7f`P+rDnN-g4he0nsJLKsL@jwJ@;T zb+`v8O#Tyd4x>F~Wwu4*xf4CZfYPUjpJk*D!y|L3?2DhXm(Cd%y}1=@^Z3lp+?He3 z+HYT$j3)+c*|f-ia<1?6IB0k|%3>ce8v34Qqa8V^dLDg28X5YcK<<1PJ1a7gslZ;q zkm*)#`|16g+*cPA@H8wUx8DP%)eS`D4nWHBc^weZy0-|uA|8EN;;kKB3QX7-B5gul z)Y>hgSLl*8{Y<><>=R(}-cD2nY!%B-UwU$px$~-Zx)mb-6}9f^l64-g&jHc6X|V)m zVzy%A?zT<)(hS8HCIh&lRd%$#P!{h#E7*@;)ay^!a@Kuu-B{kT#UcvLoq93}5Bbmptwulz#wo;2a{gsJNc+n4^D z-zNwY4$RU038nnS+0O1E!Ra1iA_ zQ6Pt-Y@sn!7Jp}m1ra50rg48z>)!bG?lK3)W&dbthA1&_1N+Zowl|ZFP z7fWzzna7xo-LGL!Eiysqz*v6UtJKc5TxMM|$?^WU zU8+pZUai2>r9Sf=c|9jbQTY75h+L0!H4um*Rq4G4jfdP2VziFi3@TneOw7%*!cSkG zoVU$uUxMj9lQE|ijlQ&hrb*3CZF9}3#ZiNaQL!i@j?%dswpo?r2%9(Bnqd$>6V}>N z^5JEt_G9#(EO}YWk0YrWag24)#Be;X+buSgVqYNev26%L<4sIF+jHuaCeg#qEHz-rO6lLw{=StY$q+?$)*l}5#7=O0QS-FZ`Zt7uCa23V#dy#`Ov{S}U zFR=s%JysT9G>xDVa8swZM3~ZU!ZSe$Hwx;P(*cB#CuoT0I|q_9*jFXuWu9*&gyMve z*xTaiuE!+qp+Hq`e^A_;>JD{O7n9wq>hs$}DWK&d0cpdqBda^;04nkwo@R~#^1UCo z@qe;T%L*PZdRwZ*ib%{|(Wf#@kLjQIy!b)MLt;;_AO^)M?I0pFaE52|^9|uR{Q3A) zCnBnnv9b(X_1s<4aWnhHm5UP-LYPlH*)8ahC_059n}wDOL2|{byjkE^O`%K=i2NQo zmps|Vk*VteTE;l=Lhc%S^YhVIy>5Q7xKg+pIG2D^sbErydp&a99-qx-#If~y#2N7k zDlN%L9hkv=`i9y8d-%+2uLL~~>6s|`pzr8-W5@4sPuyr!mJBQa zDIlJTPzo#7gB6{rTfL3Lu3(USm?ofHLNdv+`R?WH3+0FWdW2m^UY&e&1Lr(WG!B2E zmVlw71Wbp+3(0NQ&xP!^$#k^sy#h=BU_wERi>fzHp}4mv^U_DK{Chw_ri-P^dy_~J zSlB zW5%xuN5q{1$KLb<2_c>Sf*PztGSC>&u>j3Ng61I_nDBOwfr#!!|DZEHU}+p&)KHG2 zai*BT(w0n;$cLbml5!8mzl2nsu9bxR7N}a&=^D++THp8>E{^-Bb5*D4vP+{4PPQuv z4Q|UTsNa&N^_Wfl9Sc657ENYMRVA%jF`9it3z-&!DLE?*F;1q_q;b%shz4;}P$Z{S zBLju>1)y?!nJUcV{kLzlqsk9+U^NcZY(pEkJeLuwB&?Twk9J_}52E;tofUAh!5l-# zKc(L*pT?6Ze7z16Q*FB0IqhI(@RMc(43%`S69hPxer3bONIeJ1hwpH(+3=?xhI^j( zi9b{sUi-;mcg1nseR0rIRZXCOGHgg#{DDi_asUf}0A_mT;;{lwB!KN(4Pg89IZbW! zNw#u^TbN9kQL!-S6X#!y*pI!t#J_XHtxqQ^J{(H-jYl2C#QW|b)29{N&`uk2gjk~@ z{)W%|YES-8xbx=b7 zzFTI+>>>6ke~wnvX@*S3SXqBr8lU&i*)I~r)@5#rV$8qYz|?;fEk~Soac(zevHYF? zgDV-u@4}o}J`Rh2e#2jq?`^1F@%CELBVZRt{P{t9Xikn~T&mR2u^rR_|5fHxl%`K( zBR&~o;HRTd(2vU^n|0>8PY46V+??#Ey_7Z$1Y_->nYoKLb~NJgPk;QRlkQO==)^%1 zfWw6!PU=AtyF}eXVWd55__6dZ;y@-$)FBLEAj7Ai%v>OB^G6+@(M6HS zHDxRHQ9_H^Ljv1!ubURnt|^&5B*iU7`CJ~9(NexU0^H{ETnY0FnIuOY0@@3WvmLPi z?yXZesT=r@Cx=m(Vhjhub+EJ*@KH}RT{yH%p{HvHg}%d~H1g`+3cQ#)UMdVHp}}*4 zs8C!CD54X_6b(`QwjKGb)HazAd-Yl~5E<{z?Q6%O*(s+JYk=Dq7kg@Zb=X4+A8DMk zv|{*`8H2bFv3Dwk?2|leN*hmtdc=rKS>lHYozOpf} zmKDWh+mv!X`;%cUi8HrCu1AG^2y4~uQ^jP%4gwpE9o*gw}Dj=pjhX~6NDL*UeB zE>%KjvdF=piOrx@N!(NZH21{)O=G1kO%{8#`tf+lop~~Mm1s-x9Ctg&SNNbPHt~Pb z_o_cyk!J{3c2gWW*kivx3`DE3m%y>nLABC&B%(Co9Png zkzRg9CfDIJ9G%}Y&zm+JSDup3*HUY^nuw{+?2Tf0QzUqZC# zH)$?O_)R1sFOrn8DnK0;yY!qkkzSIv#6P5B=yY!=hHJ8jWK;6_yGNn;bO^0wev0jXOa zl9_U<{G`>yJocd!Mk<=|bi}*imdx=ks(Xm&JZx@xR4hpO9>KXjJJBt2$@;D;Qt=V{ zS^B4kXX6J-!f&(2R7)#kM zA-C_)F;!_|Rj&)_=eI@2+T0H%+R2SsEQ;1AGeSuw0ntA}d78#6rG`7JUU)R$<$O%g zX3C^XuNI`rl&|e?2go{8IDO~T$H8d#o!sfApGFF;2M6Ss&Vme4xJ3Hdq-3VWnM*=$ zP`&_K5yqhzRWF6cpX7oDpFx?`h8W?t;`1|s+oBP^U+YnUO36kVkHi6G^Q~n;W9&(X z#zEN_kc)~)>`)Q~QAEqKcrlkNWxzRt1e>`aTHd5)5hE&jtRsTIquT5}I+;D_L2g&g zwS7;aoG?eaCw3a&?^er?F1W1;?Hc}N7i|L%bHALHt;KJAc|rY_=k!q2XnSg)jFvOC z4_c?*zHD)5h(bD?r(W=pS@V$<79Bqtv9SDm8sl`6AC^qcmq{$-EL|wewZkXbb6J9t z$%j8#OTVG@@biZzM?vy=BN(5-+4js76 zwY?!DHtI-I)FwAQ-Z!Y@K8?&FGzQKjock5R$GG-kddIS#xoJ>g2GG_7pP$_d-h<1%sb9-VhUb6(`GJ z`s$xz)m()7Lgao%y~tQ*fb*iHmL!A3VM-yVq(x}0$bF$CQqVIe@|o7~NSi}~TZfg> znL?<3ZII@k&kMuiN>~**CeXDToU=uDBUF2=%$a+QrVnpwhE|n=tb>!14*fk@yovf9 zeP;V&Kmn$rV0FoKx#K38^pxf%(rPjjduFG;WN)W=tnCP^)7*zW5RaX~6N+jjLQb|n zqj{*s$atn;ivklS8ahGg?|YY$;t6Xa@#T1g#13cgdt9ogzST;Cx(|xY^={AsglaR= zbf=zoSA{6UMp3} zDSt1*87hS>BM7qffto_;6+scwnw&!bE&#GNLHZW_!}y0DY`Bi&PSqj9gs*F&amrSOM@qKU#9?McHKsf4UxNvv|pjfBC>*E}AE z>RmzvuTXP*%Vze1$FHxLoOLuN%0d%c_eP~k)9OcV$EQg4nQgC%IFfFWK^lB@bno@s z&da3JH6Wd;oOPS-_5*pd4KyQZLs}s37dLtvlT}lEgjU?PO0PBq^|rd{N)=yFF0S7G z0!Pi0zx$)HzDJoi7Y8N3&1?36m`~8*(Z^p~nZ_yQC^3aK zFB&hNeMj=B(Qj_EzAhdB1*Q&#oluR;(J0!FA)^7e-z7j3B5t&; z&ecdoJszokRP>)}Xk$SkGZMZ8@4stQ6zzh;!q9vEid9 ztv>y>hCwPRuShvU{QUSNS)wSC--|qz4b`H^-%)-Q)_Pyn5n1ZNrFtgHXkwf_bnHkI zMkvX~T@JpAae~jOFS6-RHt(jO52Gk2%ffdLu01T6pY$Hu=PM7;ax!b+Y{Q^c-+J|f zyXYE0gPv`CA}QnUvQ{blM>#{l%(xd5a~6{Z4dQ-4v(CSNqb3l1a$e=NN1f}M&_=u! zDdxDg91l7L8GnK2#ZEdE&KC!dP0o2vWgTG>?4dum|2`H?7Y>j@A%%%CM?xs{H)PJ0 zoB2h!U%#MGT)yb1Xp(h>B6(KisW+bH&NLYZ??kkI02}ZJ=-#0kZ}+vRZIB`4sH*;fly}!n`t#XjOB7Y{8EBk@!FaDHIT-B9ca#pZ6Z)bGn{farp(`c@)JZ!Kga^&f3bc^2J-)+2%@t*YD-GnT8o1 z(b6y9sIXl(3qO(4<+&~eNMp;r1kF$|mV7cvB)^1gF!1bh(Rd21@CRmnha$r}zE+|Y z+>Y|-nb*K_4)n8lB&*aCa+u2u-ILYD>!NIv8=8;5Le_pq^-h4trV~@LXZ9Q==BR!s zc?NrFAax#SzoXIWIQ#He1@Vu^Xoqa@+kSv$@$4r@DLFd5y?#eYI5N?_Z?$wHKbis$ zTK~?^Uj#>kGIRDR^g9gk*M4~EouTJ5(_H@$r^4?7i@9NeYyNzj4oWk+jm`8==r$r(|%f}Ksm6A>&lW^e#QJt$ijcAS@63v|eE~kso z-+@rZ;QFd*;x16e22}l(#h%k+&`(wG&5`A3|NMrG`r8~%cBL}MQM8T%P`I$229uB8 zhZjX^+Jc^z+lK2-k|uPO-PGW#h3ZeKP+g9A2Viku zs)Pk}ax?gsLt}Mph?`eLQ`fbWK2;MQaE6BQl@{r5G@w+Q>fY9yvge-eecleaDvOEJ z7=s@xe(Gde=ap{3y75p-c0P^eO^PXJmi4R4-IBsOs0)%3&U$783@4E$a=J(qj{J%G zi)}*;U4+L?=1``dBw{9d*YTFMsUrb-IfV>3gowt|u1NdyD?sUarlpwz;tp7BQ?Knk z_E1QbzHhNh_Tk{&9g#){qb|(FF=y3r?egNgvk9?U>_YY!WNQJVKEr2rjjhs&f?&0k zJGToS9Vo=P^A}Y;AqiiLI~oNH|s0nQ^!n zA~D1JLv7FYHKfX`qd_Oy)ZqH$)rxHR(LKuxjZ+Dl(!P4RD2mHmjqutoIP!7K@Y+c| z%*Ri0!k|!(j^re*%o9t|U~@NYwSbo{4>v$IOU&RS+k}w)e+#g4kG>Nq-?0k{>{$Tl zoB2kdh{%CHpO%rABFvUgQN- zxXy>S(?fO6RtHPGwuru0fsXb8WT#RVBG*tqvbWAC$wlICF6`TCL$=o;Ac6YCWQD6M zQsp5%%JZOEb>*-$iMUU66?(xQBNL>7Nfe44H;@K8KtC}CCFDMAQEX-}o|H<3HaG3^ zi_2?aMs!x`t`J~=<92e**Y`l{2#C>@KDVqxBywP)Qf*L}4A|OHB#gRt;+;ISge}@M zBuXxKV~@3GBi-Zfr%nxC)=abQ%vCD(7|W`%fd;7b^C24nC+6SF?+;T-giT>8Wtu5S zgZzWqQ;70qgD5RX8TzR7^O83e5Z{Pwck6JBF;oi&B&Qyx&V#0v2fcFL1eE%c#ciO( zb@P#^emmh*FOjkeFdFg58PjRR%x?CyX`IykZS}9@lgI~ z=#pLnEE88g3B977P?T%sdN=}a)DBB+20WUVUeC)@pM8pku3C=;Q`zkyJVBZ;&5&D= zR`FaOI+OZk>WJn0;yLK7i?4YMdQ-ocs%cbh z4{R4K#DyJYPOqC&qxn{LH||XFm;!CC9hXAa5X8(>pXoY*mSlx!kjtQwe2Q$!%nM!y zAR>o>*VyYHzdSAJmXR(_b)dH$;W=>UtTfWhbm@UB+j1^o0)M<9WM^eJ1A3^5ERAQ9 z52$T~M(C3$^6hH3m2&3ami1KLn&e-?q?c1~5QqdBFxhwt2Rj)d(}_+keY{P!i}-LZ z>{PtL#ZeqQ>_pWku(SwG(eCMc4NiVcc8;hp#)J>|xi74k*vTrHD$sv{0uo9eQLhHy zJq?O5vROrfh!_?I50A@C4;?ZHn&zmnB$PV6g?=f%jMBM-GW@p^yAN`Ya*$_cU-3Av zP0_-DwEv6Fk7cv_S_m?^|ML{Vx&l(vy$!s_VNLO{zqrbd2N~WPJanpp_{!wu2h((H-@#Dl<3}H{Ti$DGz zt>o41;C}s3WCy6k`JR=d#Ae-t@8k(v4LV`pL$<_pXaLvAg}Gub4(_c?4#VKTb|9pl z>=rlhdXqe{a9HyJ!Y}hA>E(OJ!A50Q@e-hD>iu!@NHc)Qp8HVt*>=1$FHcQLD0w1Z z$a$zuRe@~~>d%7*fo2NYlsJeS@#F|cbSJ%Bs<9V zw%HYpckSwHwN>bsA?|TOajf+MxciRCQTrWBc&{dqII_|NAR+NKj5zV`(*}Ywus_#_ zWUyuo=!1TK9{pSrX@8A-#|AgI#k6yBdt_SeN94EQ*RZWJ6VVLK-nT*68`;7pTa13q zRB?cS@~P$k#GV$U;g|W)W}5x^i5^pc-wNAy>AHh}>B7!}8hsTmY>-#OvYf`u#Z3Y{ zBu8`~&m~<{a{54kOXBN&VSWy17^nV9sJ(|LZX*=#Kvt7w$AddH_XDh(K*2Rc8jIuI zI-U^dYU%gdVMObFO2E$BArzZZ**-Wwm|a{xxG8+AH1?qcF&|wK;;!np02wmZlDYoy zWzlp@6lp1)P)yo*YXYiCZP*hPG5@(>_*yn7u7%zS{J=HOAWgpo)8~Xo_B^@~vSe~E z3;G7w0?+Q?ejnO|xr9(E`L3fa_3>f36q}r4+4L_xjUSVNK7k~YzT1F@4=7ZHm4C)> z`jnL6x3YUICC<0U9oPKHiSEa*t~`iD5(KEA^=xNL9=4j3x4axe$h~{>ciSLkjdm4e z6HyB{9m}=R=A4lDytJryS8kI*=SR9XKJliK5Lh$b3Us?%)M?7bn&>Wa*ETP`eDNsJ z5i*aNW=-32U+f9}S7Jjo`>1H_=`Zc;lryKYsJm*`Wn$BEOF6;LCa~?2(Z@(_Xb%=l zsH2p^#UU_gB`Eg<-LkdvN$-^tTpXOwnP)~Yp=7q;4DPW9(xf0?sgawPJ@eG2Aec*H z$Zg+>qg>oeBbidJ82C`MTJ!`$bWGt)Z8E|l#2V5bQcEKx#=jDtGMHvIP5}=Ey@o?YdF@HRX-kP-6t&B(Ra9pFpb<}BGao} zDs=wRm}pECMbehX!eOJveoe3uiQ*<+>lSI^PmiY^I;?KDLyyI9V8NKb%(j_}+9Akh z65MyN@#@$)8qv7qZEs_xB<-9-DJt{nDbKs@v?jeW#{$(f(&{uRj*}zy#ySyUR3uP) znR`Fz!M5+o8Selheg{I$!|p5~f7~i%HD)<+Z@?NID@16CpYyMwcc`Uo-AB-M7TM{2R5Je&DGqHz>@y(&wc^BX zL`T!ujejbHK51k9iU=!ECJtKkL`U3#*ia$ABN`gm0(?9xKGopSqqeV+965C%@uDI8oyy`^X zfVCcDKQ0}vYx?pL=w0fy$2plWE$E`=LX!FTZbi)v&Vq0oq z7YA|~tuxRILd+^h%uozg#<2VC7&;F&xf^$h-a@)zoPDOG6ICB;^~Fgc5g`7-f`f&{yLg~w+1!_-o z$=2749TB2!CKkZ4+HJxgb27YVxXXUgXjcM;f4(mgEK(+To@|=Vk1kZSFO;Iz8NcN7 zhf|-2?^aP_)ocVfA^7Os!_I|-6YrkbJyLd~G*%~`e6g?T+`f%mT!<-DOGHXI^m@U* z!vtg}J^Pk3%+Pd#4k4b$X(g|(2&3zK*AMu{VPaSQM%Bzq<$UHB75N>x1TtG5Ic;ba zKl|kDpuf)M7EdjGk7wSrQ#Z6CbxwMySl zEJm+J2m5H{2*p(9ipHjJ!CJNs8F=28}k#`b>iNW7+90%!ltX=(FCW|lTq{W#v3oQQHNmz>b% zQ%MF+iterhpE&3eNZ)TNV?b?v*LNe+$n!jhfxTD_A>C6_Gx8e3h07`r^O?jBO7}gM z4GY2!Ct?-g#O>cvdKK-_z;w2<>9qeyAeLVNvlJHYi^B_tKZ>xVT5_~@L(NJ~*}5_x z=JKes6@~s%ok!lkCZof%mF5fFXnrV1pZ87cC;Ze?%Ut-f*Gl!{-m7>Fejv7MS0+#!g!vlF|w(_ui(BhyC@vz#skNK!RQCf^8 zmByZ!pdHk61{OLsURAyGbytIOOx;*2Z$xF}q`lFiQcra_M@$&Rk@oRWX-1jkgBU(u z7Fv0qr@~h_s=L%MS`~*V6zsUSJDkc29;)FE%1G-Hh^vVnVb?3n=i`*z#$tM?ob?)4 z@|N0nA4S#n*EFB>r@AkWp1UzDHuLJDa^Vb5tMTHQWKwUe36pC9i4a)#zaEWNGa&8M zRT6hqI*B7o?Rv>skwrkpfzgJfA?6W#;cL2{W`;w>5}OUBx=E=b_Z+RW;ZGV61` znf!lry>~p-@%uN>LTRvAjM{I81H7eI3SKg1-)JqTqBUWrY$1jyIX35|$JI z{UYGc%%6*qAB|xWx+KCTU#?-cGj>O*ETBcKZ@xd~^)n>#)1d1pn@PF7=bmqlsH{-* z(|cw!@WoQ(Spg1}$n^J}rkRTnj?dK_EUwQo4XL+^yEy?A`$nesAsZ~#sL{d@g=*7? z7Ge(3L6C2_F^6wI>^!wk{QEiNwVqP2BAuqAD=L`r&SVnoZ*;>J7oPFKVzC!stp`kelQ)~n!GS9wWZnD%&wtE zD7|&BA0eRDqh{>tJRNK2+os_dObsS^OPt`#n>0^o^<Lu5m6l!ElGcii>T??00FRUA_P zGO&QsL<$r@8W;(NweyRDXEnruB+z)sXHr-Hlj-$Pw9Ks3aDe@Vv$$1vkR88Hc|MsQ z)%L?_fL#e6>pcT_TSe(5M&5j((Mz85gv=jc$33-0?@iy`#gopL5`xbYjx3Bf*;_P5 z&enZQeQqY`d;9Xb*zgxU#zH<0NnR8AlfJ-FtSQ<=pDLy-dJn{6T!f%~!f{;lEkU;CC$3gxApXk^;v=<;ugA&c5H>TiaAL%f*{ z>ytBk6YehJy7u7lRSi!+K9Kz8{jhp3o^RnU8U!NY76S$7+245ygV3aWxu~`dF6dBU zAKZmd)OQZme`G#j*lGA$jq2bgL{h3=^GL#N5!f&bv zwo~4}ON(C;;mKtP-5s2^RXg<=&;F}#03I?bb-h)77H=NpzRPk1iJ&9FIVk#x!wH9# z_4rYc?XCQ;)uk{UYI_6yJu{mym~WI!m&kBfQ{8qGT^wq0+I8Ev$Emq0)OrjN$?0eeX!zvZ|pSJvi07T_lU}zEPgEGHP91Esu0LkZRZ&CpZK zt7>cav7G@K2liP?s$2YbG3b&;uo&65xtH!d@Vsh2^y2DMO!3&oOS7f*Zor8pWNCr% zdtpR_<-r`mVHnvn`cE<9`U8C9z3{Thp#rcK7MdP7;J_;&uu%mS8mBEpK0Siwn6BB1 z-i=hW-af;*-}K5)n&FUqLnlU=2WgfqxY6dZq__q{Cm93th8?V{(eqfZrtq#vm7(5N z-r-#8;iq2cM#%s_8tw*|E6a;n&N&(>1D38n@_AWzpH=(KqQyFGmed~tt#^4b?tu21 zv_pY-pJ~v=2OVFOOI(?sJHG&wH!(q|9mDXi_jXUyt84&;+RbEt?w$_QaGq8isA~PN zbvb5*0WThR*2%)*6sZJ0CEWARNr6sh{CDJ$aRLuNB zT^E{DTH_SliK7U7fh%1JU&@E<*-m))gIlrmZ1r~p zC5(gOn68qf!<~VqTQReWV;uQj-@$=J6o}92R zcugy&_Gj_9^*xcZ@GtStRm6?(*w2{T$G-T@Sv#W}B8R`(cvt#N))}N3Wv#9JQq_-$i55SwIucZ1EX-tUzyD(2k2|=d z!|kQS^8k^Ia2bUP>*SXfz1=B}4DdwWc$mvvQ#rg4Y^XO@m zY}b8Ct|jAW?gPv8lIRMS*}KYSgQ`|Hd-JcVXo*$a)$sv7~v6hf(XD=B0O!LCN=dikCTq=~5^SH{rVSq%bU_eL3NfO>A31MEIdC z2r&;JJ3K%6X}{U$!lqyU09Uq?alT{)CvyXW67yn}11RhDfj11S`t=fomOyWUG7WH$ zv4S?_3lSWUkt}|rlo@ak4^CGEBe&}Y0uC)ULJ37I{22L*gqg6Iz+8soZG$ae{H8_JnUZO|!^{8jzH1Ju?36^XJ6nLuj`JVJ^`yZUR&5FQILX|yq7nLRTZO4@+FFVO_*!k-su8rbtV>%y(p1*d- za^NwdNo8Tz*Y5Qz8-7rdTwU>*|K3dutJ(GpRFg89C-DtA;HKwCK#T8ZEmQJ4skHb} z<3j=5K~b~Jc4leCNDr*|@{sAog7`&j6UAu{cKr5Wwjo*g)#an+@W)p=^os7L^H=DY zy8hDNn0ExHrWnZdD_b~6mU(`Q z-X*mL^)G9W-|u+wmd>}S;QB2-{JAGf zXQl`gJVyEdJces==1!uYd2a9+2WLhlZV2=m9)18dM|o5hw6MT_@MDDV#Y` z1DY{b-_atph)k3+YZmu;%gS)@<``OG0#`E)Up%`Ow6SeJ! z-6K!Nqqwd0`ff3WU89xZ+0EeLX@b!a25FPp*qDWpXSIs%Rf7{btHih!M=b&FkV<%N z!~=S-w46S<0}9@H?xB1*P}KE&7I*K})h5k~2yfW+OAD=NG3|&b`~nJ1K;}6d@auX5 zSKu;oJ~h_RT6nJ&0D~Aunn&+iufBTFnAMWVeT~mppA5c}X)b7Hi@TGo@3UaGY0Qqd z=k5k<0^qLmQJnSEw^AxrB{^mG4Hr9pj%sEv3rtsu^w--qP6L z6?L_4ThmkJ?lRT++JkrRvS9l7wS!{j-?`!#;&pydJu1qdH^a59eR9~Vm>sPCX!4+J z>AVeGx^H^xhT+wDoXo>d3IrOcLPL-uS@GbIi;$q1C#) zZ-2D^)gCWnz^iTJaZ4L)#u3&vA)8{&8PYf$@bOxKP=|THD9T;%QAe!bIVbV0obtju z^^Y=Q>*9Q#C;{k-mR0py78B4l@=4PB*kGdFqhtH;|Etj4fp~eL2BBLpjgksr6@oJ( z+ss(1;Z{~-q>pn4mLa-g>s?&hwL!NGD(#lJ1vX`XH=d`HSw_NoD@CvA1S9Yn2RU-JJ=pJfV^*7DC%BK4w`%KrdR(`Is%VlQK*5 z#jKZu#m4I;5&9by@wz}l_6We?uJP^pd-@|U0nXvLS2g#FF@F3`fJ;8Q);N_eM~v+CVWoAf*_!Pfm?+XUP_=Y9DF*wo`ScxbM?~JGY=+aB#tFG^n=%4Ur5xpO z*bVFHT;TVCI~?vnjH{lPeoFNjXSiu=nUoRe*8D%G1L~u@PA0~1)beXP`?5m%%-8!_ zPo-q?BY{A)yk}eLG(hp@w`Bbzobw-CZ}hbC6lVgJuYJt*=H06w8j_W-0rR22Fg0eg z6s+_(9pvaqrS@+tAAKuhm0bONoiVxviRM(jy!2z5G3awv7fybztMoScFsE_HZUHsVA14fKr9CO`I*qx3=_B5x5)(FK z^?A6~%EbBj$G;Lbf)Brj+M=v`x$SF7TaCG1O0tt@{QL2Cf#GTomz9oC*BgSB!EQi% zM`vi06=xKxCmK~TX|CT_WT8kLRLUxdZhI!9$~)-~8&bYYyGbItHl%W{_iWa-YX_J$ zyRM6g?g2c$HV-Zs;!@h$5!1=l*rtjL|@atEuhgG<_m&B5hnM1ItalTF+xC?wxxb zF|+7uh16_(CoyN!C3&JIQqgh1V*UJkJk`ddzLi%heCH8eCq#5Gzd!iP-Ab@^?B|<8 zdL)r~WIPZAKkU-$Ss5c*p9MK8*%m--nO{@f4_0Y^lm)QTIzl(=;U9WyQR;LuTvet& zUI5g899S%`ne#h!-MjW~v8Nf5J>-VbkkWv##d=0AqvNi#F|uVe5?R0uK3iDl_ERExyS@}i$G*Qxg! z!t|p_&c60aeO(>=RP`YH#HV$V@AktRuBMzp+TnCs*|Vau`!*2g9cGH+kdNW*EWqzh z&&iV54P1Lg49ctJGbwGoJNKyh5#HNaj*f3fkJv-_Z%8j>{-;24<64@V!qTP2T*Y`TGUp0b4?@ zU}P{&FdHBh_(eX*|9w12CDkymb?-aC8@o=&>{Kl4jvkJ`M;d-)x|m4E{==46W8dA-8v-ph!TLvYMS-nVTIOOwg18Gx~?fdbMI zOAyn105h#P=H7f*ZVAPJ*1tGhh61)=9DV*MvVyeQFoZEmf7EI3$NNCx6bT6DJe&HQ z%7ua5=SP|TN1~n3`||mZ*^=V@)`^@soL08YF(`E-;Lph?cZIg7!U-i|DuXsX0Ft6? zzK3k{gst}#pO-~<)mnVU+L4Zl9&0uee**HNVehUR?3lv2R#`eg!%hG=#RdQ{IVz$N zDCQ$WDY{*mh>9l_DY3~{LBU7BO?IFOZj|TFryu$Fu{P-|C4tgK(!Xz5A9enx6z61R zO?Z&SGDXI3dUalvB@JT9ktzWc0!S+M+XJrYq)MJaw|fABQr2f?#-pUN8GiYnv6K=N zD=g zO^4wm=B9 zFd4vwbX~K^pK9+3cG9)oneS#PYx(}45s-;Y`j=;9Kw<7Qt}GchP4wKg;Jt*Z z;I9N|GqXxm{u5ZU2chY#@qp;=}yz8C8md5tQi*_^@ z&v>k=nSNDD#5N%x@(|zsm!2_jB3i0gM-n7Op&I*C|8rnAG{^Lmu}#|_efH`T715#@ahp{ zjA-mAdtKQRJ(1?2Qr*TH`e#0hMegkhhaja=bECiiz-yBax&qwnJ`kyo$EowTo&%ER zkMq|yoHbccmmAc2L?$X;+TSfe!;6(O)=ECY)O%+QFsoR7y~J&EHfC}DhY(jYeP3md zi<6bA1iR-Z$Hh^@+V;gwK?gXu5@3)Q7m#i6#^+wdpv=FM0jQCdsvvu|Lgt8aY_pJ! zq;Mzb-LV0DOPp!x^PSt=(?R;BDURhz-Rm&??M4O)fJF!H>FzEv2a;TD-_RyTR#~SuMA3`YUzFN3}$j0DdE19J#rC zx}Uqui(y>r1p$>srn3jY33;8CHW&#UU;?RFpV9r~f2v5&`3Y%HI_-c(zin z08Lo*m0<&Lz`%Gb*tXM&f<* zQURQf0-HP=lSsh|+ul9u;%QflaPlpbnF}MDZ%KJlV>97c^?8q*w|M~g1Z*eEjtfIC z`6*JwRYtkXjDH0ULpr`a-!K??gNvmDN#KdUfXJGFBdf;i4Tm|ENiJ}p5i>E)8P43w ziv~ubxXd6e7n780nXH@>bS*QJ~^;-ysUBc#-uDT z&|c-a0qA0zUMdvG!nQUerl`wyAsq2e9(gMI2q2n*smsrDu^L!715??$@0LI#r_RN) zK|PF2;;7=!smODACgfRs2D0!;U9S8`W#hKoGkUo!@dX1mdxpkKtX|^` zOmgnJFkiG~PeaU`{Z#6FvXBjVQ;mR2apN3Oy@83o!8p@U!}SZm#~lVy`E6hq6CUY8 z%@bYJwKEw{QQt;cuWKn^cnTuosoS_32Zac2nAwHo*KNVeox=c-syMj8yrg%{lQ5dh z&7t@w>Z8`h*`9`OVS2ytFeDiMR| z6SW5QJ7xBc1P@O6e5T+?Cr~ZFT^`*f>%_z93X$+QsS10T3Vbl$0+)lTS|!&l%HzXmmDXF<5b-|KRs~4I#P$P{~7Fk%M&8l0FMuOn)E3Cx~RPP%P+&3COQ_b zG5@4i4`DxzwP=2QvnIkCZT8s@P;l#U7vH<~h_7f$j}hm`$v45UQgm?!__b@ca?}$z zm29{`ex&4FCl!AolMm1}KiyTVHa!nFjZb<705zqf^*A7Y?cS{3Q|~-02O-)N2^^4G z-G-arThG1Q+j{wDB80bzd|TP-xALlRYvF4vhhsn?uP7D6!BT82f;)v)IH#hwp6-Bp zZCfhtBH459t0`aSMFaG|VBt~T`wpn$>NXrT3V?_CP6?a*i-tc)!lumWrrCfU$T<7Q zxl4h74^L3IkbSn_QXqt$$=(*PM=_r<-QSe%DeX+kAmOgU%18Fc!Y z9H)hoNpYzsi`ZbxkW~EOy$W<;C`BgcfUgC2Gt&S#tGB+ph4Xa!b8%EV4EUNMNU5uL zo=Cn{h?;SCk>dLxXP4;P1ZVbFc}~&Q0_gYBjT?AbD>($2 ztiE#LBp2+kAl-I?JSa0TQ6B+?`=I2W zzp|!hfH2Q$yl`b?+NuiirfBoM=ce=O+*f$*6kzZ7Tww+9p^8aOl-v4nw?5IEA1z

$BaM$tc(3kxS)535m$7Qj~P-8zhx0>+CmLY{&iC|p^+Omd7QPLYAFf6izA(Cd2% zHHR5(j{ZX|hZk`4_IIE?OwB@f^TCOBRR(bRY9QXiDZX(AP1ooweJT}YIQhk-+%fN6 zIVC5RfFVzipDr+s*&b;;g3tNs(PmjPvT$AFN1k0Mtx7o0%Oibb$WC!a^B>$wy}4+G zVJhD}k1l6^0M%Loo9Y|?P_Wg#|@?DYoZ`eP|KtkteYb`T+=L_D6v`KeAEOd#Vf{K7k6b{-k)rJ*EZR1C}}6FcQ-zQr|wwc_XsH@%|x10{9=`j`t3zpOwn%?W+{$g|uqqzmR;>!?!N=@x~ zg-SQW>fZb4i2;If^ZFj*=aXCaUd+-Q*4>E=POgNE6L}N=*E1H2LT2XRef;aGiY6t2 zo)jF<#y58_pF7$eCa22AtUZl_;@r@VcEajXk_c7I&A*w&VViBA*QvkeAh=pEpm!9% zAPVmHRnSkt#q(|*uzlH1CogLJ`ktc^vN;Ady>>wa!eTAu)Umb~@yH<(0#O?vktvqk z>k{UhP%5BYbh)}HhRo#-ak%Us0U5sl3~b6iTHw4c2Hj|W(bDgkv98O00?Qw{d3oFsQ`AI6){vb|20UoXl&-YRT$6ssLu4J zl;P8*2ELq91+DKS$#~BFP9vxv*M@7LiH|LM{!lf&Nqbva9MN~V>pWxwx6U7e#!AUJ zqBy-Dz9T)rFO?QTHD!#%g|O~~f)JHU{A9L*I~lQ3U$LFk{1vogx0=IWVI0*Kx>Rs8r^!SiQ9ts4*pjk{nyYIG2|ygQ#kb3@KxzL7Bx*X zh>>jeaeo=yoqBsh%c$(Ue>i_IDwgtLxj$>2)dlc=cA?)zAe#ahbC0FEH#z(U$O1L@X zvmHZ>RI6tv&Aod#Fo<>k+04Xd@D6L6VY-r})}C2$lzs6$@|gmh1@kz&yZn#A<%$29 z3~TVYzox@j6fIalS^pMLD9MAm`;eQB9&j0p{tpDkPi9Exu(Hjvn*PI5p5@4bB8Bp}QYGO5aR(uR7ST zxIoJV$Yw?+qjKeNp)*1Zt2|AUI$8Vr{G!ow*t%I==VeITuilxOuQQLU&vmVeYyD`R zGuW$*U!`>Se3%ZN^d%(j&dS9X+AjKg_AUaF{1}z%^~QsAeTP8VlugB-);6O7?5995 zxe<58%EHC}00igP>V|G{;ux%**%oWbuv*1&0TFK?WbJ4E6jU4VUoWb#PQE zo9x#aTNPyH?N0}fYDSU0C{CWI*1V=>o)UI&J$vRqZ)?d2I&$0@hHm2;%4~qp3&_rf z$g*F^qkuu^eq&m$1P(w?D}zA~3XY#Gz8Ln(yPZF`yDkn4xuogM)W(158Ws+vB(n)@ z^_py?B)l%BHJs9M@$USksoCoh8x=FU%ymKy|2wjPmko$z4WPLfaI5S{xj#aQeG2yH zEq^RPH_#hYi9jm3;U<)Mm1w6RQTj=Nx4RvI;a*`SOR4scP@pld@mRI~Z8O08lH^JG z#K7kxk`-3L?3=+Wch(%)i@LRh2M&N!gd^Jy6SDNiGMJ}7ycp~IRC|mMyQ#3GBtON! zTrx$R`J@~sd;jhbEVDf#ysw}$EcA{SGDmrAF--k?@qO9LtmE?^3?epbRjPwecypIS&?uloe)vid<|AGG@_YA*ncwGhk z?5CN9*9$6|lnwRs9DDB$`C0Q*HMv}M2rGRRmRw$dWCD*}U({tdsKHGBwK+N8Q6<07 z8LB(Tk%5W=8F`;k6AHLYYe44}(Y+pzEFiT&5~=&(-h}?AF(ldV(O;)|Iu9b%X|dU` z5&%WrOg>tIB&67pISj-gC$941TpFz2Un#3oVNOmaPjQ@$azwUwWcjiw3)Y3;> zr)Tr*9@5htFL10EBRxPcdUm)&41FB(H9q&#pEr~|n(Ky@*IH2Q!Ql~NOh9IlecdhF-BiU5D+nI2JVOl!Z6MKuqkgqF4+r|%pUPtNMnYq!VUT4R z^r6JwtZn|o@{_si#+@cD7`_n_h)!T#rwhULYU2l?DW7W}9;isL^*R+3$?`*FF^M*t zR~PrNEXHTYrl_YW2xV#mwfypR z@{*T_D<;IX?4kL)$GjsXJ3O!eJpA7Kpt8q8(%e3D6RNYQT_9xY3Ml+zzxoc6s%NG0hZLI%4e+YeRy)zFUTiD0c z=_-2%4q-vpE$A^8M<#@sXW7v}xS!^UgQ=Th$_`cIMId%%31Vk3Datd|%-VilZ*FdL z@|B>`qTVe0C3=tNHZ))bvrA-M zTU#Dl&HUurr+IFX|9#Z0h~)DJg(|{y2U(q7Iv70VD}Tq@tmI(OXwKUNR{vFVShH{7 z-zl7xzN4qHwe=;diDSfeR?W!z|6Leem+nl)zM-T|(l~VfYWR_0{vkpDAL6DfalhF1 zaT{Om8p=qWv}iYZTsaaoOEaHm3ob%{Fx4Dy9{SYUb#_+#rl!UT?c zjauL0Yk3!ps!1W7va^Q+VzIf?-lWXe<3o5t>F=|Eugr;}zHPvUd+lIP=`k71t>q|} zF^B*CD@#(ramg3eS~R>CaUj0&E=*CTE&x`+8?L&V{DLYl(pompyn`CI`$?&rUvd0> z{~>9qDY+*FYUwXKHU{caD*7P!WBY0<82ho}z*m>AC?okXnK$i*HGvK4D(Iyad(%!Q zb@kWqXU66*WF!d2(ZY{=GJ!}@D*#4uyvL=Lp*hzOXaic~#fmwbhtdhMPJQ=Gh~ZRQ zbNS;(2)70um_$Is(Hr{3?0?RHo$mhcqbJO_0bTr4rOV0y=&oc9EbI4|yBUEHRT4Cg zeZ_FgF~500=%)qnrh;A)u-d0z$TzAK0^M%0zcx0`zmr!~zc>iZPzbZ?ulc=l2lR?5 z@j7|hktq0~zUJW<%sXIiqWmyV=STG)Zk_xzfW7sD-X(PFPnSCSfkD1kA&>DlkuQUw zx6kqZ2JoSd+P}LRMo_e7$&3W!=wevTy-7eePuB^g;obnWef?&yRQfM*@0!8iUB&J3GT(9R9lZ zxc7Y)ouIiU7$6&9txbK)aT2w_Ru1VP(-4ft*XGsERHCNyNN3ywo*rBeb04T$|GX82 z)N2Gf@60bIwYyBBwnM=MnD5HK^O-cNnoF-REiz}Z{mmZ3@cE?xqfqMW?hYb$!{Hfs zBRZf+`}g85Ei1n*e;I(=R4-H2?Okh;s3kK$vRQegbez` z1imcbo3vxN5fz05^3&!}1<>;BNM9NFLvmhZ_alItu0!_dXR- z3~qXTB?Vnm@?_>cpGi$ukX3bvMm!I_apluMr#=NaY8lYkTPzt`;W%BLM9HbD1XRf@ z1SK;%RnHw>zrfcI@IM5#`c*I|OdJFgVMxH}5I!9&FupC}G>dmc$c_fa2&0q0h(pX^ zd8squL7omTr;d{}EC2QuPzv>dUr4maw?SM}XDXIoY(D>elvDyBJIcs=wOCZP{#VWg zOXXeJ6@rVxS28Vx)E*QFkDXs*!m2cFZO@hGX&fPb55_@Wuck;%T-3EQ)EPH z@~w)3ao2t5-OGb?)ZBi)goxvvG5@QMT{1aFlf1LWMntg|~qYRC=ld}{&WRFk); zUg7W8ZI&$l=hX6OhgaXbD&-}{i~tj_PD05Ji>j`h*ofyf+%D(>}RN81*H2p;_T5o<`*O^W+n~v%@E1aa zY?g9d1~~7CJXbp~Nc*3D-XDTpi5Mj+`$G}O7jsgQ)B3Nphrin?e2*#k38D_;VOeVh zbQ_$^YO}NR>!Nc88%N#?ga3vHx1>F=jXW8UZBd`4j<}c6NLZNSu@u8J&w_&ptLXNy zID0g*Fo@vRvzwQ%-f2rFMBPnOoSmIbYu?=*31ZpJ01k4o+`4>g{U3;#^(f|i+weuM zc;Ks1$bUqs;S;;e1`s@rVZSHV{Sl3YcuAH>I*|`t+GlKoBPfFXP7XI1?d+?4&Xdo1 zjVhvEUzO~z5$)5(qlcUmiOBn!N)2S*Ys?WtN%tYX6eP)z#NOxgKO|MvZ&{%kveF{4FmJ@8~5Lr&=cZHd=(O$PH*gb5ws8;dR>Zi62KLw;5Xf>n-f{Le7&Gv8|& zcMz+7o)p;h8MKuzS6^~tM0SG93N2O9_#9WB4WocI3AqG zPRSbob}0Aa$cw3D4U;>)nhgzmvW}`eFbJ*u?AMj+yLW!Pi_sA`tH%V}$$G!TL5dBORm~wg-_`f2!eBQ@e_+T%xx~`sDi}aKFyWRe3Umd@K+<@2B=+fxo zN1achB&<3>mo1DSs%vU*4+$dtPAvcRC+oFm7B^o3zyd!dSQ+hlSwx-!r9iky$cm4k z@q3j`iqPs?LJtw0`YH=rqoh$hA=U^LYs{=KIfWYR@tSU)%uFbfzlR1B#Rq`UE9x#X z&!P&_XOzP5?`7Jxz@DEP-_L&PcMuj#Z!GtWqKzdAU3Y)vjP$v=n#K7khlyqcvE8V& z+8f{Dl3%XlJ`0ksOCNm>(_1yLTi2^5fpFfPee+v}Lq}RUf+03zn2fAC1pYTCyS)q93n5^UJ&gWlcF6<3UyX?&{j;zC&y&3s z65?&p$HE1QEjUrc%#z)Nva@)F$gx5>Z$JB47pLl!UGiX01!L-kB1SyQ$P@Jwxh%Ix z*u?NjHL2kREciKE&a&~7Myg(0|J2mG&TiR1kW;$TZoOT|Be{O&QGZ8;BJ*ak8w8nD zm&Xu?`44g^foEs3%CH}*4$EZ2FP8nG;ZlZFRGI%t8Po&oY!M;j4;jCrREa0rw~x0< zP)ta{&6SmF7{;L-JG;+m9ckGXg$-dahF|u;ibFY3lc;>smwKJ3eiVCdm_3%fc9Afc zY9GI!k&lnh$N{RwlgJpW9@hp}@%E3{`|af7$T2LgNI@qaQ6*DA3C*X*-)fy`>vWJ3 zPdH&FvCA?hdovZYGAbaxAu4E%+mS6>TU!GQQ#4$U-c9_^eNnd93@+Rgv=xuX&d!3O z6w`ucWG)9HtZCxuyEIGmBB2PMlXcS^bu{c-E>g^C0cD*-vV%^{I|~8~UZhZ(0d?hRZhc zL;6B-DOJ~`+m_mPgma z$4v6!gbpwE4h}e;N)Nn^(XCC3klSH}B~oDB%4r9#^-{%VZ+i*9pct%IsAKZ z>t2?rGxDtRc)_hhi2cKY%X{+XRvv47s}+Q2JwHheP9bt^4y31m!BZ|k=S{Ii<{Uhw z1azpMREXJ)OZ$6A-C~b8g(FHRZOX#X)`pQX??TcFOzwO4o*xPqo^xa`6bG(LzAk6` zEjGrbb1GUUz(g-cr}*FMNNB3(13JY9iJ&J1-=`!LxxW{4fZ|S{spv}|Z@Ht9kLg1Z z7@L1d;!THBMC|_lzDQ@Gj2`+(X5<-r3%Jhpj`_2RS-w8g4kv~9`Hms#FvL@2E+xK) z$l5qe*WApwJqV!`LtJ~b&EHk#f3_Q7mO-__s_)ziZkQOmKWR_k=k;aM)CZcsDn5Mn zfrh;7Z_?bg{AG*z=ce*w<-x^w|7KWk@M9z?-EyuUHYOg#GJAyAVV8o100MgtRM2Qs za22#*yh90?Px1%Cl3G4$6s#Dh!I zHQp-1R$XZToF82KkyqJ42=-YxTZh)#IJm7VBw#l*L_mn|opBmakbZ}w!#d-w6_65; zso%Qs(0=OMkCo&Sn?a#fNUb%<48d_H%8{_0i@*TaxkFskFbYpS4|P!4GHGX#2Z zU#RLncU>LGvIi+tg#piZ)Ct;Vh)2H|q3l6aiq9PG0NioHX_^eR#)qiMvJA8`9tLGy zw>Joj0KczQ?Y;w}d8%pa25@7e7j6L5t@##QoXbTOrpyOy$d)OyL5tUAyR@7B2W23g zRF37MP?VazjTOrNrYvX>aVvXfFKwSY?VN+_x&7SumnVF0s_gEA5F>;4l41)A22yZQ zCIxD4V{Vy7QVM~UX~)3EIKD9iZaf8(YHQw1m=i0HoaNC@Hw6*lcF#-DD}5ix;I&Jv z`@N!RCEWc(>j9*DEa3KQRFD>*P#m9?AT#*Fa2S{`^}oJyyTa>QaClde@E{oK9K9lQ zuV8M?G`x3`p7H3r&>=L&*K)Nloq$T9f;eaiBm}QRUNEPBjM1);FO%x|a*_DTRfrY&Tee%~(MBk(l+VgG}=*=*$y)+szS| z88PR|n9rI<-0A~fM)OLSOXi!yT$%`j$x49tx?rXA&b$Jy`0yk5VHn79D1NH0zd?Qr z+o5HG5^0-(l|+H^=+Aw~4J3KA5zNxlts{UJrf&l6X9UT`k{rk7PhcqL8$aGG1<2m6 zq33My&isZCfVD|vL&U@g2Ep&`GM)pMDj^^dko^S6;ka_gRXgzaa$f$qcmTo^T8HL2 z`sZfM6jx9$og|lO7>1OuyJA*eEVwXI*(;tGzEc|cL)7QBtGg%q|@<%|SR)M&>HC59>R z8fwfslo;T~=E_?be*mb-^wL|O~?9P;~zj>C8>g9WRB>&rJ8|g&&4!5c4jqW>F z^hUAx%gFot*MqmtR*GiL{LT0zEt=M^QJD1xxaSn^#jwvcie7H8Mi4gRYw8|AgBpAX z6rNo7&k+3CFi8_iLRHPQ3!4R6j znQTF-J4rYQQ#=qlTA|(FiaGm9%gMun!@==W>>YKrY-n%cF+YeX7Pgr51@;Ar#Db z1n)VTOuQg0tEIDOj4z-lQa4OiqlY8hi}QGz+XvocaPaZ^>ajqU$0t5JWe;#L%7L6M zf^&aZ)>C#E@lxq5iR}qvfD(%+s}qqU*U4Kp2|T4wH9BsrrDIdMlbSgJwrF znvNAeGOGEJxC}Dh--w%q-oKx73Z4yCXKSWIm__=XV6F93b7ypRz=ku&q}FF~cGgrC z$@9D$BqbG*%+fxKazbrzYY#L^Ys5`n3%FPMzkU1G8l1PP@+bv-V*BdVqZCny5EM$e zS^IQ}w6db2kcj9RqZ5Kq+;6zX+fzLUUmeA-ldmTOG)r^6OjoWX&H75Ufvr|)-W^2F z%=7Av&TAP`te5-g{`GY;Q@+BF6d{>2QLKS5j!dpqsz(L~gVNF;Pv09py_8;GUOfvV z`^xTpi4+o9M#I^9$AU}Q&rhaOt>6a?RGV)?3?>tqKu)H5Ip+`2lk44KB4m=+-x@!N zxD(3gf`d<1+u+sMR>sFa4Z+j12i8P=;LDUUsqylvYr)b#`AT5N5>dTv#~e)n+8vmm zLw=g-x%k&Wwq+$QGqk~@$WJ+3cMh2sWx-u3{t%zTIYq>#VG+Tqb{2zFV#FR2CaV(= ztO}v{!Lc&uGJ4mN`0{G-fUkEKlJ$1qv-vj?&i6qKM{cK5DcCv*)=PYQ1~~e57)JPI zF1=KW^qYpaab_l>=35y+sbo2okYZg#x@BI*6MEIYLq=VJR(rAHr8Z@jJ`z$~ZaCtl z^dwvnER2OB(*C&egqDvF4z6k+D*@5AqGQ=*Q0~74QcC*3O~{RCbT;B4L)7{gyWlSL z3xeF6zLRw_?jYg9@bMuJI)4BJ_D))KN_i* z_pL1qgt*QI5Qgm>kPdumnbnL4rb5u8qS2QL*++BuXG83wZ(#SCMVP+HXrstL;#I1p7p3M|j$%;3(9o-Hit3 zo3xoOKSlV_>S+d~`x0m5@-Bo}2F(WDEI18{c_^;%y#E3*F^~@jtN*i|ROYP~ zT$O)EFYsr_ZEi}fP=*99*;wf(sH{Lo+=Afsz<&xkx#&~udzgzJ!t;6M|JDa4IY6O_ z4-GOohmqBClY(jE-R5K@mJhy`QB2qd7h$S%HVrrooFS6Yh@u8nwFN1bCgvdZ(I&>d zY^~4I=Wq(vpZo#LG8mLGel#yCB+U^2lGbhXQbZ7h zbghZ&g@fnJ5_F(A`ltc2hqYytFplwUgU^!ALu@$BvREWZY2?82ToATInAPE}YkgAm zYfAz?nZS5ZIjKM~Ma_J)GI{;9GUv-qJ>~z3h$+EjpbI6_Z zf0|;%4b!^`afGwo|MqcOevZw+Rl3o3#qtSxo-p%iEAL|9T^IK!YNX2Ab0`2RseGD*!PhKcx1>KIp$qJUh$Dop0 z^NhJ`c31y6MUO72t|Q;vGI3ux4l&pcl)GXiNM?C3k?ow6yY zXS)Hl5ziK7?FQrYy{G#Bu=SQS^!l1FNxVrJmvrel}1El6@&BK8hWjOzs-n zOuT)Sija3xhd-Py{a?fm%H%Uug$v6NZbrpAK|qF(UVoLj84Yl(Dev<*0FG5WWdWBu zUHUKY%<>7o-$z#(;1L%t4zlO-e?}x!P05imxKw!HjaIbjQ6zPZvD7t8dbJWV*N;&v zRJj%2kXT)w7W=I^d)zdm75Idy$eQXUm; zyQ#kN5%^62q0GK>i?bXYNQqeB^V5Go8vB*gY{o?VGzQLQzC`_WD-XJL!+a)^Ht+3* zlq~H=ZjhOkwUY?FnI2Z%Gx#3~tw|ZnzOJl|{qR5fOE_S(DYI35@*Dbsjnu0sS! z5BQ`N4HW(paBQ1l0`nOQ1c>kw>!I|}KIZx_$Gg++ndC0Tn1Pg;`)cyQR} zn&%3El*ZeL^ zavFO7_gy7>&R3pYTDqZ^lzWWMm(;Q)6!OiNCZv2z9av-%hoU?Cj@*E-cE z-Fqd~iqTg6Vzc@GJgNr|r4!)vVY#q3 zxKV4+r0W?gx;;t~9$)Xs(Uv5$C|E#7`RL^-)^=|e&1}NDLRXRG8Nv_nQ79Gbx z#`xaa4$gIiFABoH&2{^_T@4HPuHA>ceHG7_@~Lz0<|n4M*ymLjm%py0E{_EAP_wOU zM>G;mm{U+nQWy@eQF56ec@38WM--n$axa*|gd0BpZT$2(wXznXiD7iUUGe1gppg}~ zmUzd6gQ>Z~{7g3UH#uBHca(v~hAJd7_-%jfbb#g22Us4>Gb+P=w?cgSz1Ao!=jEiB zkuJ*Y@=Ca@r2khRr4-jw8ACmIux($`Tql)w%t}-aUOa#gV;CTe)U^WWWG=qI&blxw zi|d#WhOGDg(V1-$A2WqCUuwlzSpez|f%j&Lim=$maX6LMw;#~&0m`la2oydd1f7uQ zLmQyVfV=@WLwkXSJzgp6j%CdVna{w z%2T;aD<=~RR>`B3encaMd&9fg@7G#K zv96~?{t=UCObPqtl*ZC#;#hSr5%b7xU?Qq5hFH1Dl)L@ZLyyvYYM>q$E^RTJ)EZP#J;K^(Y(+3>MKa+le@Pg*C(i)P*JVhgnzd2MV(L~)dFkryOI^NX3-vh z(+Ak5PBY?L(MbXIK5j5$DZH#$42HmOqtJUaSPDhCVZuQV4!orNz2D1z*3p%iqlG6k zBh{b!T897jLFhs!vVowu`*WAdG4g@>S(Z+9iQ*I{vqTT@B6l@Bj4m}%c#D*o_=P0i z9gzOUhQp&mD`7cRKUghUg`$i-VJ10eCiXuXH(D6Gh@An7D}VVKxQ2nX?3MrEWyR>{ zDq61LPLMqT{n&tycrh5m!9_5ApO(EBH}9j6FDTG#qZ>jzUo#+S*bK%5Fr63!+teq! z-S<|kg*r~1*7pUh1R-0;L1?K>Jyg-JXoK|bey^W$LFYQ@L9B)!ACIPgRDHgP)3Kik z^JK{$5{0gIV9r+Rhudnu==4shCz}DzPTu>tT#WXyCE)t*U9#-^c30FNUWF9;zds)P zgO^V|6-QP@$dyKPW=9h3Co`_k#yN*dNoK``3q(RoBOlEvN+rc)jaYP>!%Eo zO6@bJrmB4s%Jmyxb6bkhL$zyO#pQGa0VIDVeAMJTKd~uN{%q>{dl|5m zsFAL<6j(~YAwr>Zc7697coC`(ZF;{9CDwI+RmtTe!c9{Jp0t0mdk<*dq@+z*Xg^tx zm3m-(s}f$)Pb588E?|h)2&0Tu?o$6cn?zXc{is-PCUcE1T<>`xa^`y(u0i`0k>yS_ zbam@tJ5*sfZrlm6YduzP2*_b*zeRvYNFt0x8%P%6d86ML_u2PhsPDVG$3y}aCA?u5 zI3vrGOkw=3k*m~qGmp`8#ZNa}4+Nr`jQeZtM9NvM$yBn@a;y401usYUCoB5y0ZG;F9=hsz&||@sG;tT7NPDkD&dQBiXg? zrvlbD2h)M9qc#Jf?QgZ4!kL&X%-62o<}K{SOb17Wl>2qpyMvz;6tF~V65g1QfiA(n zY9ufA&Iy@}JD-gbEhl_Wr}bmnhklFUbv|Udt-AHu`GUJt`YngiP(iTUN1)xER4#M6 z1Y#JWGJ$(dNQwj>dvthc{cwfAEw*~SeI&$`%!WKlHlM%IA0^F2SY5)5_7pSkI9XIS z&S)I3#O%JHLM+;B|OLtYy8usmquuQs6pZ%CWfNm2U zNMUk^B6hwp?WRM=0Z0gyPa80s9>lY4LR$%zG_y)i7!Cf&S)Nf?N^-#qp20`_AP^1* zU}ujC$23k~wYjIK#)fIyeQa#a z)Dyo-D-A^aK{cqO&;D!;_H3ng`jY2~T!y1h%E4`W)6Q?kcw(YTh)hSTOJR8KbIlH| zLF@8p;yqDsy45zne1{MzMvbC0;_rxba5Bp8}g> zoKW|+1{Ky`!Z-3y!pJ6IOsp6dKufW~*N$P9dU@$MoEUV8i6|>LmfVkOD=`p%3jSG&vMfR35 zDptu0mAO3V$)B~E)=8o)VXSbY!uQ~ats&F@D%$P##8JYi^_2n~gy5A4x%s32eWYY> z`=IHa)QBbCnn6aEt3mXMg9Bs+&#G5V zY9)Q7p!e^%!6>wYpd{_e5dWYRy+zOLA4PcWSZPdYOiH627)2%1;p*a-nT6b#5B1uP zQ*qzt>MW63!sPIa*ADe~LIz_b54N4r;@6&^QN{1iF9hY@6rt;^mdnH#xnHi^D&!Qw z%NBk3@VQiCR)3-q{Zkv8|6mL^W3p>1GV!rTg?jvJ5wHs1$=iY(rVRuPbuM{V9mx1~ z!ZZs{Ei>;?DQ@&{v9C`qNAA)!1vaNuENnj?_0i5k)Peyvv(Ox3Uy=yi3BA6A}X$^_eQQ;3CS2{ni83W~Q{w)aRrhUFu{dJEWRY!(Jp4 z@()l~BfL>>X>dPUC%&zq641XUmfx8*6$Nox5uhHGPszJ)O245s(bGZIqe4R53{#yz*M zEdF9<+5Cds&(3lBSEPk` zjY>W?HQ=nbgh*z+e~?w7iYpE0;E;wL75256S011fAOx!snt6vradd2Ay)+~G>8yu(Dk`YI8msRT@-&Jgu?41R%%p_qlva*K zif@mQ)t*#FeNo`a82qW()Gj4;f-JtqBShmZ4A_uUupPf%lX6M>5ehrFj)xyv!lOO) zN*1*=)JawG2H73H*s9pI`yYs4sMAPQP=Rl<0uhBg0Iq;-;`)j1UcS+%dVs8qbW7mO z>?--PN*zKi8QPhMA&$lVk5FErh8qsZ79XEUaZmz_797}2@%yeUy4PAlRHXY4^)3-l zQuD>g{_UytJ4)9PE|rpHyt~uT6KsDnv#~&ZvRVjM}vZ*0A@T{M>f# zK!)lR^)H=NXoR<+7N=z^a7)s*;S$qr@Qs3JQHhd!A(=(^m_+Ds8w-iG(VVKwbJ$H6 zEGDd43a-o-D?>p3(*tOOjG9jP!iz*KD>>SkCntMn-!(k;FDmj`*G>)TzO`VGu1`?1 z*!S8*iKO#jy2)bjJ2Oj)o)QZquy&?sF3M4)x79D>jPZLeV`RVVl^XAOn}9>_TI|gv zNSmopDUO7jD0sun_ns4Jk2+F!GKp~F;hD{^RxRl96PChg*|Bh)C4pV~vtRQHZEmAE zlT~2K%`gw&onpd5Ot_d7!g>7j1-Cz1&FY3_uO`VDYRw!d%ZuL2UfMCKxAMH-I&^K%ogf#NcKv>E4(iFjq)n@fI9YZFb!=%| z>S0c2=>sSKq5XZ_)rw=wEMw$Y!@|Fr(+IzYyk-6nZd~&R>Cf*G-m!{{W#Gsk9}fBf zH!@Jhv%%-DU^+9&AHE=CbZikVoSpKecD7T;Q6MOTnLzpHtUK$z=Cfn0UTBL^ z43bDG^uF0p(k|c(KZrV?Dfs$qf3mbtP)!zv6RfUT;-1s~Auh{{T%5d9R3o7UqoP$o z@#bv3&-kIYu_ypguUNf?PJN@e{j=8?jWyV_gom3NNY^Ls^CH_GfP;*pTs%qH1&h@9fn2VFyE%2{) zlB#<}#pW)G3`Vph?p4%$d?#(?_2#l23G6>`9a50EHh`YG0Pli0*DY1J;)|JDAEBD3 zYNdrYTH?nHAbF=5pb=CcrBPrJWBs{$d?~C`L6h{$yf8FuJ2ad%yDQ=3gq8lJ%txSE zCX2!Z1+E&1U*yYS%>-QhPLl7oL5zp*z!{#pEGRo@krfdTPJ$O5jNY_(@odQF{cH~c{VrDDhe zc8zSc1N*{(7b(p=hMRy>ZLV1xrqjpoeibu+FmHmq=QE?{x7Yp12GeAWODgPa6sL{P6lt}c)r^oHAWhSO)YIW zP4qG>-6%&Z4acll%`7)yh(iGv@m^9)@l3*U9TQ&eU4)5Mbn)0{iZ5K8My!xC)S(&M z#eXmw{3Wb9elXkvq7<$Ds!u2@^dj=Df*z3|dSo)Jw%MJxnWJZWF{}8E2UEpq>#G29 z6LETZClA08KG~X5M_^!6sYyJZZ84YWUl4PEP_GY7&)}P(>P5uWVrkn-QUxQpcqxi= zHsha3yk3u}<39-T?Hp)3{PS4K3c2Kk?U94)fE+XksgiTv$e6UY1ENGpQR)m%6C5f%9-7r5IK&ApD8gS3tIT6Lkpl70iNlP}iQ4HZ zRii&TY&UBnM~wd48U~em^JiMt5=FB?E23w;-TDgvZDjJ+avLYQ9Kt+Rs-ysPN>`L@ z7qh`hgnTkljQCo8=#kH`a4FcL=QRQFdgBe&_GnO{^hOX=T00JgEL?$3Crm>bK`ZT!13=j}H1Lv6?DUb!VdzZ3^0r zC=H!4&3gGnAb%K2;31?YOsX9aLDD0?wfX*fxiydzUnIEAx+Zs??gTkt!jSI1i30+d zj~JWfcB>&+!if8TH& zOQyxh476pp!06V$;ZvuK_;&n)5>SltqkR>5B=4em=LBeN0XP_gb`Jb?E`sS7Iweq$ zg<^@3v>OhflVr*fg#+XQ1Ni69n`lnFN=a%^XsBYGtAT_0$medTY%r*6uKeJ6eD3HZari)t&^0+p z70FTSoEMN5a*^XThx4ed2WL#RXq-H6PtWONJXqK2Vg~V^09M*4Fmll7TNtFgT~Yya z$_@Y`hrK|5RfH5KxG3xF2@EgpKD0BKe#zwJC=Gmm`gn{+JJrhe#|WO!h6 z3gHKW(9g@mc6SJ*40#CTc|Wc);tf;rkoM9#2q!yA9q~y#ZVoKUEByX6Td}+rKZ-~s z6qi=2!Bj-iP3?R((Rv~bsgb0cx?y&_IJ)y1`bdX_aWt8wP%%Vo4Qz_K0}TVvceA|( z;U|8}jc|4sI~pq4?OPmXVUKE%yTzE5xsT}-TawEV$8DQyX`~VtWcG{9jG0$=0fA`R z73?_Z91zoFVNiivhC}f$LnG8yft{C}&?Li8NXLP{~k2+^HlcU8-kp5Pi3yP{R-~ zuPME3^v`M^j3Z%Z*bLK_iy;?Z4?xk)>14k)f050fpcqS_mh@6J4aLHr78tMuvN{VE!gB<90#6fCcbpiucT6|9Lp)YX8Jrx+W^(21-Ur`n9x@C|xzZT{>fr zz{vxKEA-4D-nu~|&GR`8ezD|Qju$~_QE3@x{^ion(W@d3YU}mu%5TmyyV|JELWgnG z8o0~lad&DcgrxbK91K$EMvS&`!9ng@gXh{BZO(mc&%VFdEq)}6H#WNg!aL@p|DV3x zrHj}7C3@H7!bMDeG^Of(Lce3gQ(2{HuD%&Y?522gXn+0ym&MgWE>M19j)iCZI_Ec~ zDAwT5YDi#Z>#p>2>hFM3cgDT@l<0X2f9Vp;uW<&%;{)Vr8HyMS?q_7iwpcU$K8+P}x4_ z<*~;;XsX!bJ}?;(H{!0+3!=j~k`azjEtHQA9oyAw5$U7b_|mT=+b-U6 zCEr&08`^wG2#W?{l==^*Qhy4Uy8(K^TAav;uA=N6uKxm6Wq$Jx%LzO?bT5u=6FbFd zB3OYhkjw)cV_MzO$CTEdgOl#0Uxw4{T5)W5T=0p{;*m*(HlY9|N5f-Tv(_`N+^D%h z=`Th%n2Wnls@|X?B%MrJlCo1v-1jvV9mY+7<9W7Xqu#v1t^=P*D6;qK{4eq0Kfr+p z&iF(WHC}Fm*6%OwXuPs$Owxm%J$oVyP&kVt$>sXbJQ- zr_)vdT=cfgYMueW>1SgUpNJuxfZb@c51k3I!~In`8j@B4Z=5YBxZZ5LTdUMspxyqXZHV+NE0a5>eJlp>uVq0E5I?FZ$4wv zFSH1OvbY;pXqnnVs0b%kgpD-jG%hnc8H_41gg+eCOsPX|S2BQ$|Hw#SmEjCtozzj5 z?{`-#08HpB_^8MlnUANYFY#56=7YQL#Nzc7rdwsp9;w(4QwR$Zyb@fAqdsrSckU49AB+Klv<43#-BqTcJIlGR{z;Ikh0(5j#mJloj+GMhTV@p>(p z;hkPbX=GXp(^MbU>rw+20EWO(uD7s*UH*1lwp+g@0j38Y-n6P&Ob>uZg2i-Y){icU zF3wh-(eAc~0`xR`ni{|imgn%scr4+-Ko2z}OdG0>^mZx3ETI}jls8BtWyu}pN+Z;?Yq8wcWu4bK_>Fk?MuA{BkBD?B>{=6GdtS8 zaKj!lK1aI|0PP|M6;S^^xI?DhT@2m5R;^LM5z!baAgNy`OK2ga{BBUBg(pu5qpu?f z5Lv{H+GZGJ_o8PYa7+(HHmgHQx)6&0ue@tJ zTm82*Ft2_2M8So||NRT&B?jw~?I!acjlgD4_8h`IV1|@ypgy>g$b4_wqS&_uwGw3!WrEuqJ zfy9gB{>T~?^cGOQRcinCZ;l=NxG;qStA+=-fZ>lqT*al1)S$(2Ko5G${;jmdX&{er z5Pg&IB`II_%L-%CFAm$iKLPq{L>sxp2B1?*M89-t^FFLo(F2wbSU2d72srJ3L1|9i zUSNP|Wr5;T#)h~X;*1SJ^qX*%{V3T3oFxs3nw7S%jM%Nm!W$V*6EOpsw8FnIlg0<3 z8z|7Yi@k}^-&3bw_NrlSZ{iZf(;Z+QRX%9zWu(W(XE~@~1fbi4kM68%jOn2n#0AmT z6+_`UoeTo-g@5+2or~$dJ^#I#N~om__VpB!HPX`2C*7%8DfUtLgs_D7RJ|Z2>i<~W zxyUuyD<3i_ALX^*8pCARSMrM@SVrd9p6KI;%mQv96~}i{0%@-!0ydE@SYP}!9qNa| z$eiz4uHX97(_t5VU>j!d2o~71oEux=t%C+_3Jk>ddi}PPWl!^52C)|b`J8PZOBkRI zzF}lTufMByqVnun5CM)RbUCPCSIG8xB})e3Zo0{e0#4^uGUb{ z@A46vnwj7ILE0og<>8kUK(5e9mNeYI8-QWzzCq^yy!8Cv*L zmkj94Z6KFIFV!Ub<7=Lrs)lNr6^cL~iT61rCl|25aGa8&8wI}tU6{~*AuE&pNWqGq zbJ(RB%b#|lpv4mO^4!5u$bF0ILB^Ssxh23Pg|R7}@dbA;_Le1tX;G`lg3YqzXPNFO zZz{8$H|tmL-+%OgFFrHxOx!4wi04r(<#0UgZs#IvX{1>Qezt(yLA{kV3LKe8=zJFS z-iB1nh+@ZSI`~wJ#`xuu*Jjff4LzV?D9n`#XZj1_%AolAxn*j~Lj>{55O3)0HhFd( zf2_Te|B>L8ta-X6-mQ0xk^&@IaRUdbff$3-nk}bBe`KIKXYMpxiADMMdr+bu|N1YFm-jZA0`x7B@byMr|nc6o9`rsZkX@edibTh)T0L)Dy&>k1qBwss?RJ6 z(#wY!!AP3%dSa#GHgB$sYPT?$;O z0IsF@>)F}H)x&oY=qOy;5YAaI%bFg?Kif<3`w8X#cDr94(6B74Lm-L_zEa4QU$!T& zAb=0bn08KY!Sjxf@dJ+K zo8c$pE{g;W>_-LayOy1CfK?xwd;!p3B&{c!8YSAK{UV%X089(zIT3f1|aGzweFyb zpMY)s8lePGO2@E4*ysLjN&qP8We_GN-{kpMWd{G_NjxD*Bm@<%zLKI=GrHNihR>9K9DH8u(0UtRvXy3|sCtYYJI~Qv&Hu z3-ZYGqd&D04lNA$Ari7i9|G=`itfC$Cb10}Q<$Q-%ik$#vyB;k=AaxxfTVQrN@CMw zdwy~S)24!Yh2E$`+eOZ)a(}Yjk2q^AdSzWxbGxU6V7M<6k)EDP zfh!%?iOrk_d?TO^Vks~)Z{a&xKDGO{`U?9H6@v{BA$U?#+Ena8w+w75PYUSz|2Ye; z#`eI-puZK7)OvrE%EjV2jjX9ueD0(f7Lf3O23lL)EZ33XAcX&=ABwfIk=8#N6=M5> zn#v6C@@Jf$COam}?3ZyTKYTQy@Uy#H%@1%{-xf{R7Z#gr=j7%l)tST!{-Xtm!RHdx z(d3Xux0w|OG*BGqW?!|jQFu1{8!m+y0Uk>be2iwNU9co42tq;(mChC^+8wBfF~F&S zKnH?Q&=U!3=*KieULussW`}FF=!)3RIq+u$MOX8d`{!o43`GDAk%*3Q9gv{h{^k=q z>XhVcgv=hjS3<_~L39*9iCEl{(gsTnykEO4Fw?v0|{8)3lNqx$(4{5@l>hjVQynnNm2Z5de;MrqyfgX^Mtf; z|Bd$~FTP9SzmgFP7NdxLEmma!*4hm5jVNul5z8Q`>e@Dz?i@v@S35>i9yp&Rf$Fy~ zyvgN_6}%uI|QU3&Ht*UNU=2QRnFH{?(&f88n)r@Pztfmi2-`J)iC z=y?%j-L%9mLLq?7=<_z3SL5t(AHTxLv{UvO5Lf(ONXuVi6A& z>4yL6X<}#p=qU<#`1HpH=;^2ug0cUvisv_&$fjKML;ZZ`opZ;BBZ4iBG?ZU`(c)oe z=646QrS%8;|1_4Kyqkh{FpH91cG9z3>dmlArF8q-IWEE}+`p~W3(DW;2X;WM{IPdg z8mIz)Qb|oBR()TNQSUVP%4pk5mGuXgz9mS^c`%07u65enBXF(JRWJqbe>5H;n_=+_ z6VgV-th%{4-x3s;)0Im3j84j>jy$;~OJu0^0T8{nw)`HTcCBZc!rV=c?7gqQWH+Qk zRjs^OA1ZsvsnU`p;x6`O_b2itOMKQuo29YwoXQKL!^N~*b);W)>hNk*h!l$6xLIzC zP;LFo-nLS_!>_^+&^ie6dJ^|!1j14;s;DCGcI#D@<@iZRcxc%jzm#;+? z0VHi}Kir?XF3zw7aInOr8V~|Af$=wyra1GuKlFR#e|2p{A_YW*&AQaUjS|!V7;<6Y z5I2hNhq}DVWSRj!K+EuS=TK+Uc|Sf>-9MS8TNZ(H5Nl~WZ8jc`jlv36ZPqf1 zsv_}(7riGwsn5C!0XTmEowRK@gJFP|u0k%HneRlc^7(8mKmk()eiTD|4xtBz{+ygB zb$S1ieyLvM?2bHD9mBZV#wC>Qyw$VOygbD$Ex7(84Ei+Dy8OwTZW(;Fl9IexBW2_m z!O-#b>9Y=2cf`P>8h%$$xLlI$s1GMNiAG0SU)To^U+qC$vQO4MqMX~*dH&fTkm{aF zIHk`|h2t|7r$@gY@C$zJeq0f4&z!=qR(7f(FJMqVyvdsakd9{8e&Bxnc>;Yg>WUJI zcGm0qJJL!MCQ_`x#`pP)U!cH$qgr?A6!rsqqeNuXb}=+6=m~gU zaptPcUWplT@3JQXW2U!;Bb0N#L+$}x+igd{2{iLF6VjaHY6MJCqr9)RaZsQhQH}}p zfCI<*omB~hu;Df&6!--(gld~eh!07Y!RdcncxNf&`-Aa|03R-!^YsgDa`I}QxZ)}z z4RkQ2v5k1o3s^t@T&=&@6evg%XaA8IP?*Rge(robTtPFLqFwR_{Ow~W^T|oIwps4G zG~FGAlx2dDmN*~=(WQ)L08OO36uSPxjQZZz#?-rn`QT?mg>@+5}flB+!>V z(6W_2EyoPuv-R+*86K}}WzV#zW9(N;Z6z=V!#UJX&5CJ77@uuKDZm9`lHRT!i)dAl zB9wRIe6KZZPpy?#eAh;Id9aW=_?%QiW4l&C2-J_8NQfbXVb+V}FgAse^T^?J|LpMz zFd^;&(kKyiJi#IW{4Q%#Kt|{wvR=L>v)}}bX{e3qCFnlsDVn{1?iU|qRT(w(H{TNyhh zfuf5K4oYN6W!2|%vJZ6Dg>+({Aq(+VN&<{3r#H0cxzh?blE^q!vVU8@QbT_1yixPU zpMhf*P$j^llTmjdY>nn6ougwvhKD!Qnsiczh8jqPP8Q}s{ol1ZhysGG3<6c6{jY)~ zt6xkEB1PWl^i~jo7WE{nJJ9Cspu7$j%TBmwRIdt#-~4-&>41>}X)YJr`!udhP(mAq6_D%R%%+%62U&REW4*zf)i8jk|%g4XFX&_j$9xNV-Uz`Aavwb4ZIfoy;aG z72DD^iDbS=fN7zsSDL?3o44XzgIHJ+;PJhuW;6M&77z%^vnR6A>VE02R8pNi;Lw%nfoBnMa(vf)Cn(_>tCs^0(?nkHb8+fo8msz_`@S^-z zn|XC1us9;UM~=&uRGf3Lsw387E9;|PK96ZAIq+mRJstU)5?vvAM|9bHc)EB*3J%Ip z{tHIX^SU)B-Odh84Jm)x`&BkRQ>v~^FfMgaM8yWssFDnlOR#Fo`P&&;i7|fust{ejFD4D3RS(phZw~l7{qYu^mCnGYqh{I|1Y6Lm3sz z#UT1h$j&>1yOnc%ozst^E{WY+N`>QaUo^Bx_NEI_<8s?Wr}<4t7@2$wR9&>>POjo^ z+DYD&Zz3775btAh?_nLmVI))$O|vg1E2SCYpHUt9dRY`NQNko2w6TJiSp_c-BlrWV zJJCR&p8gQY8hz~O#D29m2t7=SRx8rSSDg9?m&ndf0s3jvGun+t) z;EQSsjLA!4$Q};zPYjfWUabaxp%K4_%N&>exqXyG>EwZ`eBs4(snWitH`=Qb7PYtl z8w!I7woe#Ob&bjnkL8)db$009P{f2e1~_2yKmRFP7M^_WO@^vazY00Oceq*QnF@u@ z?-3`LN9{--mR)yRYTemgD^T^79p?DZ{XqRvPh8nF2vjC%Xl>=xZ6P+NaM&d%wfMcwFHKAlsEV zO=3VbOZk479;;2)|5qc&GK zG0eVis%9FsjMGS!VIV<_1fnUgt$SUqLzpSG7A9fWPU3y#^;IQAuMb5EX~C>XAey@! zyaSh}#WXnJAT?Np$3*Dz7#gHiqJ&K>0v3r=3&2u|7=Mvg2Xw}eH5(V!zrY7fsQxy- zxe%C-<)Swm$?-_IWnI<&Kht-<{h$ISx{3f75@ie=?C$~4MaBk71L5(ba0;F3nZxD` zlKbAR109=^-jDL-33*?&6M!hV6W?9#6BurP-O?EC&tBS@WDKzEoC<=~Xk;sqzJdB& z65CiHYyk}#Xf=dzyL?45w6@Tan@XbPSXhD(q#w;{T%(p}Z{N!X4sumxIBzoFSBv+x zp0YqmVIQ75;g&P{SNJLq7$4~wl)AM`aWxc0DGU*%l+JuXyI1vbn-B#>BA{c7Rx(V}I0LlwbZqXl z_32fsW8f$(6eKX*DILpm?oBii5H<;gZz4Ksu;ly{9#lM=AhzsM!DCSkl{t}|YJq4W zc6jIC>k06M9Viy<9mQn=x|G?9Vs%6OdX*xR#`-F|4dH9)sfM*g5U>rLH?6J)4=T`auoPHNPBN|lgN%b3KpV{Q+o?%oUwQ0N2N zU$NI!t1Sth8S1tIZsjs59Uyq_=+fS~tn@)sQKWL`5rWB0wJzkO-2*aToZ z9n39aKZMX}f#lP%!qCYJu(1zb2!3<@{sgmwY9qk@&IM7Jyn>5KCj6SZjda1l%a&7s z7bFVBk*Q|TnY-|%-~)mg;rrNs__9ohSh?W%&CFu`n9gG_H}#uK^${6pVbeB~lnnLv3HhpKg>l zDDmt@QgMkHeQfAd&spHG0h%jruSJ>LDh<{GTn@!q-*`MmDp~JWnm-0+HjxN=jsSF51-D56isr(oCtdi)>t;?UrODF^Au}B-4Y#@w--DoKkM(`=%ny{@zR5n!2 zv6@s%{bsA%2!9QSFQQywU$aozxlRMk|4_c~Y^B)(@L_$SumfLP}Tv_>47JjEXbp0o}mB-=qJ5!r(keb5Phmc2%N3aqRF zbS-q2WHuq`NG!^}0X+n;x;|)$WT81smFZKV_@1$seAJQTp@>nAF6B3|wLrk6c1CJ9 z#fbZ!7-nJ(H;G|(IQ!GoqsQy_MvVX%Fb?c0pgyf-ji+F1y-}EOW0Y3bN(DxSn>obt zXZq+mm6cd&$BBt;O792HR;X-EaXa0})acF^+BgtTlpyh(h~zYsTL;xt6aQWSG$#G( z-$oBWg@O*Z_1h%SL-FAk6+^UWTia+KB%aZ(-xv=3l9$?gbF_;;Y z))_gYym(XqLBHqSEdd++D59cUsZcNb63SquH5Ot-42eWe$d^pW2RoZfbgPV7_kS~F z8A?Ej5yE_3DN>_pEA2^<;Nu%-mF3wpV+8B+Xa`HSQ?sY=ldot}vu0LzzS-(p6@L z0}AJVbDO4sKUU-~l0#Rn5OzBsr+(e!pp^n3#pa@7rU}-Te{zIsL{o^vefOE}Z(1pU zHekQD!hNh4);s+~MX@_>2Kw$EEnpDVe^;FkUQ|Q`K#JlRA(9%fiQ^kvuc{(r&E{5Z z_Ht*buwH?a*!>NbTFg1SPQ7JmZCiguyT}RUHM%1l849O-PlJ)9McgB1{jn;5fY8F! z7nnao_aJ+hkW>2p9RZfqFZ!p}q$Ty`cdv#g^T45&QPMDz^wIROjI;>*K(4-6w>cZh=$%3qY`x}rfSK|#|iS? zkv-!wMMh-Plur`xzj4`xbI;hUe>i*m6QwH&RBP_P` zU&#H>`3`-F_+wR1!Ko&;%zSoCqYdS%uxNTpu~L$P{mg7BGEeKXl=$&=aXYgX-WM`D zdYqkdv2nH|I?VBTt#W;IZ%>Fv zBoI_BN%&}wngEumW6tPEF}D$eEXUVIvi{8bz|0cg`dVo$h9nf?pe0SI#%J&DboTdyso&-`RqkTr}L+~#G{dX{>UTm&)#1}MAt;e zL|)u*H$7|qE0U?ayH$*cg_%XEgOZ1pLIMd$8f?6Y>+lyZ{$=(9+p#{hbn{N0tZj3$ zc_P70Qxv@X8l_mx6<4ihLJO?&FeEBfJnB;sV8a`7y1cH=l(yLSSiguB!pxgkjZkg# zCY@qQ1V;@UiI2``F`z)10=0hPwz3a+v_EdZa34MT0nj=F5p!~e z?fWtnscV3*^p&yXW>}?yW>gIdl@`Yg4@#DZaBD~uo}>uI8~MNL0A4(Mlqu-|*g$83 z@KTs1*p7qZf7hyqa**-6wOc0Pr{F+&Ea&ctgsJ8iLNxD8fJwkCFbrgYYIXy_K$`4t zo#Urxsv zn9H=Rh0XGlXuxA}sNV3|Q|pQ7&?XY}rMAsdt|R&KZlgbIu$bH<=7H5KNuxu-55cVE zKHG8RJ-k>yUA8wom=cAUQ-tU7DiB&yfVPKD1_&%%1g7ng;K1s)T(8hoOg5$c zXkWE!*zo$^Ut|_Nd#vp`53GSM{P=Udi?Z+xfSTdp_PXW^0!^%gp1~CpcLZZ)xufu0 zu)dk{!O$qaW&(+$Xh;c8Y5k_Ww;I2rXa2I{EPpI~z2f${aqeBhW*%4LwbMA+j_+cl zYy*qAFqKX`r#}N!W&9&gNA06P%LuxFL-BmwAc|q9%-V8+r3GT;w2ah+v(dq(SGEqK(`>?Da8>c{S_>E$O5A=(=9^tdyYXG z!#wWS5BftU5Jx}M?WZp}qlh`pNH!^el+VeaM6S+!5a=WlqP9m zDE;X3vB)hF@HFd0@OW%*zvgy*eg#)qMU-=S3w-mbqy5(-2$KqO^#8PXp3!iAZKEfV zgot1W5(I;2VWNvfZ=*yPo#+w-(I$G2UL$G{qIY75j2bO^uZc2cCL3pK9Sl6{d@YFv0Y@mqOT*sEhQYMjnJv>`|cPOHv=_pY?OE$uz)T~4#xOjuMjgUibJh0&|=-j*jix(qn6)}V! z*Nt>eG45MurJ**Yyep+y_raNwuwvKL(OvGXuTj;9RF#!(9qaG z%pOs=FWyfXUzq$BwAfRy$Ld2in^`NYe8;BDI=g1nP~~r&pRJ^ll2wwv zb{Z1&=Q*Xvg*Z_9SsBOe`q20`7<)yn^*=7ie3cC&5%7rk))O|iW%Iae;jCd4v9$F5 z_2%|g{f$obtn07Sjz!13BPu%*`eY-ucaId&xx3tLia!h|gY>5>vHgaV!_t^7Hcum{ zjtn~VJZ|YuJ_)0I9#u-<#cL`l&|;^2BCBdi$0UAb3reDmfjYKl9dEaJDW@K+49>z+ z_(F$|mdeFt>It3vO_bU-{~9-FHp~In!x?YZ8vjs zL+lMCC;WHES!lq1I3`pB9L?hL$ouRMO{Il5W&CwFsf(YJF4NhK$?sy=oLn+W?v1yH z$XB`iZu|Z&dZ1z(-brwKbp{@woFORA*jQLG#dvYum8iXcnJX(~_cC=}B$mNT_}zKN zQ`)VVj=Xg0+3P1~b~mWsG{DHbS-b)t>Fvn*T$h-sm^ zOr0CcRBN=3tT757hi_;{myRb>DvsGo_J1OCJU#lRFu>}|_T8PncfAaXykPP9Mjp`K zr|Na)_oj`|G#!1WzH2@hoWHX^V1xI(z!7SeXSxba5G+AcI+txEoqfUF7--L1-}_l{ zT~?>w@y65vO)yz1bUg zrl9?xLbp|ZIe`d4?8t1BY#39A5m#ACmQWa~K2o`T3;z|79LndL>(!-mJ(KkzHpKa8 zqFVi`_D<1SK$#wY1Vx#qZnrfCy>_6_dM0mHoFRDot=JjPX5;;+8$M^DU&*e&!a#E` zrUm!paUcMTj{LI>i`<3uuLtG{`~g+7h`*TZ$4_XP!u^Djp16j`5_1 z`1k4q2dAVs_ZBTPrgWFIn1^k#ka~jXqJi!c(w!lS1l(A1d`F#SVJ-IHq-^bt1&1Lu zLw%$4^#o?s24xOIV+kI5rr}KSoZwoQH)QI$2>I}<7wjB!wGiRgGYtXn*~#d^X{thp z3wPh|5w>2GtD1N38Wp31@aZN;MWFt8Y|V3>?Qf@o+iBgu=V_9Dq=kB={i=uZT&H8_ zmb%y#v;QOf`MK_O^#!R?!_(+Y*{VK0RHsfpJB89sS%L(9Xrg)0XD~~$tc|7+7;^J* zpt6_TYNGpBrg)R0laJ=fLTi^xnrt+?B$yYamG(!4ckAjyt0)%ocpEdbyKo#^^?8i9 z3T#7SL}?FrTaNmEold>WH`NG4Fx({{`pwrSS-LK0&uBLP>UjMp*DqtGbK=#>`>TYS z&C&_=>O~4~nG`?pwO4TJ-UX+yjJhaDw-Q*NR(Bcv!SjoJW#g?+p(5L0FiA3fTyUti z+KG%rgz^q2YBnSqaHO(9(<|CSs(HpM(nC2tBE@jOw}yKxQ?$?OAzu(&fi-S#A9yWTxBj7=fvJ7?_0ftKOV>4vbnayo^x%H= z*0@XYmSe2rV1YW8RZ5LBj0wJz(Yb6s9bqoo%4F0zmXKqTTA^g2CtcstFuPjr^KzDy zm=qROw8<6!?r@;@S#rImofyIR87<~RapKR%l`F*`tjdyF!y6bhViiK+^;(unt;}st zeMG2oCtF7@g%UU`Y&R|2gf)GP3T(OS6X3`W>3V7WKL$uhfzpSEuWUnvMoVA*f@9Fj zAG>1&4s1f~&W>f-?%4=p{B11sRk{b$+>Z2;*L9-!?U`RBA?8bbtpy2MrG4W?JFWqE z5hO#TBN(P{pB4EO8%D^ZgmUokXCHi|hh>wF6YsX9umzkHomlYM&MJnJy$MCkk5_Vd zPc=wi(rBS#Y$LZeQ>+;zevkIF6`7R?_b$FortBw!G3{lgLk!JR;jIbZ{Kaxv)x?bk zG#U>T%iGQHri8YYhMGwPjM#u2KGD*?Pqg(5KODmz0zRf_yC8>rFKysT2Fq$*BxahEy<*T&N6MAz$^mHgbz66P ziGpu@gBGJ=BfRN89-7hUsZf=a{a%diwLeAf;n(?{O_WI=auqbryvw*8eTjiavO~XG zcadnaYUx-!vK@jQbO>+`B;!?#qQDH0suuB7CSs)eZ}$$gUTSdaIBQKSO7~279M=}U z9VF9FR2t-mt>xQspWe8v|2)J{R#?@uFf}a z4WNTs6$1l9>=@%-s&5SEPJFJ^l3t9xl3Fw3niv#+V?BkEJCp^=6^f{Qy(MIL_C=#n zEoHbz%`3~xXY*!DB>zK`qf8S!EL(>>zZF-%)7_v|=QG(}>Ah~mIRJ0&580%O+8~{ml8$x? zlk?W<2PdjntVd*Dm~~4#GTrY)5|LgtMRQ*CBsD^=cARZ9#i(eh_55IWVw8_-4Y^mA zpBMR1uUaibdH3|AM`7(tQLm{G+vSiFA2!RA-k(c|vwktw6fsK6hz;80PY_IX%torO zYyD0~hEGAfmFNgdUqsSVIZB^96;Mf3%&FPlJ9LoDgL`!*dO+n;1o-$i9s~S$2#FD= z!FL#nzRD*GlA{;*t~?!W;`@}zP(d<>B)v@xaUjMmHT(5Fp`514jUS!e9=KK zB_*Vi-ZpcKUkN6g6CQI|+!&An-KfQ9O0A#-*B*um)-&T4fmt-E4$&K?vozA-E#2?h z!>f%t@VsR$@e^=W2(_T7h0~R^eLGQ{6;T4jV_-T)_r+6_R4^?M{Bl$O&n-U+K&Vv@ zQsSqIv~OB}D_T&b9w{!4Hcrrx!G_uS`Ub?j_Y!%+1KQL1cSKIymQ$W-jn3DyPh8yR zT^Iep6ImvGL)AJ@U95}yzs0M0>a?1FsS>Tne*{i-kqkeI^Tl@6CH>AeA0_D4@30#s z7zpTUJ(DV5JRY20_KGD53}W>#?9%Ax@*%$0;_P)}pGg{vaLU#oKjfBPT66z#6FXefon%^n0*?V?x#&grSyJ# zaS*)OnIOV@yCgLrqu)^c#dz~EX4$Fp!yiMXdq0W(I9Xv)yxgUqAx-E`l#QT$wtG$` zxP70*zoV~pI8TLiD37u5`NJTv&Gsj$GE$wKDbM2-QuYsA{*z{>X03~lg{RSxd}T!$ zrzYF;WVP-KANo)v+|Y8H^0iZF*kX66eejWV@cyc%tW3`Z2zQJ92}IpvuiL|t8KxSy zYqq7vB^Dg4&(o|cqRQ?6xaseVrM^_n@gzw5hS0S#GN|*s!};x2%O^822KzqZN3|aM zahS;VT%6bayjt6tqBFrp&=Kt{G}FB!@W%XYKbs{(*fYFv z;S4vKbQ)c5r}b>|Ux zfM}xwLEhC*RF+uD;-@MxLJM*w`34d|GPAmnD?@dpYoqyr`N@=|unhn(%{OvhnV8Q; z>A|to-Pff(+Iivl2p-48J#|Erd!`(jZp;@=@d@xkg~hl; zu_m-*^NE`p(v=>hT{&&xkcQqbtc-`HGqqPYHK|zyV67amya;n=MLU-t>Xo_3GDP#w zlpm`*ggX2-pDAD`k!P~Jq-tj9;x=V&kLM_%tkyJpFg0*RlHKI}i?xMMAk``#SA_36qn0pX5V}+>JP_p@n zXe-q|7|H2($=GKWI6_^h5OyPYyPr?dme4m3na|MxdClD6Fd9+sn@aaXQ(W6tYK*&fsNf%dwzEx$_7G9jp1J$wc>eLb}r#|x%Yx(6tOmoQL-LW{l zw}wD|9-mfrj?Z+T8^~pTeQ|~it9uJi?s@2TdEQneQ3a0)Mi#I7gt_sNqh&fYWR@(@)fULX_!#$Yk?WBWzO?S)Y3Y= zKD<>H!(UbTc#S!kp@E&6(O*q0;VZXMC?sx_^kC|d&lQT2nf#gtCH$>IF$kvRu4R81 zWp#oBV>0`jQTtz4y+=LwU1{ace`5+LvsK7pLaQ;pf3RNjf@-iO36${WI_dp{NajA% z-LUoy1cfj@JOef(c&_-7>$4r*U$0`maywzxbc&QoLpxE&yq<}DT`_u)OHI_nu@{ap_E=G;McA_pN85Z0%`t{P9vjHT- zB{uqJC@0-TMzIi_c($wR;4T?9Bc%)1fe{fvA5oVT3v63yXQPeLT7}JP$({f=Bes(61 zg22p0n42((AM$vwOn$nXa_m!B!`4|3wNy&2X<@^(lv$9xvFwiUes$WuTD7cJ>F#96 z_r;6Xr`}FlI#gjPj1f95z7?-`XIOUMKe|%?6+E!Wj#XW%C00zGuw_uKpwe)$Q9(5( z+n6w4GP~a>?s0|Pwn=dk_m!iqOTQt0{VdfP8Xcjxk9MCIm+?N*EAeEp%~IAh=ypM>HnGYBAj*?Gh*3 z{Pb~JA2h}`2&dX&AzJKGoyEm7B#xa?Lit|omG^`RR6K9i3=C6@tL@IHnt?QPj&vWl@fEX9wd__0_UB-*o+80U5K&HwbU*15*9P z&k1H?A0+qzl2qS02%pd^aHdJ|}Rt_~Vw^olSHp9R#G zDg(tV9sSWkx=@KH#j?N`70HZKHmIA+h#R3{UkC+U&27bKeh3YDVU?Kzr80u@&;f6Vg`L^cl)!YXl zcPRKSwP1?<@9zP4XmQ_oZ<0Pi@dH0z|V2tenE&@$b9SJjGNyyh!2{x#59Y|u#L^r3W=#cyZ#AsBS}QvXbe zBIw10CyvNE}#G;Ljo>}lF&dsvL+E8Bx)mMp~p~QF}l=j0Vgs# z$scrHBhXJ4K@G5DhJt%a@d^`3f^SU-Wm)|R4{}R1aEGw5B~Q~tiFS;vtRe1Zs@r|> zJnw80?+a~I@}W!M{%XitLKiy2=R#uozElODZIx@yftB~lY)-<(5mMwG7MS3%y*zC< zUkuH?MgU~m*_mf_Ug5ve-8k)EGouv1zN9$!v#tBG3x%y+olc1dTGRuQCu^yb2%x9LxC|J4c?qU+Op5QT_-xOS&KU2K}CX6b8JCA%-zk_Pu4r* zMROMsm5{rz@}^E0vyDJO;>q2KFV>K)86RKNo%MyLDf?KxP;y;`vDUQ#HP%vOmNVh~ z=;3+Fa`4F2K2dTQ=eOIYOFbk%8=NRDY;I)f_!(S6V3L$2&H47fP0mq{dgb{b$Yux@ zhkn52uSTeqN=x);XEkjFbj)eg@B>cw`z#+X9p0FITFNfL>~UAj!iKfdvMU638fa}| zx;t=R4@KhP^Ro^=6v8a|0m45Ab^!y0)+qT;isjM-`~Vu8L4A0zZu*6-zVMwgR2pRT z-lfXGOPz0Fzg?`)X+EnYQY0k&dbaj_*fsS$3t@7IKPYMsOt2Vh^%|%-s{a}8?)K%bePdqVH)K6kLDxV#P0UQV zEaQ(yuwTXdyKDs75qynozkk>B@7VKx$1d3MYQUFc`f&hHj*E-^KuV(L#f$)X)J>aM zP8wv21WfB*xt*V3^iWApOz1_q^p$)LWw)YthLLUX^OK@M)2)>TO*W0WzWP%HWAA!x z2@gzK^{eB$TTcdgaa%apo#ln!Ne!sMsiwB)t9_;Elb2QaC2(vp%g(noKs~9ID;s^g zci(GIyI8Js@v+{Sym98u2Zc)UdRkqKx|N@E@4oBj4Mex?;bWX2A=Fr_Y>!lVn+~k9 zzr&XT!2qn%h>9;o30S3klYVRX+1JWX&*Furww}0+c6+5q?;HiyCw+Z!Cs+wz#xx-` zhI8*5`}BN11&m2wf(K@q-|wS+)IAnOTHn|@=g13?2M4T|(AtsPCW+y`^<0~utD8g| zeXQBH+WSy8tPfA;oY1XRFsqhXP^@2)F7GSP9JxnEn- z%nb>lTsj}QG(ICLP;Ve#H9K*(fAO)4TsIdlonb-B@oYp_Q~gSJtsgiN+)H@ahY>6% zic0ehC<4}6&+*Hi{3*}^GJQwDpkiDj$F~5<>*6E8%ILJn=hp!Sa#{k?k(M7TJVtMs za~T=h2Xw~rlW`d~gh92$hS5Q1Pe8p}4@b!5@F`ex2))0Vx+V;X`^xQtQ28|mj@YcY zd>7C92^?_?$}@i|3n>B(KAuL8Wi?d+vI5VRTyS4`PY)d*Du0&52fbRS*4UnBKB}8yy!{AmU7qOQc4JHt$GW0Ho7}gqPJ@*Sifdl9GM;B z%b-i6JKGfE8qwNapuVyxXxRwHD0M?0(xv zJfn@4uy|iO37P+d9p4<5}Wc8&r?n@43D7CLN>rjQA|$aMlCt71JdF!LBh1=Tb0{8UYlC3T`487ZLY1H|Wu|D{j>(zUtutyMAn5Be%6&`zTj}b5FSsBA5GMC@_*%_C{))II@&qXaB*S_C1>rY2OiN31 zj?ES$AZ#z&Ngkm8|DXmEAAp(Y{us^}aU~F@mTGsNgAY*rZ@x@tgUY1`x!4mHTu2BD zpaBeCX9p2LlNOSmjn-graka?+Cz_{?FdQ%LHI*VL7gwMrsX^N|SR&z6LH@pvMOaX> zi^I~})qg{8;sD-o{?synUQ_uef1HQ`7Yp2~lQGCen8b8e3D zYCo{(I}wAXw&uH@%LC|&!nMc#Aa*82uyuAhOqCOa-VXtCAyO4z&z~efaJQO!lx^YQ zV(RO^pj;ZLayb2Ja0QqX>h9F~SR8`d4`5v5yXH1q*f{|t$PTQ(n|rPVJN-6`)-`fK za7V1(uF4<=s9Pa-}*@4Hp0V z>pv~w{eLf~QS-o59Gt5z^3oET|IvuQSk7+SJjh%BVG!WW418{NIB&*Uzz&IBe-NO< zanAUcS^d+#dX$0TPcllM{_~T6y=nRlT*iba&VUy%{{Q-V^aQkO8BMYY{O@1BO(g@y zx`!)%1gd)fYVUu(Z`uH@VzDIpFJJlM*gZzJ= oNk2HIWg~d#zmqFH?kVJ0tY>_pgs5r~7YF>w%P32iO1|*_AI&f7i~s-t From bdcb57f61c447588f15579daf87b58b190ecc584 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 13 Feb 2024 13:50:48 +0100 Subject: [PATCH 80/97] Resolve conflicts --- simulator/simple_planning_simulator/README.md | 40 +++++++++++-------- 1 file changed, 23 insertions(+), 17 deletions(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index d2ed19708c5c5..4c465c720ca2b 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -63,28 +63,34 @@ The purpose of this simulator is for the integration test of planning and contro - `DELAY_STEER_ACC` - `DELAY_STEER_ACC_GEARED` - `DELAY_STEER_MAP_ACC_GEARED`: applies 1D dynamics and time delay to the steering and acceleration commands. The simulated acceleration is determined by a value converted through the provided acceleration map. This model is valuable for an accurate simulation with acceleration deviations in a real vehicle. +- `LEARNED_STEER_VEL`: launches learned python models. More about this [here](https://github.com/atomyks/autoware.universe/tree/feature/simulator_py_model/simulator/learned_model). The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves based on a 1st-order with time delay model. The `STEER` means the model receives the steer command. The `VEL` means the model receives the target velocity command, while the `ACC` model receives the target acceleration command. The `GEARED` suffix means that the motion will consider the gear command: the vehicle moves only one direction following the gear. The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V). -| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | D_ST_M_ACC_G | Default value | unit | -| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :----------- | :------------ | :------ | -| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | o | 0.1 | [s] | -| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | o | 0.24 | [s] | -| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | x | 0.25 | [s] | -| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | o | 0.1 | [s] | -| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | o | 0.27 | [s] | -| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | x | 0.0 | [rad] | -| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | x | 0.5 | [s] | -| vel_lim | double | limit of velocity | x | x | x | o | o | o | o | 50.0 | [m/s] | -| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | 7.0 | [m/ss] | -| steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | 1.0 | [rad] | -| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | 5.0 | [rad/s] | -| steer_bias | double | bias for steering angle | x | x | x | o | o | o | o | 0.0 | [rad] | -| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | x | 1.0 | [-] | -| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | x | 1.0 | [-] | -| acceleration_map_path | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x | x | x | x | x | x | o | - | [-] | +| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | D_ST_M_ACC_G | L_S_V | Default value | unit | +| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :----------- | :---- | :------------ | :------ | +| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | o | x | 0.1 | [s] | +| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | o | x | 0.24 | [s] | +| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | x | x | 0.25 | [s] | +| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | o | x | 0.1 | [s] | +| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | o | x | 0.27 | [s] | +| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | x | x | 0.0 | [rad] | +| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | x | x | 0.5 | [s] | +| vel_lim | double | limit of velocity | x | x | x | o | o | o | o | x | 50.0 | [m/s] | +| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | x | 7.0 | [m/ss] | +| steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | x | 1.0 | [rad] | +| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | x | 5.0 | [rad/s] | +| steer_bias | double | bias for steering angle | x | x | x | o | o | o | o | x | 0.0 | [rad] | +| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | x | x | 1.0 | [-] | +| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | x | x | 1.0 | [-] | +| acceleration_map_path | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x | x | x | x | x | x | o | x | - | [-] | +| model_python_paths | string | path to a file where the model is implemented | x | x | x | x | x | x | x | o | - | [-] | +| model_param_paths | string | path to the file where model parameters are stored (can be empty string if no parameter file is required) | x | x | x | x | x | x | x | o | - | [-] | +| model_class_names | string | name of the class that implements the model | x | x | x | x | x | x | x | o | - | [-] | + +_Note:_ Parameters `model_python_paths`, `model_param_paths`, and `model_class_names` need to have the same length. The `acceleration_map` is used only for `DELAY_STEER_MAP_ACC_GEARED` and it shows the acceleration command on the vertical axis and the current velocity on the horizontal axis, with each cell representing the converted acceleration command that is actually used in the simulator's motion calculation. Values in between are linearly interpolated. From 9fd530390d49bcca02147993065009f77ed89d96 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 13 Feb 2024 17:49:49 +0100 Subject: [PATCH 81/97] Fix naming on parameter, change documentation accordingly --- simulator/learned_model/README.md | 6 +++--- simulator/simple_planning_simulator/README.md | 4 ++-- .../simple_planning_simulator_core.cpp | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md index eac18750399a5..9b25c7dc54309 100644 --- a/simulator/learned_model/README.md +++ b/simulator/learned_model/README.md @@ -88,7 +88,7 @@ Add a new sub-model to the model. Inputs: * model_descriptor: Describes what model should be used. The model descriptor contains three strings: - * The first string is a path to a file where the model is implemented. + * The first string is a path to a python module where the model is implemented. * The second string is a path to the file where model parameters are stored. * The third string is the name of the class that implements the model. @@ -139,13 +139,13 @@ InterconnectedModel vehicle; // Example of model descriptors std::tuple model_descriptor_1 = { - (char*)"path_to_file_with_model_class_1", + (char*)"path_to_python_module_with_model_class_1", (char*)nullptr, // If no param file is needed you can pass 'nullptr' (char*)"ModelClass1" }; std::tuple model_descriptor_2 = { - (char*)"path_to_file_with_model_class_2", + (char*)"path_to_python_module_with_model_class_2", (char*)"/path_to/param_file", (char*)"ModelClass2" // Name of the python class. Needs to use the interface from 'Assumptions' }; diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 4c465c720ca2b..bd1554bf4ef81 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -86,11 +86,11 @@ The table below shows which models correspond to what parameters. The model name | debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | x | x | 1.0 | [-] | | debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | x | x | 1.0 | [-] | | acceleration_map_path | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x | x | x | x | x | x | o | x | - | [-] | -| model_python_paths | string | path to a file where the model is implemented | x | x | x | x | x | x | x | o | - | [-] | +| model_module_paths | string | path to a python module where the model is implemented | x | x | x | x | x | x | x | o | - | [-] | | model_param_paths | string | path to the file where model parameters are stored (can be empty string if no parameter file is required) | x | x | x | x | x | x | x | o | - | [-] | | model_class_names | string | name of the class that implements the model | x | x | x | x | x | x | x | o | - | [-] | -_Note:_ Parameters `model_python_paths`, `model_param_paths`, and `model_class_names` need to have the same length. +_Note:_ Parameters `model_module_paths`, `model_param_paths`, and `model_class_names` need to have the same length. The `acceleration_map` is used only for `DELAY_STEER_MAP_ACC_GEARED` and it shows the acceleration command on the vertical axis and the current velocity on the horizontal axis, with each cell representing the converted acceleration command that is actually used in the simulator's motion calculation. Values in between are linearly interpolated. diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 958abdad34c2e..710d1e3edd3c6 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -238,7 +238,7 @@ void SimplePlanningSimulator::initialize_vehicle_model() const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; - std::vector model_python_paths = declare_parameter>("model_python_paths", std::vector({""})); + std::vector model_module_paths = declare_parameter>("model_module_paths", std::vector({""})); std::vector model_param_paths = declare_parameter>("model_param_paths", std::vector({""})); std::vector model_class_names = declare_parameter>("model_class_names", std::vector({""})); @@ -289,7 +289,7 @@ void SimplePlanningSimulator::initialize_vehicle_model() vehicle_model_type_ = VehicleModelType::LEARNED_STEER_VEL; vehicle_model_ptr_ = std::make_shared(timer_sampling_time_ms_ / 1000.0, - model_python_paths, model_param_paths, model_class_names); + model_module_paths, model_param_paths, model_class_names); }else{ throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); From 46ce53ed118b1375fe93596ac9cdb8b9a902865f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 13 Feb 2024 18:29:06 +0000 Subject: [PATCH 82/97] style(pre-commit): autofix --- simulator/learned_model/README.md | 74 +++++--- .../learned_model/interconnected_model.hpp | 6 +- .../model_connections_helpers.hpp | 8 +- .../include/learned_model/simple_pymodel.hpp | 6 +- .../learned_model/submodel_interface.hpp | 2 +- simulator/learned_model/package.xml | 2 +- .../src/interconnected_model.cpp | 172 +++++++++--------- .../src/model_connections_helpers.cpp | 52 +++--- .../learned_model/src/simple_pymodel.cpp | 122 +++++++------ .../learned_model/src/submodel_interface.cpp | 2 +- .../vehicle_model/sim_model.hpp | 2 +- .../sim_model_learned_steer_vel.hpp | 9 +- .../simple_planning_simulator_core.cpp | 22 ++- .../sim_model_learned_steer_vel.cpp | 16 +- 14 files changed, 262 insertions(+), 233 deletions(-) diff --git a/simulator/learned_model/README.md b/simulator/learned_model/README.md index 9b25c7dc54309..35a6e673ff589 100644 --- a/simulator/learned_model/README.md +++ b/simulator/learned_model/README.md @@ -34,7 +34,7 @@ class PythonSubmodelInterface: Calculate forward pass through the model and returns next_state. """ return list() - + def get_state_names(self): # Required """ Return list of string names of the model states (outputs). @@ -55,7 +55,7 @@ class PythonSubmodelInterface: def load_params(self, path): # Required """ - Load parameters of the model. + Load parameters of the model. Inputs: - path: Path to a parameter file to load by the model. """ @@ -76,71 +76,89 @@ class PythonSubmodelInterface: -To successfully create a vehicle model an InterconnectedModel class needs to be set up correctly. +To successfully create a vehicle model an InterconnectedModel class needs to be set up correctly. ### InterconnectedModel class -#### ```Constructor``` +#### `Constructor` + The constructor takes no arguments. -#### ```void addSubmodel(std::tuple model_descriptor)``` +#### `void addSubmodel(std::tuple model_descriptor)` + Add a new sub-model to the model. Inputs: -* model_descriptor: Describes what model should be used. The model descriptor contains three strings: - * The first string is a path to a python module where the model is implemented. - * The second string is a path to the file where model parameters are stored. - * The third string is the name of the class that implements the model. + +- model_descriptor: Describes what model should be used. The model descriptor contains three strings: + - The first string is a path to a python module where the model is implemented. + - The second string is a path to the file where model parameters are stored. + - The third string is the name of the class that implements the model. Outputs: -* None -#### ```void generateConnections(std::vector in_names, std::vector out_names)``` +- None + +#### `void generateConnections(std::vector in_names, std::vector out_names)` + Generate connections between sub-models and inputs/outputs of the model. Inputs: -* in_names: String names for all of the model inputs in order. -* out_names: String names for all of the model outputs in order. + +- in_names: String names for all of the model inputs in order. +- out_names: String names for all of the model outputs in order. Outputs: -* None -#### ```void initState(std::vector new_state)``` +- None + +#### `void initState(std::vector new_state)` + Set the initial state of the model. Inputs: -* new_state: New state of the model. + +- new_state: New state of the model. Outputs: -* None -#### ```std::vector updatePymodel(std::vector psim_input)``` +- None + +#### `std::vector updatePymodel(std::vector psim_input)` + Calculate the next state of the model by calculating the next state of all of the sub-models. Inputs: -* psim_input: Input to the model. + +- psim_input: Input to the model. Outputs: -* next_state: Next state of the model. -#### ```dtSet(double dt)``` -Set the time step of the model. +- next_state: Next state of the model. + +#### `dtSet(double dt)` + +Set the time step of the model. Inputs: -* dt: time step + +- dt: time step Outputs: -* None + +- None ### Example + Firstly we need to set up the model. + ```C++ InterconnectedModel vehicle; // Example of model descriptors std::tuple model_descriptor_1 = { (char*)"path_to_python_module_with_model_class_1", - (char*)nullptr, // If no param file is needed you can pass 'nullptr' + (char*)nullptr, // If no param file is needed you can pass 'nullptr' (char*)"ModelClass1" }; @@ -158,14 +176,14 @@ vehicle.addSubmodel(model_descriptor_2); std::vector state_names = {(char*)"STATE_NAME_1", (char*)"STATE_NAME_2"}; std::vector input_names = {(char*)"INPUT_NAME_1", (char*)"INPUT_NAME_2"}; -// Automatically connect sub-systems with model input +// Automatically connect sub-systems with model input vehicle.generateConnections(input_names, state_names); // Set the time step of the model vehicle.dtSet(dt); ``` -After the model is correctly set up, we can use it the following way. +After the model is correctly set up, we can use it the following way. ```C++ // Example of an model input @@ -174,7 +192,7 @@ std::vector vehicle_input = {0.0, 1.0}; // INPUT_NAME_1, INPUT_NAME_2 // Example of an model state std::vector current_state = {0.2, 0.5}; // STATE_NAME_1, STATE_NAME_2 -// Set model state +// Set model state vehicle.initState(current_state); // Calculate the next state of the model diff --git a/simulator/learned_model/include/learned_model/interconnected_model.hpp b/simulator/learned_model/include/learned_model/interconnected_model.hpp index 0cfc1a5bbcbf1..52aef67adf3e6 100644 --- a/simulator/learned_model/include/learned_model/interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/interconnected_model.hpp @@ -2,8 +2,8 @@ #define LEARNED_MODEL__INTERCONNECTED_MODEL_HPP_ #include "learned_model/model_connections_helpers.hpp" -#include "learned_model/submodel_interface.hpp" #include "learned_model/simple_pymodel.hpp" +#include "learned_model/submodel_interface.hpp" #include #include @@ -13,7 +13,7 @@ namespace py = pybind11; -class __attribute__ ((visibility ("default"))) InterconnectedModel +class __attribute__((visibility("default"))) InterconnectedModel { // Vector of unique names of inputs and outputs of sub-models std::vector signals_vec_names; @@ -108,4 +108,4 @@ class __attribute__ ((visibility ("default"))) InterconnectedModel std::vector updatePymodel(std::vector psim_input); }; -#endif // LEARNED_MODEL__INTERCONNECTED_MODEL_HPP_ \ No newline at end of file +#endif // LEARNED_MODEL__INTERCONNECTED_MODEL_HPP_ diff --git a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp index de76f84979c75..d8dd7cb697f44 100644 --- a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp +++ b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp @@ -1,13 +1,13 @@ #ifndef LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ #define LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ -#include #include +#include std::vector fillVectorUsingMap( - std::vector vector1, std::vector vector2, std::vector map, bool inverse); + std::vector vector1, std::vector vector2, std::vector map, bool inverse); std::vector createConnectionsMap( - std::vector connection_names_1, std::vector connection_names_2); + std::vector connection_names_1, std::vector connection_names_2); -#endif // LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ \ No newline at end of file +#endif // LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ diff --git a/simulator/learned_model/include/learned_model/simple_pymodel.hpp b/simulator/learned_model/include/learned_model/simple_pymodel.hpp index 4af43209100a4..fe9bf5596bf9f 100644 --- a/simulator/learned_model/include/learned_model/simple_pymodel.hpp +++ b/simulator/learned_model/include/learned_model/simple_pymodel.hpp @@ -40,7 +40,8 @@ class SimplePymodel : public SubModelInterface * @param [in] param_file_path path to saved parameter file of the python sub-model * @param [in] py_class_name name of the python class */ - SimplePymodel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name); + SimplePymodel( + std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name); /** * @brief calculate the next state of a python model @@ -76,7 +77,6 @@ class SimplePymodel : public SubModelInterface * @param [in] signal_vec_names names of signals in model signal vector */ void mapOutputs(std::vector signals_vec_names) override; - }; -#endif // LEARNED_MODEL__SIMPLE_PYMODEL_HPP_ \ No newline at end of file +#endif // LEARNED_MODEL__SIMPLE_PYMODEL_HPP_ diff --git a/simulator/learned_model/include/learned_model/submodel_interface.hpp b/simulator/learned_model/include/learned_model/submodel_interface.hpp index b1dd24a6b3068..00511bf934107 100644 --- a/simulator/learned_model/include/learned_model/submodel_interface.hpp +++ b/simulator/learned_model/include/learned_model/submodel_interface.hpp @@ -45,4 +45,4 @@ class SubModelInterface std::vector model_signals_vec, std::vector model_signals_vec_next) = 0; }; -#endif // LEARNED_MODEL__SUBMODEL_INTERFACE_HPP_ \ No newline at end of file +#endif // LEARNED_MODEL__SUBMODEL_INTERFACE_HPP_ diff --git a/simulator/learned_model/package.xml b/simulator/learned_model/package.xml index 9fa8c511a858d..0680fea2b3fea 100644 --- a/simulator/learned_model/package.xml +++ b/simulator/learned_model/package.xml @@ -23,4 +23,4 @@ ament_cmake - \ No newline at end of file + diff --git a/simulator/learned_model/src/interconnected_model.cpp b/simulator/learned_model/src/interconnected_model.cpp index bcecba0aa09db..87807ebb39ab2 100644 --- a/simulator/learned_model/src/interconnected_model.cpp +++ b/simulator/learned_model/src/interconnected_model.cpp @@ -1,124 +1,126 @@ #include "learned_model/interconnected_model.hpp" - void InterconnectedModel::mapInputs(std::vector in_names) { - // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index - // in "signals_vec_names" - map_in_to_sig_vec = createConnectionsMap(signals_vec_names, in_names); - } + // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index + // in "signals_vec_names" + map_in_to_sig_vec = createConnectionsMap(signals_vec_names, in_names); +} void InterconnectedModel::mapOutputs(std::vector out_names) { - // index in "map_sig_vec_to_out" is index in "out_names" and value in "map_sig_vec_to_out" is - // index in "signals_vec_names" - map_sig_vec_to_out = createConnectionsMap(signals_vec_names, out_names); + // index in "map_sig_vec_to_out" is index in "out_names" and value in "map_sig_vec_to_out" is + // index in "signals_vec_names" + map_sig_vec_to_out = createConnectionsMap(signals_vec_names, out_names); } void InterconnectedModel::addNamesToSigVec(const std::vector & names) { - // Check if the name is already in the vector. If not add it. - for (char * name : names) { - if ( - std::find(signals_vec_names.begin(), signals_vec_names.end(), name) == - signals_vec_names.end()) { - signals_vec_names.push_back(name); - } + // Check if the name is already in the vector. If not add it. + for (char * name : names) { + if ( + std::find(signals_vec_names.begin(), signals_vec_names.end(), name) == + signals_vec_names.end()) { + signals_vec_names.push_back(name); } + } } -void InterconnectedModel::getSignalNames(std::vector in_names, std::vector out_names) +void InterconnectedModel::getSignalNames( + std::vector in_names, std::vector out_names) { - addNamesToSigVec(in_names); - addNamesToSigVec(out_names); - for (auto & submodel : submodels) { - addNamesToSigVec(submodel->getInputNames()); - addNamesToSigVec(submodel->getStateNames()); - } + addNamesToSigVec(in_names); + addNamesToSigVec(out_names); + for (auto & submodel : submodels) { + addNamesToSigVec(submodel->getInputNames()); + addNamesToSigVec(submodel->getStateNames()); + } } -void InterconnectedModel::generateConnections(std::vector in_names, std::vector out_names) +void InterconnectedModel::generateConnections( + std::vector in_names, std::vector out_names) { - // Create vector of signal names - getSignalNames(in_names, out_names); - num_signals = signals_vec_names.size(); - // Init vector of signal values - for (int i = 0; i < num_signals; i++) model_signals_vec.push_back(0); - - // For every sub-model create mapping from vector of signals to inputs and outputs - for (auto & submodel : submodels) { + // Create vector of signal names + getSignalNames(in_names, out_names); + num_signals = signals_vec_names.size(); + // Init vector of signal values + for (int i = 0; i < num_signals; i++) model_signals_vec.push_back(0); + + // For every sub-model create mapping from vector of signals to inputs and outputs + for (auto & submodel : submodels) { submodel->mapInputs(signals_vec_names); submodel->mapOutputs(signals_vec_names); - } + } - // Create mapping from vector of signals to inputs and outputs of PSIM - mapInputs(in_names); - mapOutputs(out_names); + // Create mapping from vector of signals to inputs and outputs of PSIM + mapInputs(in_names); + mapOutputs(out_names); } -void InterconnectedModel::addSubmodel(std::tuple submodel_desc) +void InterconnectedModel::addSubmodel( + std::tuple submodel_desc) { - const auto [lib_path, param_path, class_name] = submodel_desc; - auto new_model = new SimplePymodel(lib_path, param_path, class_name); - submodels.push_back(std::unique_ptr(new_model)); + const auto [lib_path, param_path, class_name] = submodel_desc; + auto new_model = new SimplePymodel(lib_path, param_path, class_name); + submodels.push_back(std::unique_ptr(new_model)); } void InterconnectedModel::initState(std::vector new_state) { - bool state_changed_externally = false; + bool state_changed_externally = false; + + // Check if some state was changed externally + for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { + if ( + abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > + 1e-6) { + state_changed_externally = true; + break; + } + } + + if (state_changed_externally) { + // Reinitialize model + std::cout << "Reseting model" << std::endl; + + // Currently initializing model to zero -> TODO find a way how to initialize them to some + // other default values + std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); - // Check if some state was changed externally for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { - if ( - abs(model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] - new_state[PSIM_STATE_IDX]) > - 1e-6) { - state_changed_externally = true; - break; - } - } - - if (state_changed_externally) { - // Reinitialize model - std::cout << "Reseting model" << std::endl; - - // Currently initializing model to zero -> TODO find a way how to initialize them to some - // other default values - std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); - - for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < new_state.size(); PSIM_STATE_IDX++) { - model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] = new_state[PSIM_STATE_IDX]; - } + model_signals_vec[map_sig_vec_to_out[PSIM_STATE_IDX]] = new_state[PSIM_STATE_IDX]; } + } } void InterconnectedModel::dtSet(double dt) { - for (auto & submodel : submodels) { - submodel->dtSet(dt); - } + for (auto & submodel : submodels) { + submodel->dtSet(dt); + } } std::vector InterconnectedModel::updatePymodel(std::vector psim_input) { - // map input to vector of all variables - for (size_t PSIM_INPUT_IDX = 0; PSIM_INPUT_IDX < psim_input.size(); PSIM_INPUT_IDX++) { - model_signals_vec[map_in_to_sig_vec[PSIM_INPUT_IDX]] = psim_input[PSIM_INPUT_IDX]; - } - - // Compute forward pass through all models (order should not matter) - std::vector model_signals_vec_next = model_signals_vec; - for (auto & submodel : submodels) { - model_signals_vec_next = submodel->getNextState(model_signals_vec, model_signals_vec_next); - } - - // Map vector of all variables to - std::vector psim_next_state; - for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < map_sig_vec_to_out.size(); PSIM_STATE_IDX++) { - psim_next_state.push_back(model_signals_vec_next[map_sig_vec_to_out[PSIM_STATE_IDX]]); - } - - // Update vector of all variables - model_signals_vec = model_signals_vec_next; - - return psim_next_state; -} \ No newline at end of file + // map input to vector of all variables + for (size_t PSIM_INPUT_IDX = 0; PSIM_INPUT_IDX < psim_input.size(); PSIM_INPUT_IDX++) { + model_signals_vec[map_in_to_sig_vec[PSIM_INPUT_IDX]] = psim_input[PSIM_INPUT_IDX]; + } + + // Compute forward pass through all models (order should not matter) + std::vector model_signals_vec_next = model_signals_vec; + for (auto & submodel : submodels) { + model_signals_vec_next = submodel->getNextState(model_signals_vec, model_signals_vec_next); + } + + // Map vector of all variables to + std::vector psim_next_state; + for (size_t PSIM_STATE_IDX = 0; PSIM_STATE_IDX < map_sig_vec_to_out.size(); PSIM_STATE_IDX++) { + psim_next_state.push_back(model_signals_vec_next[map_sig_vec_to_out[PSIM_STATE_IDX]]); + } + + // Update vector of all variables + model_signals_vec = model_signals_vec_next; + + return psim_next_state; +} diff --git a/simulator/learned_model/src/model_connections_helpers.cpp b/simulator/learned_model/src/model_connections_helpers.cpp index 6b3541b4e960f..49bc166dd9673 100644 --- a/simulator/learned_model/src/model_connections_helpers.cpp +++ b/simulator/learned_model/src/model_connections_helpers.cpp @@ -1,33 +1,33 @@ #include "learned_model/model_connections_helpers.hpp" std::vector fillVectorUsingMap( - std::vector vector1, std::vector vector2, std::vector map, bool inverse) - { - // index in "map" is index in "vector1" and value in "map" is index in "vector2" - // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 - for (std::size_t idx = 0; idx < map.size(); idx++) { - if (map[idx] == -1) continue; - inverse ? vector1[idx] = vector2[map[idx]] : vector2[map[idx]] = vector1[idx]; - } - return inverse ? vector1 : vector2; + std::vector vector1, std::vector vector2, std::vector map, bool inverse) +{ + // index in "map" is index in "vector1" and value in "map" is index in "vector2" + // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 + for (std::size_t idx = 0; idx < map.size(); idx++) { + if (map[idx] == -1) continue; + inverse ? vector1[idx] = vector2[map[idx]] : vector2[map[idx]] = vector1[idx]; } + return inverse ? vector1 : vector2; +} - std::vector createConnectionsMap( - std::vector connection_names_1, std::vector connection_names_2) - { - std::vector connection_map; - // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is - // index in "connection_names_1" - for (auto name_2 : connection_names_2) { - int mapped_idx = -1; // -1 means that we cannot create a connection between some signals - for (std::size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { - if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same - // and we can create connection - mapped_idx = NAME_1_IDX; - break; - } +std::vector createConnectionsMap( + std::vector connection_names_1, std::vector connection_names_2) +{ + std::vector connection_map; + // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is + // index in "connection_names_1" + for (auto name_2 : connection_names_2) { + int mapped_idx = -1; // -1 means that we cannot create a connection between some signals + for (std::size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { + if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same + // and we can create connection + mapped_idx = NAME_1_IDX; + break; } - connection_map.push_back(mapped_idx); } - return connection_map; - } \ No newline at end of file + connection_map.push_back(mapped_idx); + } + return connection_map; +} diff --git a/simulator/learned_model/src/simple_pymodel.cpp b/simulator/learned_model/src/simple_pymodel.cpp index 6ebfff3f27a6c..ecfcd526048a0 100644 --- a/simulator/learned_model/src/simple_pymodel.cpp +++ b/simulator/learned_model/src/simple_pymodel.cpp @@ -1,92 +1,102 @@ #include "learned_model/simple_pymodel.hpp" -#include - #include #include +#include + namespace py = pybind11; -SimplePymodel::SimplePymodel(std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name) +SimplePymodel::SimplePymodel( + std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name) { -// Import model class -pymodel_import_name = pymodel_import_name_; -if (!pymodel_import_name.empty()) { + // Import model class + pymodel_import_name = pymodel_import_name_; + if (!pymodel_import_name.empty()) { // Import python module py::module_ imported_module = py::module_::import(pymodel_import_name.c_str()); // Initialize model class from imported module py_model_class = imported_module.attr(py_class_name.c_str())(); -} else { + } else { // TODO throw exception/error return; -} + } -// Load model parameters and reset the model -if (!param_file_path.empty()) { + // Load model parameters and reset the model + if (!param_file_path.empty()) { py::object load_params_succ = py_model_class.attr("load_params")(param_file_path.c_str()); py_model_class.attr("reset")(); std::cout << "Params loaded" << std::endl; -} else { + } else { // TODO warning that using default model params -} + } -// Get string names of states of python model, convert them to C++ string and store them in -// pymodel_state_names -py::list pymodel_state_names_ = py_model_class.attr("get_state_names")(); -num_outputs_py = pymodel_state_names_.size(); -for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++) { + // Get string names of states of python model, convert them to C++ string and store them in + // pymodel_state_names + py::list pymodel_state_names_ = py_model_class.attr("get_state_names")(); + num_outputs_py = pymodel_state_names_.size(); + for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++) { pymodel_state_names.push_back(PyBytes_AS_STRING( - PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); -} + PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); + } -// Get string names of actions (inputs) of python model, convert them to C++ string and store -// them in pymodel_input_names -py::list pymodel_input_names_ = py_model_class.attr("get_action_names")(); -num_inputs_py = pymodel_input_names_.size(); -for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++) { + // Get string names of actions (inputs) of python model, convert them to C++ string and store + // them in pymodel_input_names + py::list pymodel_input_names_ = py_model_class.attr("get_action_names")(); + num_inputs_py = pymodel_input_names_.size(); + for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++) { pymodel_input_names.push_back(PyBytes_AS_STRING( - PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); -} + PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); + } -std::cout << "PymodelInterface import name: " << pymodel_import_name << std::endl; + std::cout << "PymodelInterface import name: " << pymodel_import_name << std::endl; } std::vector SimplePymodel::getNextState( - std::vector model_signals_vec, std::vector model_signals_vec_next) - { - // get inputs and states of the python model from the vector of signals - std::vector py_inputs(num_inputs_py); - std::vector py_state(num_outputs_py); - py_inputs = fillVectorUsingMap(py_inputs, model_signals_vec, map_sig_vec_to_pyin, true); - py_state = fillVectorUsingMap(py_state, model_signals_vec, map_pyout_to_sig_vec, true); - - // forward pass through the base model - py::tuple res = py_model_class.attr("forward")(py_inputs, py_state); - std::vector py_state_next = res.cast>(); - - // map outputs from python model to required outputs - std::vector next_state = - fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); - - return next_state; - } + std::vector model_signals_vec, std::vector model_signals_vec_next) +{ + // get inputs and states of the python model from the vector of signals + std::vector py_inputs(num_inputs_py); + std::vector py_state(num_outputs_py); + py_inputs = fillVectorUsingMap(py_inputs, model_signals_vec, map_sig_vec_to_pyin, true); + py_state = fillVectorUsingMap(py_state, model_signals_vec, map_pyout_to_sig_vec, true); + + // forward pass through the base model + py::tuple res = py_model_class.attr("forward")(py_inputs, py_state); + std::vector py_state_next = res.cast>(); + + // map outputs from python model to required outputs + std::vector next_state = + fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); -void SimplePymodel::dtSet(double dt) { py_model_class.attr("dtSet")(dt); } + return next_state; +} + +void SimplePymodel::dtSet(double dt) +{ + py_model_class.attr("dtSet")(dt); +} -std::vector SimplePymodel::getInputNames() { return pymodel_input_names; } +std::vector SimplePymodel::getInputNames() +{ + return pymodel_input_names; +} -std::vector SimplePymodel::getStateNames() { return pymodel_state_names; } +std::vector SimplePymodel::getStateNames() +{ + return pymodel_state_names; +} void SimplePymodel::mapInputs(std::vector signals_vec_names) { -// index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is -// index in "signals_vec_names" -map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); + // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is + // index in "signals_vec_names" + map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); } -void SimplePymodel::mapOutputs(std::vector signals_vec_names) +void SimplePymodel::mapOutputs(std::vector signals_vec_names) { - // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in - // "map_pyout_to_sig_vec" is index in "signals_vec_names" - map_pyout_to_sig_vec = createConnectionsMap(signals_vec_names, pymodel_state_names); -} \ No newline at end of file + // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in + // "map_pyout_to_sig_vec" is index in "signals_vec_names" + map_pyout_to_sig_vec = createConnectionsMap(signals_vec_names, pymodel_state_names); +} diff --git a/simulator/learned_model/src/submodel_interface.cpp b/simulator/learned_model/src/submodel_interface.cpp index b7edded274e6e..b9563ffe49ef3 100644 --- a/simulator/learned_model/src/submodel_interface.cpp +++ b/simulator/learned_model/src/submodel_interface.cpp @@ -1 +1 @@ -#include "learned_model/submodel_interface.hpp" \ No newline at end of file +#include "learned_model/submodel_interface.hpp" diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp index fe044beb95cf7..ced7349f28914 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp @@ -19,10 +19,10 @@ #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp" #endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp index 2ca4cfbeb8359..de86fd1f916d9 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp @@ -39,10 +39,9 @@ class SimModelLearnedSteerVel : public SimModelInterface /*delay steer velocity* * @brief constructor * @param [in] dt delta time information to set input buffer for delay */ - SimModelLearnedSteerVel(double dt, - std::vector model_python_paths, - std::vector model_param_paths, - std::vector model_class_names); + SimModelLearnedSteerVel( + double dt, std::vector model_python_paths, + std::vector model_param_paths, std::vector model_class_names); /** * @brief destructor @@ -142,4 +141,4 @@ class SimModelLearnedSteerVel : public SimModelInterface /*delay steer velocity* } }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ \ No newline at end of file +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 710d1e3edd3c6..524597e927f61 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -238,9 +238,12 @@ void SimplePlanningSimulator::initialize_vehicle_model() const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; - std::vector model_module_paths = declare_parameter>("model_module_paths", std::vector({""})); - std::vector model_param_paths = declare_parameter>("model_param_paths", std::vector({""})); - std::vector model_class_names = declare_parameter>("model_class_names", std::vector({""})); + std::vector model_module_paths = declare_parameter>( + "model_module_paths", std::vector({""})); + std::vector model_param_paths = declare_parameter>( + "model_param_paths", std::vector({""})); + std::vector model_class_names = declare_parameter>( + "model_class_names", std::vector({""})); if (vehicle_model_type_str == "IDEAL_STEER_VEL") { vehicle_model_type_ = VehicleModelType::IDEAL_STEER_VEL; @@ -285,13 +288,13 @@ void SimplePlanningSimulator::initialize_vehicle_model() vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_bias, acceleration_map_path); - } else if (vehicle_model_type_str == "LEARNED_STEER_VEL"){ + } else if (vehicle_model_type_str == "LEARNED_STEER_VEL") { vehicle_model_type_ = VehicleModelType::LEARNED_STEER_VEL; - vehicle_model_ptr_ = std::make_shared(timer_sampling_time_ms_ / 1000.0, - model_module_paths, model_param_paths, model_class_names); + vehicle_model_ptr_ = std::make_shared( + timer_sampling_time_ms_ / 1000.0, model_module_paths, model_param_paths, model_class_names); - }else{ + } else { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } } @@ -592,10 +595,9 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED) { state << x, y, yaw, vx; } else if ( // NOLINT - vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL) - { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL) { state << x, y, yaw, vx, steer; - } else if (vehicle_model_type_ == VehicleModelType::LEARNED_STEER_VEL){ + } else if (vehicle_model_type_ == VehicleModelType::LEARNED_STEER_VEL) { state << x, y, yaw, yaw_rate, vx, vy, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC || diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp index 46280370cdda0..3e325d15123cf 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp @@ -18,16 +18,14 @@ #include -SimModelLearnedSteerVel::SimModelLearnedSteerVel(double dt, - std::vector model_python_paths, - std::vector model_param_paths, - std::vector model_class_names - ) : SimModelInterface(7 /* dim x */, 2 /* dim u */) +SimModelLearnedSteerVel::SimModelLearnedSteerVel( + double dt, std::vector model_python_paths, + std::vector model_param_paths, std::vector model_class_names) +: SimModelInterface(7 /* dim x */, 2 /* dim u */) { - for (size_t i = 0; i < model_python_paths.size(); i++){ + for (size_t i = 0; i < model_python_paths.size(); i++) { std::tuple descriptor = { - model_python_paths[i], model_param_paths[i], model_class_names[i] - }; + model_python_paths[i], model_param_paths[i], model_class_names[i]}; vehicle.addSubmodel(descriptor); } @@ -89,4 +87,4 @@ void SimModelLearnedSteerVel::update(const double & dt) // Calculate current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; prev_vx_ = input_(IDX_U::VX_DES); -} \ No newline at end of file +} From 35523fc0f80face422329da62df17f0da25d9b18 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 19 Feb 2024 13:45:18 +0100 Subject: [PATCH 83/97] Add copyright --- .../include/learned_model/interconnected_model.hpp | 14 ++++++++++++++ .../learned_model/model_connections_helpers.hpp | 14 ++++++++++++++ .../include/learned_model/simple_pymodel.hpp | 14 ++++++++++++++ .../include/learned_model/submodel_interface.hpp | 14 ++++++++++++++ .../learned_model/src/interconnected_model.cpp | 14 ++++++++++++++ .../src/model_connections_helpers.cpp | 14 ++++++++++++++ simulator/learned_model/src/simple_pymodel.cpp | 14 ++++++++++++++ simulator/learned_model/src/submodel_interface.cpp | 14 ++++++++++++++ .../vehicle_model/sim_model_learned_steer_vel.hpp | 2 +- .../vehicle_model/sim_model_learned_steer_vel.cpp | 2 +- 10 files changed, 114 insertions(+), 2 deletions(-) diff --git a/simulator/learned_model/include/learned_model/interconnected_model.hpp b/simulator/learned_model/include/learned_model/interconnected_model.hpp index 52aef67adf3e6..e5985cdf065bb 100644 --- a/simulator/learned_model/include/learned_model/interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/interconnected_model.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef LEARNED_MODEL__INTERCONNECTED_MODEL_HPP_ #define LEARNED_MODEL__INTERCONNECTED_MODEL_HPP_ diff --git a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp index d8dd7cb697f44..7d40c4955f3dd 100644 --- a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp +++ b/simulator/learned_model/include/learned_model/model_connections_helpers.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ #define LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ diff --git a/simulator/learned_model/include/learned_model/simple_pymodel.hpp b/simulator/learned_model/include/learned_model/simple_pymodel.hpp index fe9bf5596bf9f..9895532a84cff 100644 --- a/simulator/learned_model/include/learned_model/simple_pymodel.hpp +++ b/simulator/learned_model/include/learned_model/simple_pymodel.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef LEARNED_MODEL__SIMPLE_PYMODEL_HPP_ #define LEARNED_MODEL__SIMPLE_PYMODEL_HPP_ diff --git a/simulator/learned_model/include/learned_model/submodel_interface.hpp b/simulator/learned_model/include/learned_model/submodel_interface.hpp index 00511bf934107..9ec5fcfcee59f 100644 --- a/simulator/learned_model/include/learned_model/submodel_interface.hpp +++ b/simulator/learned_model/include/learned_model/submodel_interface.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef LEARNED_MODEL__SUBMODEL_INTERFACE_HPP_ #define LEARNED_MODEL__SUBMODEL_INTERFACE_HPP_ diff --git a/simulator/learned_model/src/interconnected_model.cpp b/simulator/learned_model/src/interconnected_model.cpp index 87807ebb39ab2..a29e695cbf338 100644 --- a/simulator/learned_model/src/interconnected_model.cpp +++ b/simulator/learned_model/src/interconnected_model.cpp @@ -1,3 +1,17 @@ +// Copyright 2024 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "learned_model/interconnected_model.hpp" void InterconnectedModel::mapInputs(std::vector in_names) diff --git a/simulator/learned_model/src/model_connections_helpers.cpp b/simulator/learned_model/src/model_connections_helpers.cpp index 49bc166dd9673..4a4ccabe3ceaa 100644 --- a/simulator/learned_model/src/model_connections_helpers.cpp +++ b/simulator/learned_model/src/model_connections_helpers.cpp @@ -1,3 +1,17 @@ +// Copyright 2024 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "learned_model/model_connections_helpers.hpp" std::vector fillVectorUsingMap( diff --git a/simulator/learned_model/src/simple_pymodel.cpp b/simulator/learned_model/src/simple_pymodel.cpp index ecfcd526048a0..2fbf07f750d4c 100644 --- a/simulator/learned_model/src/simple_pymodel.cpp +++ b/simulator/learned_model/src/simple_pymodel.cpp @@ -1,3 +1,17 @@ +// Copyright 2024 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "learned_model/simple_pymodel.hpp" #include diff --git a/simulator/learned_model/src/submodel_interface.cpp b/simulator/learned_model/src/submodel_interface.cpp index b9563ffe49ef3..cd14a2954fbd1 100644 --- a/simulator/learned_model/src/submodel_interface.cpp +++ b/simulator/learned_model/src/submodel_interface.cpp @@ -1 +1,15 @@ +// Copyright 2024 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "learned_model/submodel_interface.hpp" diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp index de86fd1f916d9..c622edfd0fb14 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2024 The Autoware Foundation. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp index 3e325d15123cf..84bf1279c3c5c 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2024 The Autoware Foundation. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 9dd0557b041cf30d3306a6fd1d9a309d1b4f98d5 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 19 Feb 2024 14:27:44 +0100 Subject: [PATCH 84/97] Remove prints --- simulator/learned_model/src/interconnected_model.cpp | 2 -- simulator/learned_model/src/simple_pymodel.cpp | 3 --- .../vehicle_model/sim_model_learned_steer_vel.cpp | 2 -- 3 files changed, 7 deletions(-) diff --git a/simulator/learned_model/src/interconnected_model.cpp b/simulator/learned_model/src/interconnected_model.cpp index a29e695cbf338..5b8491ad4844c 100644 --- a/simulator/learned_model/src/interconnected_model.cpp +++ b/simulator/learned_model/src/interconnected_model.cpp @@ -95,8 +95,6 @@ void InterconnectedModel::initState(std::vector new_state) if (state_changed_externally) { // Reinitialize model - std::cout << "Reseting model" << std::endl; - // Currently initializing model to zero -> TODO find a way how to initialize them to some // other default values std::fill(model_signals_vec.begin(), model_signals_vec.end(), 0.0); diff --git a/simulator/learned_model/src/simple_pymodel.cpp b/simulator/learned_model/src/simple_pymodel.cpp index 2fbf07f750d4c..46d026e7aed80 100644 --- a/simulator/learned_model/src/simple_pymodel.cpp +++ b/simulator/learned_model/src/simple_pymodel.cpp @@ -40,7 +40,6 @@ SimplePymodel::SimplePymodel( if (!param_file_path.empty()) { py::object load_params_succ = py_model_class.attr("load_params")(param_file_path.c_str()); py_model_class.attr("reset")(); - std::cout << "Params loaded" << std::endl; } else { // TODO warning that using default model params } @@ -62,8 +61,6 @@ SimplePymodel::SimplePymodel( pymodel_input_names.push_back(PyBytes_AS_STRING( PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); } - - std::cout << "PymodelInterface import name: " << pymodel_import_name << std::endl; } std::vector SimplePymodel::getNextState( diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp index 84bf1279c3c5c..aa812d452ab47 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp @@ -32,8 +32,6 @@ SimModelLearnedSteerVel::SimModelLearnedSteerVel( vehicle.generateConnections(input_names, state_names); vehicle.dtSet(dt); - - std::cout << "Python model loaded successfully " << std::endl; } double SimModelLearnedSteerVel::getX() From 95ebf2d7b24760b6fe6ed25ab23116925de26502 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 19 Feb 2024 14:28:39 +0100 Subject: [PATCH 85/97] Fix maintainer email --- simulator/learned_model/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulator/learned_model/package.xml b/simulator/learned_model/package.xml index 0680fea2b3fea..fda761f30c9dd 100644 --- a/simulator/learned_model/package.xml +++ b/simulator/learned_model/package.xml @@ -4,8 +4,8 @@ learned_model 1.0.0 learned_model for simple_planning_simulator - Takamasa Horibe - Tomoya Kimura + Maxime Clement + Tomas Nagy Apache License 2.0 ament_cmake_auto From 11ed37ba5d68849932eb538441f5241a619c5a7c Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 19 Feb 2024 14:29:14 +0100 Subject: [PATCH 86/97] Fix documentation --- simulator/simple_planning_simulator/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index bd1554bf4ef81..e62befa030378 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -63,7 +63,7 @@ The purpose of this simulator is for the integration test of planning and contro - `DELAY_STEER_ACC` - `DELAY_STEER_ACC_GEARED` - `DELAY_STEER_MAP_ACC_GEARED`: applies 1D dynamics and time delay to the steering and acceleration commands. The simulated acceleration is determined by a value converted through the provided acceleration map. This model is valuable for an accurate simulation with acceleration deviations in a real vehicle. -- `LEARNED_STEER_VEL`: launches learned python models. More about this [here](https://github.com/atomyks/autoware.universe/tree/feature/simulator_py_model/simulator/learned_model). +- `LEARNED_STEER_VEL`: launches learned python models. More about this [here](../learned_model). The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves based on a 1st-order with time delay model. The `STEER` means the model receives the steer command. The `VEL` means the model receives the target velocity command, while the `ACC` model receives the target acceleration command. The `GEARED` suffix means that the motion will consider the gear command: the vehicle moves only one direction following the gear. From 5792a509eea4b14aff16a0a38cc12deab94e2ba5 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 19 Feb 2024 21:09:27 +0100 Subject: [PATCH 87/97] Improve documentation --- simulator/simple_planning_simulator/README.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index e62befa030378..cd822c8510581 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -94,6 +94,20 @@ _Note:_ Parameters `model_module_paths`, `model_param_paths`, and `model_class_n The `acceleration_map` is used only for `DELAY_STEER_MAP_ACC_GEARED` and it shows the acceleration command on the vertical axis and the current velocity on the horizontal axis, with each cell representing the converted acceleration command that is actually used in the simulator's motion calculation. Values in between are linearly interpolated. +We created a few basic models to showcase how `LEARNED_STEER_VEL` works. + +1. Install [a library](https://github.com/atomyks/control_analysis_pipeline/tree/v0.1_autoware) that contains basic Python models. (branch: `v0.1_autoware`) + +2. In a file `src/vehicle/sample_vehicle_launch/sample_vehicle_description/config/simulator_model.param.yaml` set `vehicle_model_type` to `LEARNED_STEER_VEL`. In the same file set the following parameters. These models are for testing and do not require any parameter file. +```yaml +model_module_paths: ["control_analysis_pipeline.autoware_models.vehicle.kinematic", "control_analysis_pipeline.autoware_models.steering.steer_example", "control_analysis_pipeline.autoware_models.drive.drive_example"] +model_param_paths: ["", "", ""] +model_class_names: ["KinematicModel", "SteerExample", "DriveExample"] +``` +3. Build the Autoware packages. + +4. Launch the script `ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit` + Example of `acceleration_map.csv` ```csv From 219f4b4ba4abf7a2d26cfff37fccc0c64195205a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 19 Feb 2024 20:11:35 +0000 Subject: [PATCH 88/97] style(pre-commit): autofix --- simulator/simple_planning_simulator/README.md | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index cd822c8510581..e47f0004e4e32 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -99,11 +99,18 @@ We created a few basic models to showcase how `LEARNED_STEER_VEL` works. 1. Install [a library](https://github.com/atomyks/control_analysis_pipeline/tree/v0.1_autoware) that contains basic Python models. (branch: `v0.1_autoware`) 2. In a file `src/vehicle/sample_vehicle_launch/sample_vehicle_description/config/simulator_model.param.yaml` set `vehicle_model_type` to `LEARNED_STEER_VEL`. In the same file set the following parameters. These models are for testing and do not require any parameter file. + ```yaml -model_module_paths: ["control_analysis_pipeline.autoware_models.vehicle.kinematic", "control_analysis_pipeline.autoware_models.steering.steer_example", "control_analysis_pipeline.autoware_models.drive.drive_example"] +model_module_paths: + [ + "control_analysis_pipeline.autoware_models.vehicle.kinematic", + "control_analysis_pipeline.autoware_models.steering.steer_example", + "control_analysis_pipeline.autoware_models.drive.drive_example", + ] model_param_paths: ["", "", ""] model_class_names: ["KinematicModel", "SteerExample", "DriveExample"] ``` + 3. Build the Autoware packages. 4. Launch the script `ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit` From 3d3b26dcb48df899715ed04dc33d94fb3bc1f049 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 4 Mar 2024 18:43:38 +0100 Subject: [PATCH 89/97] Improve documentation --- simulator/simple_planning_simulator/README.md | 40 +++++++++---------- 1 file changed, 19 insertions(+), 21 deletions(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index e47f0004e4e32..f2023db4ecbb8 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -94,27 +94,6 @@ _Note:_ Parameters `model_module_paths`, `model_param_paths`, and `model_class_n The `acceleration_map` is used only for `DELAY_STEER_MAP_ACC_GEARED` and it shows the acceleration command on the vertical axis and the current velocity on the horizontal axis, with each cell representing the converted acceleration command that is actually used in the simulator's motion calculation. Values in between are linearly interpolated. -We created a few basic models to showcase how `LEARNED_STEER_VEL` works. - -1. Install [a library](https://github.com/atomyks/control_analysis_pipeline/tree/v0.1_autoware) that contains basic Python models. (branch: `v0.1_autoware`) - -2. In a file `src/vehicle/sample_vehicle_launch/sample_vehicle_description/config/simulator_model.param.yaml` set `vehicle_model_type` to `LEARNED_STEER_VEL`. In the same file set the following parameters. These models are for testing and do not require any parameter file. - -```yaml -model_module_paths: - [ - "control_analysis_pipeline.autoware_models.vehicle.kinematic", - "control_analysis_pipeline.autoware_models.steering.steer_example", - "control_analysis_pipeline.autoware_models.drive.drive_example", - ] -model_param_paths: ["", "", ""] -model_class_names: ["KinematicModel", "SteerExample", "DriveExample"] -``` - -3. Build the Autoware packages. - -4. Launch the script `ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit` - Example of `acceleration_map.csv` ```csv @@ -146,6 +125,25 @@ default, 0.00, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 1 _Note_: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a _delay_ model. The definition of the _time constant_ is the time it takes for the step response to rise up to 63% of its final value. The _deadtime_ is a delay in the response to a control input. +### Example of LEARNED_STEER_VEL model + +We created a few basic models to showcase how `LEARNED_STEER_VEL` works. + +1. Install [a library](https://github.com/atomyks/control_analysis_pipeline/tree/v0.1_autoware) that contains basic Python models. (branch: `v0.1_autoware`) + +2. In a file `src/vehicle/sample_vehicle_launch/sample_vehicle_description/config/simulator_model.param.yaml` set `vehicle_model_type` to `LEARNED_STEER_VEL`. In the same file set the following parameters. These models are for testing and do not require any parameter file. + +```yaml +model_module_paths: + [ + "control_analysis_pipeline.autoware_models.vehicle.kinematic", + "control_analysis_pipeline.autoware_models.steering.steer_example", + "control_analysis_pipeline.autoware_models.drive.drive_example", + ] +model_param_paths: ["", "", ""] +model_class_names: ["KinematicModel", "SteerExample", "DriveExample"] +``` + ### Default TF configuration Since the vehicle outputs `odom`->`base_link` tf, this simulator outputs the tf with the same frame_id configuration. From 2f4ca6dcc4a828eb914b491c1e29f1089bde9bac Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 11 Mar 2024 10:46:34 +0100 Subject: [PATCH 90/97] Fix unnecessary comments --- .../learned_model/include/learned_model/submodel_interface.hpp | 2 -- .../vehicle_model/sim_model_learned_steer_vel.hpp | 3 --- 2 files changed, 5 deletions(-) diff --git a/simulator/learned_model/include/learned_model/submodel_interface.hpp b/simulator/learned_model/include/learned_model/submodel_interface.hpp index 9ec5fcfcee59f..a16db7b8ca344 100644 --- a/simulator/learned_model/include/learned_model/submodel_interface.hpp +++ b/simulator/learned_model/include/learned_model/submodel_interface.hpp @@ -20,8 +20,6 @@ class SubModelInterface { public: - // virtual ~PymodelInterface() = 0; - /** * @brief set time step of the model * @param [in] dt time step diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp index c622edfd0fb14..7ffab91b1faf7 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp @@ -25,9 +25,6 @@ #include #include -// #include -// namespace py = pybind11; - /** * @class SimModelLearnedSteerVel * @brief calculate delay steering dynamics From c31ae9fd85974b368b4132f43e63bc716c5d42ed Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 19 Mar 2024 16:00:39 +0100 Subject: [PATCH 91/97] Fix pre-commit check errors --- .../include/learned_model/interconnected_model.hpp | 4 ++++ .../include/learned_model/simple_pymodel.hpp | 3 +++ simulator/learned_model/src/simple_pymodel.cpp | 3 --- .../vehicle_model/sim_model_learned_steer_vel.hpp | 11 +++++++---- 4 files changed, 14 insertions(+), 7 deletions(-) diff --git a/simulator/learned_model/include/learned_model/interconnected_model.hpp b/simulator/learned_model/include/learned_model/interconnected_model.hpp index e5985cdf065bb..53c14bff97382 100644 --- a/simulator/learned_model/include/learned_model/interconnected_model.hpp +++ b/simulator/learned_model/include/learned_model/interconnected_model.hpp @@ -24,6 +24,10 @@ #include #include +#include +#include +#include +#include namespace py = pybind11; diff --git a/simulator/learned_model/include/learned_model/simple_pymodel.hpp b/simulator/learned_model/include/learned_model/simple_pymodel.hpp index 9895532a84cff..c4b1707b812ca 100644 --- a/simulator/learned_model/include/learned_model/simple_pymodel.hpp +++ b/simulator/learned_model/include/learned_model/simple_pymodel.hpp @@ -21,6 +21,9 @@ #include #include +#include +#include + namespace py = pybind11; /** diff --git a/simulator/learned_model/src/simple_pymodel.cpp b/simulator/learned_model/src/simple_pymodel.cpp index 46d026e7aed80..454671c3c4910 100644 --- a/simulator/learned_model/src/simple_pymodel.cpp +++ b/simulator/learned_model/src/simple_pymodel.cpp @@ -32,7 +32,6 @@ SimplePymodel::SimplePymodel( // Initialize model class from imported module py_model_class = imported_module.attr(py_class_name.c_str())(); } else { - // TODO throw exception/error return; } @@ -40,8 +39,6 @@ SimplePymodel::SimplePymodel( if (!param_file_path.empty()) { py::object load_params_succ = py_model_class.attr("load_params")(param_file_path.c_str()); py_model_class.attr("reset")(); - } else { - // TODO warning that using default model params } // Get string names of states of python model, convert them to C++ string and store them in diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp index 7ffab91b1faf7..a7ff86c38b1cf 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp @@ -24,6 +24,8 @@ #include #include #include +#include +#include /** * @class SimModelLearnedSteerVel @@ -51,11 +53,12 @@ class SimModelLearnedSteerVel : public SimModelInterface /*delay steer velocity* inputs of this model to states and inputs of the python model. */ - std::vector state_names = {(char *)"POS_X", (char *)"POS_Y", (char *)"YAW", - (char *)"YAW_RATE", (char *)"VX", (char *)"VY", - (char *)"STEER"}; + std::vector state_names = {const_cast("POS_X"), const_cast("POS_Y"), + const_cast("YAW"), const_cast("YAW_RATE"), + const_cast("VX"), const_cast("VY"), + const_cast("STEER")}; - std::vector input_names = {(char *)"VX_DES", (char *)"STEER_DES"}; + std::vector input_names = {const_cast("VX_DES"), const_cast("STEER_DES")}; enum IDX { X = 0, From 47a1d0efb07e09cd360c6ebcba58f97c6c62ab18 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Tue, 19 Mar 2024 16:40:07 +0100 Subject: [PATCH 92/97] Fix pybind11 name in package.xml --- simulator/learned_model/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/learned_model/package.xml b/simulator/learned_model/package.xml index fda761f30c9dd..fc58efd6630ed 100644 --- a/simulator/learned_model/package.xml +++ b/simulator/learned_model/package.xml @@ -13,7 +13,7 @@ autoware_cmake - pybind11 + pybind11_vendor python3-dev ament_cmake_ros From 819ef588f925fb57dc21eefe16800e4598a35358 Mon Sep 17 00:00:00 2001 From: Tomas Nagy Date: Mon, 25 Mar 2024 20:37:33 +0100 Subject: [PATCH 93/97] Change name of the package --- .../CMakeLists.txt | 2 +- .../README.md | 0 .../image/python_model_interface.png | Bin .../interconnected_model.hpp | 12 ++++++------ .../model_connections_helpers.hpp | 6 +++--- .../simple_pymodel.hpp | 10 +++++----- .../submodel_interface.hpp | 6 +++--- .../package.xml | 4 ++-- .../src/interconnected_model.cpp | 2 +- .../src/model_connections_helpers.cpp | 2 +- .../src/simple_pymodel.cpp | 2 +- .../src/submodel_interface.cpp | 2 +- simulator/simple_planning_simulator/CMakeLists.txt | 4 ++-- simulator/simple_planning_simulator/README.md | 2 +- .../vehicle_model/sim_model_learned_steer_vel.hpp | 2 +- simulator/simple_planning_simulator/package.xml | 2 +- .../vehicle_model/sim_model_learned_steer_vel.cpp | 2 +- 17 files changed, 30 insertions(+), 30 deletions(-) rename simulator/{learned_model => learning_based_vehicle_model}/CMakeLists.txt (93%) rename simulator/{learned_model => learning_based_vehicle_model}/README.md (100%) rename simulator/{learned_model => learning_based_vehicle_model}/image/python_model_interface.png (100%) rename simulator/{learned_model/include/learned_model => learning_based_vehicle_model/include/learning_based_vehicle_model}/interconnected_model.hpp (91%) rename simulator/{learned_model/include/learned_model => learning_based_vehicle_model/include/learning_based_vehicle_model}/model_connections_helpers.hpp (81%) rename simulator/{learned_model/include/learned_model => learning_based_vehicle_model/include/learning_based_vehicle_model}/simple_pymodel.hpp (90%) rename simulator/{learned_model/include/learned_model => learning_based_vehicle_model/include/learning_based_vehicle_model}/submodel_interface.hpp (90%) rename simulator/{learned_model => learning_based_vehicle_model}/package.xml (86%) rename simulator/{learned_model => learning_based_vehicle_model}/src/interconnected_model.cpp (98%) rename simulator/{learned_model => learning_based_vehicle_model}/src/model_connections_helpers.cpp (96%) rename simulator/{learned_model => learning_based_vehicle_model}/src/simple_pymodel.cpp (98%) rename simulator/{learned_model => learning_based_vehicle_model}/src/submodel_interface.cpp (90%) diff --git a/simulator/learned_model/CMakeLists.txt b/simulator/learning_based_vehicle_model/CMakeLists.txt similarity index 93% rename from simulator/learned_model/CMakeLists.txt rename to simulator/learning_based_vehicle_model/CMakeLists.txt index e6461f17f56f3..58a3ad57e6f9e 100644 --- a/simulator/learned_model/CMakeLists.txt +++ b/simulator/learning_based_vehicle_model/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14.4) -project(learned_model) +project(learning_based_vehicle_model) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/simulator/learned_model/README.md b/simulator/learning_based_vehicle_model/README.md similarity index 100% rename from simulator/learned_model/README.md rename to simulator/learning_based_vehicle_model/README.md diff --git a/simulator/learned_model/image/python_model_interface.png b/simulator/learning_based_vehicle_model/image/python_model_interface.png similarity index 100% rename from simulator/learned_model/image/python_model_interface.png rename to simulator/learning_based_vehicle_model/image/python_model_interface.png diff --git a/simulator/learned_model/include/learned_model/interconnected_model.hpp b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp similarity index 91% rename from simulator/learned_model/include/learned_model/interconnected_model.hpp rename to simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp index 53c14bff97382..13fbb206f6763 100644 --- a/simulator/learned_model/include/learned_model/interconnected_model.hpp +++ b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LEARNED_MODEL__INTERCONNECTED_MODEL_HPP_ -#define LEARNED_MODEL__INTERCONNECTED_MODEL_HPP_ +#ifndef LEARNING_BASED_VEHICLE_MODEL__INTERCONNECTED_MODEL_HPP_ +#define LEARNING_BASED_VEHICLE_MODEL__INTERCONNECTED_MODEL_HPP_ -#include "learned_model/model_connections_helpers.hpp" -#include "learned_model/simple_pymodel.hpp" -#include "learned_model/submodel_interface.hpp" +#include "learning_based_vehicle_model/model_connections_helpers.hpp" +#include "learning_based_vehicle_model/simple_pymodel.hpp" +#include "learning_based_vehicle_model/submodel_interface.hpp" #include #include @@ -126,4 +126,4 @@ class __attribute__((visibility("default"))) InterconnectedModel std::vector updatePymodel(std::vector psim_input); }; -#endif // LEARNED_MODEL__INTERCONNECTED_MODEL_HPP_ +#endif // LEARNING_BASED_VEHICLE_MODEL__INTERCONNECTED_MODEL_HPP_ diff --git a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/model_connections_helpers.hpp similarity index 81% rename from simulator/learned_model/include/learned_model/model_connections_helpers.hpp rename to simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/model_connections_helpers.hpp index 7d40c4955f3dd..3f21595662fc5 100644 --- a/simulator/learned_model/include/learned_model/model_connections_helpers.hpp +++ b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/model_connections_helpers.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ -#define LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ +#ifndef LEARNING_BASED_VEHICLE_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ +#define LEARNING_BASED_VEHICLE_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ #include #include @@ -24,4 +24,4 @@ std::vector fillVectorUsingMap( std::vector createConnectionsMap( std::vector connection_names_1, std::vector connection_names_2); -#endif // LEARNED_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ +#endif // LEARNING_BASED_VEHICLE_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ diff --git a/simulator/learned_model/include/learned_model/simple_pymodel.hpp b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp similarity index 90% rename from simulator/learned_model/include/learned_model/simple_pymodel.hpp rename to simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp index c4b1707b812ca..d1b7468516604 100644 --- a/simulator/learned_model/include/learned_model/simple_pymodel.hpp +++ b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LEARNED_MODEL__SIMPLE_PYMODEL_HPP_ -#define LEARNED_MODEL__SIMPLE_PYMODEL_HPP_ +#ifndef LEARNING_BASED_VEHICLE_MODEL__SIMPLE_PYMODEL_HPP_ +#define LEARNING_BASED_VEHICLE_MODEL__SIMPLE_PYMODEL_HPP_ -#include "learned_model/model_connections_helpers.hpp" -#include "learned_model/submodel_interface.hpp" +#include "learning_based_vehicle_model/model_connections_helpers.hpp" +#include "learning_based_vehicle_model/submodel_interface.hpp" #include #include @@ -96,4 +96,4 @@ class SimplePymodel : public SubModelInterface void mapOutputs(std::vector signals_vec_names) override; }; -#endif // LEARNED_MODEL__SIMPLE_PYMODEL_HPP_ +#endif // LEARNING_BASED_VEHICLE_MODEL__SIMPLE_PYMODEL_HPP_ diff --git a/simulator/learned_model/include/learned_model/submodel_interface.hpp b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/submodel_interface.hpp similarity index 90% rename from simulator/learned_model/include/learned_model/submodel_interface.hpp rename to simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/submodel_interface.hpp index a16db7b8ca344..d4662036709d5 100644 --- a/simulator/learned_model/include/learned_model/submodel_interface.hpp +++ b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/submodel_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LEARNED_MODEL__SUBMODEL_INTERFACE_HPP_ -#define LEARNED_MODEL__SUBMODEL_INTERFACE_HPP_ +#ifndef LEARNING_BASED_VEHICLE_MODEL__SUBMODEL_INTERFACE_HPP_ +#define LEARNING_BASED_VEHICLE_MODEL__SUBMODEL_INTERFACE_HPP_ #include @@ -57,4 +57,4 @@ class SubModelInterface std::vector model_signals_vec, std::vector model_signals_vec_next) = 0; }; -#endif // LEARNED_MODEL__SUBMODEL_INTERFACE_HPP_ +#endif // LEARNING_BASED_VEHICLE_MODEL__SUBMODEL_INTERFACE_HPP_ diff --git a/simulator/learned_model/package.xml b/simulator/learning_based_vehicle_model/package.xml similarity index 86% rename from simulator/learned_model/package.xml rename to simulator/learning_based_vehicle_model/package.xml index fc58efd6630ed..18db5236c272e 100644 --- a/simulator/learned_model/package.xml +++ b/simulator/learning_based_vehicle_model/package.xml @@ -1,9 +1,9 @@ - learned_model + learning_based_vehicle_model 1.0.0 - learned_model for simple_planning_simulator + learning_based_vehicle_model for simple_planning_simulator Maxime Clement Tomas Nagy Apache License 2.0 diff --git a/simulator/learned_model/src/interconnected_model.cpp b/simulator/learning_based_vehicle_model/src/interconnected_model.cpp similarity index 98% rename from simulator/learned_model/src/interconnected_model.cpp rename to simulator/learning_based_vehicle_model/src/interconnected_model.cpp index 5b8491ad4844c..c8805fff11607 100644 --- a/simulator/learned_model/src/interconnected_model.cpp +++ b/simulator/learning_based_vehicle_model/src/interconnected_model.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "learned_model/interconnected_model.hpp" +#include "learning_based_vehicle_model/interconnected_model.hpp" void InterconnectedModel::mapInputs(std::vector in_names) { diff --git a/simulator/learned_model/src/model_connections_helpers.cpp b/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp similarity index 96% rename from simulator/learned_model/src/model_connections_helpers.cpp rename to simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp index 4a4ccabe3ceaa..09f7359af45bf 100644 --- a/simulator/learned_model/src/model_connections_helpers.cpp +++ b/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "learned_model/model_connections_helpers.hpp" +#include "learning_based_vehicle_model/model_connections_helpers.hpp" std::vector fillVectorUsingMap( std::vector vector1, std::vector vector2, std::vector map, bool inverse) diff --git a/simulator/learned_model/src/simple_pymodel.cpp b/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp similarity index 98% rename from simulator/learned_model/src/simple_pymodel.cpp rename to simulator/learning_based_vehicle_model/src/simple_pymodel.cpp index 454671c3c4910..1a2a251d70308 100644 --- a/simulator/learned_model/src/simple_pymodel.cpp +++ b/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "learned_model/simple_pymodel.hpp" +#include "learning_based_vehicle_model/simple_pymodel.hpp" #include #include diff --git a/simulator/learned_model/src/submodel_interface.cpp b/simulator/learning_based_vehicle_model/src/submodel_interface.cpp similarity index 90% rename from simulator/learned_model/src/submodel_interface.cpp rename to simulator/learning_based_vehicle_model/src/submodel_interface.cpp index cd14a2954fbd1..906aff4b01ae0 100644 --- a/simulator/learned_model/src/submodel_interface.cpp +++ b/simulator/learning_based_vehicle_model/src/submodel_interface.cpp @@ -12,4 +12,4 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "learned_model/submodel_interface.hpp" +#include "learning_based_vehicle_model/submodel_interface.hpp" diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 8835d568b213f..331d5e819df77 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -3,7 +3,7 @@ project(simple_planning_simulator) find_package(autoware_cmake REQUIRED) find_package(Python3 COMPONENTS Interpreter Development) -find_package(learned_model REQUIRED) +find_package(learning_based_vehicle_model REQUIRED) autoware_package() # Component @@ -22,7 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp src/simple_planning_simulator/utils/csv_loader.cpp ) -target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS} ${learned_model_INCLUDE_DIRS}) +target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS} ${learning_based_vehicle_model_INCLUDE_DIRS}) # Node executable rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "simulation::simple_planning_simulator::SimplePlanningSimulator" diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index f2023db4ecbb8..1f8762b722a29 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -63,7 +63,7 @@ The purpose of this simulator is for the integration test of planning and contro - `DELAY_STEER_ACC` - `DELAY_STEER_ACC_GEARED` - `DELAY_STEER_MAP_ACC_GEARED`: applies 1D dynamics and time delay to the steering and acceleration commands. The simulated acceleration is determined by a value converted through the provided acceleration map. This model is valuable for an accurate simulation with acceleration deviations in a real vehicle. -- `LEARNED_STEER_VEL`: launches learned python models. More about this [here](../learned_model). +- `LEARNED_STEER_VEL`: launches learned python models. More about this [here](../learning_based_vehicle_model). The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves based on a 1st-order with time delay model. The `STEER` means the model receives the steer command. The `VEL` means the model receives the target velocity command, while the `ACC` model receives the target acceleration command. The `GEARED` suffix means that the motion will consider the gear command: the vehicle moves only one direction following the gear. diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp index a7ff86c38b1cf..c6301db42620e 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp @@ -15,7 +15,7 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ -#include "learned_model/interconnected_model.hpp" +#include "learning_based_vehicle_model/interconnected_model.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 52f8591c0ee48..14509f83ae2fe 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -21,7 +21,7 @@ geometry_msgs lanelet2_core lanelet2_extension - learned_model + learning_based_vehicle_model motion_utils nav_msgs rclcpp diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp index aa812d452ab47..44e037ab19e07 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp @@ -14,7 +14,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp" -#include "learned_model/interconnected_model.hpp" +#include "learning_based_vehicle_model/interconnected_model.hpp" #include From 9ae8046ae9054af580d4c678760e4044e9c28d1c Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 5 Apr 2024 12:11:07 +0900 Subject: [PATCH 94/97] add cspell:ignore libpython Signed-off-by: Maxime CLEMENT --- .../learning_based_vehicle_model/interconnected_model.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp index 13fbb206f6763..ba496e35e543c 100644 --- a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp +++ b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp @@ -57,6 +57,7 @@ class __attribute__((visibility("default"))) InterconnectedModel InterconnectedModel() { // Initialize python library + // cspell:ignore libpython // Manually load libpython3.10.so as we need it for python.h. dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW); /* From b69d9680230a68e11c164d29b377c1c4c034c51b Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 5 Apr 2024 12:11:33 +0900 Subject: [PATCH 95/97] rename pyin/pyout to py_model_inputs/py_model_outputs Signed-off-by: Maxime CLEMENT --- .../simple_pymodel.hpp | 14 +++++++------ .../src/simple_pymodel.cpp | 21 ++++++++++--------- 2 files changed, 19 insertions(+), 16 deletions(-) diff --git a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp index d1b7468516604..f7d6b6d14e1f8 100644 --- a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp +++ b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp @@ -27,7 +27,7 @@ namespace py = pybind11; /** - * @class SimModelPymodels + * @class SimplePymodel * @brief This class is an interface between C++ and python models. */ class SimplePymodel : public SubModelInterface @@ -41,11 +41,13 @@ class SimplePymodel : public SubModelInterface py::object py_model_class; std::vector - map_sig_vec_to_pyin; // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in - // "map_sig_vec_to_pyin" is index in "signals_vec_names" - std::vector - map_pyout_to_sig_vec; // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and - // value in "map_pyout_to_sig_vec" is index in "signals_vec_names" + map_sig_vec_to_py_model_inputs; // index in "map_sig_vec_to_py_model_inputs" is index in + // "py_inputs" and value in "map_sig_vec_to_py_model_inputs" is + // index in "signals_vec_names" + std::vector map_py_model_outputs_to_sig_vec; // index in "map_py_model_outputs_to_sig_vec" + // is index in "pymodel_outputs" and value in + // "map_py_model_outputs_to_sig_vec" is index + // in "signals_vec_names" std::vector pymodel_input_names; std::vector pymodel_state_names; diff --git a/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp b/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp index 1a2a251d70308..b0200d523ec9d 100644 --- a/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp +++ b/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp @@ -66,16 +66,17 @@ std::vector SimplePymodel::getNextState( // get inputs and states of the python model from the vector of signals std::vector py_inputs(num_inputs_py); std::vector py_state(num_outputs_py); - py_inputs = fillVectorUsingMap(py_inputs, model_signals_vec, map_sig_vec_to_pyin, true); - py_state = fillVectorUsingMap(py_state, model_signals_vec, map_pyout_to_sig_vec, true); + py_inputs = + fillVectorUsingMap(py_inputs, model_signals_vec, map_sig_vec_to_py_model_inputs, true); + py_state = fillVectorUsingMap(py_state, model_signals_vec, map_py_model_outputs_to_sig_vec, true); // forward pass through the base model py::tuple res = py_model_class.attr("forward")(py_inputs, py_state); std::vector py_state_next = res.cast>(); // map outputs from python model to required outputs - std::vector next_state = - fillVectorUsingMap(py_state_next, model_signals_vec_next, map_pyout_to_sig_vec, false); + std::vector next_state = fillVectorUsingMap( + py_state_next, model_signals_vec_next, map_py_model_outputs_to_sig_vec, false); return next_state; } @@ -97,14 +98,14 @@ std::vector SimplePymodel::getStateNames() void SimplePymodel::mapInputs(std::vector signals_vec_names) { - // index in "map_sig_vec_to_pyin" is index in "py_inputs" and value in "map_sig_vec_to_pyin" is - // index in "signals_vec_names" - map_sig_vec_to_pyin = createConnectionsMap(signals_vec_names, pymodel_input_names); + // index in "map_sig_vec_to_py_model_inputs" is index in "py_inputs" and value in + // "map_sig_vec_to_py_model_inputs" is index in "signals_vec_names" + map_sig_vec_to_py_model_inputs = createConnectionsMap(signals_vec_names, pymodel_input_names); } void SimplePymodel::mapOutputs(std::vector signals_vec_names) { - // index in "map_pyout_to_sig_vec" is index in "pymodel_outputs" and value in - // "map_pyout_to_sig_vec" is index in "signals_vec_names" - map_pyout_to_sig_vec = createConnectionsMap(signals_vec_names, pymodel_state_names); + // index in "map_py_model_outputs_to_sig_vec" is index in "pymodel_outputs" and value in + // "map_py_model_outputs_to_sig_vec" is index in "signals_vec_names" + map_py_model_outputs_to_sig_vec = createConnectionsMap(signals_vec_names, pymodel_state_names); } From 3b72bc80198854e2ec9a517da7feef7779e447d1 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 5 Apr 2024 12:12:35 +0900 Subject: [PATCH 96/97] rename Pymodel -> PyModel and pymodel -> py_model Signed-off-by: Maxime CLEMENT --- .../learning_based_vehicle_model/README.md | 6 +-- .../interconnected_model.hpp | 4 +- .../simple_pymodel.hpp | 18 +++---- .../src/interconnected_model.cpp | 6 +-- .../src/simple_pymodel.cpp | 52 +++++++++---------- .../sim_model_learned_steer_vel.cpp | 2 +- 6 files changed, 44 insertions(+), 44 deletions(-) diff --git a/simulator/learning_based_vehicle_model/README.md b/simulator/learning_based_vehicle_model/README.md index 35a6e673ff589..b6df96b2d576c 100644 --- a/simulator/learning_based_vehicle_model/README.md +++ b/simulator/learning_based_vehicle_model/README.md @@ -18,7 +18,7 @@ This library creates an interface between models in Python and PSIM (C++). It is The idea behind this package is that the model we want to use for simulation consists of multiple sub-models (e.g., steering model, drive model, vehicle kinematics, etc.). These sub-models are implemented in Python and can be trainable. Each sub-model has string names for all of its inputs/outputs, which are used to create model interconnections automatically (see image below). This allows us to easily switch sub-models for better customization of the simulator. -![pymodel_interface](./image/python_model_interface.png "PyModel interface") +![py_model_interface](./image/python_model_interface.png "PyModel interface") ## Assumptions / Known limits @@ -124,7 +124,7 @@ Outputs: - None -#### `std::vector updatePymodel(std::vector psim_input)` +#### `std::vector updatePyModel(std::vector psim_input)` Calculate the next state of the model by calculating the next state of all of the sub-models. @@ -196,7 +196,7 @@ std::vector current_state = {0.2, 0.5}; // STATE_NAME_1, STATE_NAME_2 vehicle.initState(current_state); // Calculate the next state of the model -std::vector next_state = vehicle.updatePymodel(vehicle_input); +std::vector next_state = vehicle.updatePyModel(vehicle_input); ``` ## References / External links diff --git a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp index ba496e35e543c..56a196c257189 100644 --- a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp +++ b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp @@ -44,7 +44,7 @@ class __attribute__((visibility("default"))) InterconnectedModel // in "all_variables_names" std::vector map_in_to_sig_vec; - // index in "map_sig_vec_to_out" is index in "pymodel_outputs" and value in "map_sig_vec_to_out" + // index in "map_sig_vec_to_out" is index in "py_model_outputs" and value in "map_sig_vec_to_out" // is index in "all_variables_names" std::vector map_sig_vec_to_out; @@ -124,7 +124,7 @@ class __attribute__((visibility("default"))) InterconnectedModel * @brief compute next step of the PSIM model using python sub-models * @param [in] psim_input vector of input values provided by PSIM */ - std::vector updatePymodel(std::vector psim_input); + std::vector updatePyModel(std::vector psim_input); }; #endif // LEARNING_BASED_VEHICLE_MODEL__INTERCONNECTED_MODEL_HPP_ diff --git a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp index f7d6b6d14e1f8..99f927c6f5a9b 100644 --- a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp +++ b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp @@ -27,13 +27,13 @@ namespace py = pybind11; /** - * @class SimplePymodel + * @class SimplePyModel * @brief This class is an interface between C++ and python models. */ -class SimplePymodel : public SubModelInterface +class SimplePyModel : public SubModelInterface { private: - std::string pymodel_import_name; + std::string py_model_import_name; int num_inputs_py; int num_outputs_py; @@ -45,22 +45,22 @@ class SimplePymodel : public SubModelInterface // "py_inputs" and value in "map_sig_vec_to_py_model_inputs" is // index in "signals_vec_names" std::vector map_py_model_outputs_to_sig_vec; // index in "map_py_model_outputs_to_sig_vec" - // is index in "pymodel_outputs" and value in + // is index in "py_model_outputs" and value in // "map_py_model_outputs_to_sig_vec" is index // in "signals_vec_names" - std::vector pymodel_input_names; - std::vector pymodel_state_names; + std::vector py_model_input_names; + std::vector py_model_state_names; public: /** * @brief constructor - * @param [in] pymodel_import_name_ path to python model + * @param [in] py_model_import_name_ path to python model * @param [in] param_file_path path to saved parameter file of the python sub-model * @param [in] py_class_name name of the python class */ - SimplePymodel( - std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name); + SimplePyModel( + std::string py_model_import_name_, std::string param_file_path, std::string py_class_name); /** * @brief calculate the next state of a python model diff --git a/simulator/learning_based_vehicle_model/src/interconnected_model.cpp b/simulator/learning_based_vehicle_model/src/interconnected_model.cpp index c8805fff11607..4dd3b321312d0 100644 --- a/simulator/learning_based_vehicle_model/src/interconnected_model.cpp +++ b/simulator/learning_based_vehicle_model/src/interconnected_model.cpp @@ -75,8 +75,8 @@ void InterconnectedModel::addSubmodel( std::tuple submodel_desc) { const auto [lib_path, param_path, class_name] = submodel_desc; - auto new_model = new SimplePymodel(lib_path, param_path, class_name); - submodels.push_back(std::unique_ptr(new_model)); + auto new_model = new SimplePyModel(lib_path, param_path, class_name); + submodels.push_back(std::unique_ptr(new_model)); } void InterconnectedModel::initState(std::vector new_state) @@ -112,7 +112,7 @@ void InterconnectedModel::dtSet(double dt) } } -std::vector InterconnectedModel::updatePymodel(std::vector psim_input) +std::vector InterconnectedModel::updatePyModel(std::vector psim_input) { // map input to vector of all variables for (size_t PSIM_INPUT_IDX = 0; PSIM_INPUT_IDX < psim_input.size(); PSIM_INPUT_IDX++) { diff --git a/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp b/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp index b0200d523ec9d..9c3cb9aa06150 100644 --- a/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp +++ b/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp @@ -21,14 +21,14 @@ namespace py = pybind11; -SimplePymodel::SimplePymodel( - std::string pymodel_import_name_, std::string param_file_path, std::string py_class_name) +SimplePyModel::SimplePyModel( + std::string py_model_import_name_, std::string param_file_path, std::string py_class_name) { // Import model class - pymodel_import_name = pymodel_import_name_; - if (!pymodel_import_name.empty()) { + py_model_import_name = py_model_import_name_; + if (!py_model_import_name.empty()) { // Import python module - py::module_ imported_module = py::module_::import(pymodel_import_name.c_str()); + py::module_ imported_module = py::module_::import(py_model_import_name.c_str()); // Initialize model class from imported module py_model_class = imported_module.attr(py_class_name.c_str())(); } else { @@ -42,25 +42,25 @@ SimplePymodel::SimplePymodel( } // Get string names of states of python model, convert them to C++ string and store them in - // pymodel_state_names - py::list pymodel_state_names_ = py_model_class.attr("get_state_names")(); - num_outputs_py = pymodel_state_names_.size(); + // py_model_state_names + py::list py_model_state_names_ = py_model_class.attr("get_state_names")(); + num_outputs_py = py_model_state_names_.size(); for (int STATE_IDX = 0; STATE_IDX < num_outputs_py; STATE_IDX++) { - pymodel_state_names.push_back(PyBytes_AS_STRING( - PyUnicode_AsEncodedString(pymodel_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); + py_model_state_names.push_back(PyBytes_AS_STRING( + PyUnicode_AsEncodedString(py_model_state_names_[STATE_IDX].ptr(), "UTF-8", "strict"))); } // Get string names of actions (inputs) of python model, convert them to C++ string and store - // them in pymodel_input_names - py::list pymodel_input_names_ = py_model_class.attr("get_action_names")(); - num_inputs_py = pymodel_input_names_.size(); + // them in py_model_input_names + py::list py_model_input_names_ = py_model_class.attr("get_action_names")(); + num_inputs_py = py_model_input_names_.size(); for (int INPUT_IDX = 0; INPUT_IDX < num_inputs_py; INPUT_IDX++) { - pymodel_input_names.push_back(PyBytes_AS_STRING( - PyUnicode_AsEncodedString(pymodel_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); + py_model_input_names.push_back(PyBytes_AS_STRING( + PyUnicode_AsEncodedString(py_model_input_names_[INPUT_IDX].ptr(), "UTF-8", "strict"))); } } -std::vector SimplePymodel::getNextState( +std::vector SimplePyModel::getNextState( std::vector model_signals_vec, std::vector model_signals_vec_next) { // get inputs and states of the python model from the vector of signals @@ -81,31 +81,31 @@ std::vector SimplePymodel::getNextState( return next_state; } -void SimplePymodel::dtSet(double dt) +void SimplePyModel::dtSet(double dt) { py_model_class.attr("dtSet")(dt); } -std::vector SimplePymodel::getInputNames() +std::vector SimplePyModel::getInputNames() { - return pymodel_input_names; + return py_model_input_names; } -std::vector SimplePymodel::getStateNames() +std::vector SimplePyModel::getStateNames() { - return pymodel_state_names; + return py_model_state_names; } -void SimplePymodel::mapInputs(std::vector signals_vec_names) +void SimplePyModel::mapInputs(std::vector signals_vec_names) { // index in "map_sig_vec_to_py_model_inputs" is index in "py_inputs" and value in // "map_sig_vec_to_py_model_inputs" is index in "signals_vec_names" - map_sig_vec_to_py_model_inputs = createConnectionsMap(signals_vec_names, pymodel_input_names); + map_sig_vec_to_py_model_inputs = createConnectionsMap(signals_vec_names, py_model_input_names); } -void SimplePymodel::mapOutputs(std::vector signals_vec_names) +void SimplePyModel::mapOutputs(std::vector signals_vec_names) { - // index in "map_py_model_outputs_to_sig_vec" is index in "pymodel_outputs" and value in + // index in "map_py_model_outputs_to_sig_vec" is index in "py_model_outputs" and value in // "map_py_model_outputs_to_sig_vec" is index in "signals_vec_names" - map_py_model_outputs_to_sig_vec = createConnectionsMap(signals_vec_names, pymodel_state_names); + map_py_model_outputs_to_sig_vec = createConnectionsMap(signals_vec_names, py_model_state_names); } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp index 44e037ab19e07..fba34e04ade7c 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp @@ -77,7 +77,7 @@ void SimModelLearnedSteerVel::update(const double & dt) vehicle.initState(new_state); // model forward - std::vector vehicle_state_ = vehicle.updatePymodel(vehicle_input_); + std::vector vehicle_state_ = vehicle.updatePyModel(vehicle_input_); // std::vector to Eigen::VectorXd for (size_t i = 0; i < vehicle_state_.size(); i++) state_[i] = vehicle_state_[i]; From 48379dc240a54b4d4d1a396dbaac354afc32b6d4 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 5 Apr 2024 13:28:25 +0900 Subject: [PATCH 97/97] add cpell:ignore pymodel Signed-off-by: Maxime CLEMENT --- .../include/learning_based_vehicle_model/simple_pymodel.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp index 99f927c6f5a9b..fd9cf5cf44de9 100644 --- a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp +++ b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +// cspell:ignore pymodel #ifndef LEARNING_BASED_VEHICLE_MODEL__SIMPLE_PYMODEL_HPP_ #define LEARNING_BASED_VEHICLE_MODEL__SIMPLE_PYMODEL_HPP_